fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Accomodating for offboard control setups

Browse Source
This commit is contained in:
Lorenz Meier 2012-10-22 16:08:48 +02:00
parent 64c5096c9f
commit ea36154e39
4 changed files with 61 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
apps/mavlink/mavlink.c
View File

@ -119,6 +119,7 @@ bool mavlink_hil_enabled = false;
/* protocol interface */
static int uart;
static int baudrate;
bool gcs_link = true;
/* interface mode */
static enum {
@ -600,14 +601,14 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
/* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else {

6
apps/mavlink/mavlink_receiver.c
View File

@ -93,6 +93,8 @@ static orb_advert_t flow_pub = -1;
static orb_advert_t offboard_control_sp_pub = -1;
static orb_advert_t vicon_position_pub = -1;
extern bool gcs_link;
static void
handle_message(mavlink_message_t *msg)
{
@ -218,6 +220,10 @@ handle_message(mavlink_message_t *msg)
//printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode);
if (mavlink_system.sysid < 4) {
/* switch to a receiving link mode */
gcs_link = false;
/*
* rate control mode - defined by MAVLink
*/

85
apps/mavlink/orb_listener.c
View File

@ -61,6 +61,8 @@
#include "mavlink_hil.h"
#include "util.h"
extern bool gcs_link;
struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct vehicle_status_s v_status;
@ -172,15 +174,16 @@ l_sensor_combined(struct listener *l)
baro_counter = raw.baro_counter;
}
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
raw.gyro_rad_s[1], raw.gyro_rad_s[2],
raw.magnetometer_ga[0],
raw.magnetometer_ga[1],raw.magnetometer_ga[2],
raw.baro_pres_mbar, 0 /* no diff pressure yet */,
raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated);
if (gcs_link)
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
raw.gyro_rad_s[1], raw.gyro_rad_s[2],
raw.magnetometer_ga[0],
raw.magnetometer_ga[1],raw.magnetometer_ga[2],
raw.baro_pres_mbar, 0 /* no diff pressure yet */,
raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated);
sensors_raw_counter++;
}
@ -194,8 +197,9 @@ l_vehicle_attitude(struct listener *l)
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
/* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0,
if (gcs_link)
/* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
att.roll,
att.pitch,
@ -271,8 +275,9 @@ l_rc_channels(struct listener *l)
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan,
if (gcs_link)
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan,
rc.timestamp / 1000,
0,
rc.chan[0].raw,
@ -322,7 +327,8 @@ l_local_position(struct listener *l)
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
if (gcs_link)
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
local_pos.timestamp / 1000,
local_pos.x,
local_pos.y,
@ -344,7 +350,8 @@ l_global_position_setpoint(struct listener *l)
if (global_sp.altitude_is_relative)
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
coordinate_frame,
global_sp.lat,
global_sp.lon,
@ -360,7 +367,8 @@ l_local_position_setpoint(struct listener *l)
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
if (gcs_link)
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
MAV_FRAME_LOCAL_NED,
local_sp.x,
local_sp.y,
@ -376,7 +384,8 @@ l_attitude_setpoint(struct listener *l)
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
if (gcs_link)
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
att_sp.timestamp/1000,
att_sp.roll_body,
att_sp.pitch_body,
@ -399,7 +408,8 @@ l_actuator_outputs(struct listener *l)
/* copy actuator data into local buffer */
orb_copy(ids[l->arg], *l->subp, &act_outputs);
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
if (gcs_link)
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */,
act_outputs.output[0],
act_outputs.output[1],
@ -425,7 +435,8 @@ l_manual_control_setpoint(struct listener *l)
/* copy manual control data into local buffer */
orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
mavlink_msg_manual_control_send(MAVLINK_COMM_0,
if (gcs_link)
mavlink_msg_manual_control_send(MAVLINK_COMM_0,
mavlink_system.sysid,
man_control.roll * 1000,
man_control.pitch * 1000,
@ -441,23 +452,25 @@ l_vehicle_attitude_controls(struct listener *l)
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators);
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl0 ",
actuators.control[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl1 ",
actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl2 ",
actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl3 ",
actuators.control[3]);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl0 ",
actuators.control[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl1 ",
actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl2 ",
actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl3 ",
actuators.control[3]);
}
/* Only send in HIL mode */
if (mavlink_hil_enabled) {

4
apps/multirotor_att_control/multirotor_rate_control.c
View File

@ -181,8 +181,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
// (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
}
/* calculate current control outputs */