diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-22 16:08:48 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-22 16:08:48 +0200 |
commit | ea36154e3975b12bf72da132e71abdbfb6f5b2bb (patch) | |
tree | 92329af90a400c8bd74be9cfaf58a109af1b73ee /apps/mavlink/orb_listener.c | |
parent | 64c5096c9f56b4ec1c995a0129ce5088ea8be719 (diff) | |
download | px4-firmware-ea36154e3975b12bf72da132e71abdbfb6f5b2bb.tar.gz px4-firmware-ea36154e3975b12bf72da132e71abdbfb6f5b2bb.tar.bz2 px4-firmware-ea36154e3975b12bf72da132e71abdbfb6f5b2bb.zip |
Accomodating for offboard control setups
Diffstat (limited to 'apps/mavlink/orb_listener.c')
-rw-r--r-- | apps/mavlink/orb_listener.c | 85 |
1 files changed, 49 insertions, 36 deletions
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index c2428874f..0b073cc65 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -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) { |