diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 482 |
1 files changed, 442 insertions, 40 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d03b3a198..032b23ca8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ /* XXX trim includes */ @@ -54,6 +55,7 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> +#include <drivers/drv_range_finder.h> #include <time.h> #include <float.h> #include <unistd.h> @@ -102,17 +104,19 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), + _range_pub(-1), _offboard_control_sp_pub(-1), - _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), + _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), _vicon_position_pub(-1), + _vision_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), _target_pos_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -122,7 +126,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : { // make sure the FTP server is started - (void)MavlinkFTP::getServer(); + (void)MavlinkFTP::get_server(); } MavlinkReceiver::~MavlinkReceiver() @@ -137,6 +141,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_long(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + case MAVLINK_MSG_ID_OPTICAL_FLOW: handle_message_optical_flow(msg); break; @@ -149,8 +157,16 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_message_set_position_target_local_ned(msg); + break; + + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_message_set_attitude_target(msg); + break; + + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); break; case MAVLINK_MSG_ID_RADIO_STATUS: @@ -173,8 +189,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - MavlinkFTP::getServer()->handle_message(_mavlink, msg); + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: + MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; default: @@ -201,6 +217,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_state_quaternion(msg); break; + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + default: break; } @@ -281,6 +301,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + +void MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) { /* optical flow */ @@ -309,6 +385,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } + + /* Use distance value for range finder report */ + struct range_finder_report r; + memset(&r, 0, sizeof(f)); + + r.timestamp = hrt_absolute_time(); + r.error_count = 0; + r.type = RANGE_FINDER_TYPE_LASER; + r.distance = flow.ground_distance; + r.minimum_distance = 0.0f; + r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable + r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + + if (_range_pub < 0) { + _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + } else { + orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + } +} + +void MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) { mavlink_set_mode_t new_mode; @@ -368,25 +490,263 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) +MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg) +{ + mavlink_set_position_target_local_ned_t set_position_target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == set_position_target_local_ned.target_system || + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0)) { + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + switch (set_position_target_local_ned.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; + } + offboard_control_sp.position[0] = set_position_target_local_ned.x; + offboard_control_sp.position[1] = set_position_target_local_ned.y; + offboard_control_sp.position[2] = set_position_target_local_ned.z; + offboard_control_sp.velocity[0] = set_position_target_local_ned.vx; + offboard_control_sp.velocity[1] = set_position_target_local_ned.vy; + offboard_control_sp.velocity[2] = set_position_target_local_ned.vz; + offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; + offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; + offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; + offboard_control_sp.yaw = set_position_target_local_ned.yaw; + offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; + offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + + /* If we are in force control mode, for now set offboard mode to force control */ + if (offboard_control_sp.isForceSetpoint) { + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; + } + + /* set ignore flags */ + for (int i = 0; i < 9; i++) { + offboard_control_sp.ignore &= ~(1 << i); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); + } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << + OFB_IGN_BIT_YAW; + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << + OFB_IGN_BIT_YAWRATE; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + if (_control_mode.flag_control_offboard_enabled) { + if (offboard_control_sp.isForceSetpoint && + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ + struct vehicle_force_setpoint_s force_sp; + force_sp.x = offboard_control_sp.acceleration[0]; + force_sp.y = offboard_control_sp.acceleration[1]; + force_sp.z = offboard_control_sp.acceleration[2]; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + struct position_setpoint_triplet_s pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + + /* set the local pos values if the setpoint type is 'local pos' and none + * of the local pos fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local vel values if the setpoint type is 'local pos' and none + * of the local vel fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } else { + pos_sp_triplet.current.velocity_valid = false; + } + + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; + pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; + pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.acceleration_is_force = + offboard_control_sp.isForceSetpoint; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw sp value if the setpoint type is 'local pos' and the yaw + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + //XXX handle global pos setpoints (different MAV frames) + + + if (_pos_sp_triplet_pub < 0) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } + + } + + } + + } + } +} + +void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(msg, &pos); + + struct vision_position_estimate vision_position; + memset(&vision_position, 0, sizeof(vision_position)); + + // Use the component ID to identify the vision sensor + vision_position.id = msg->compid; + + vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_computer = pos.usec; + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; - /* Only accept system IDs from 1 to 4 */ - if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + // XXX fix this + vision_position.vx = 0.0f; + vision_position.vy = 0.0f; + vision_position.vz = 0.0f; - /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; + math::Quaternion q; + q.from_euler(pos.roll, pos.pitch, pos.yaw); + + vision_position.q[0] = q(0); + vision_position.q[1] = q(1); + vision_position.q[2] = q(2); + vision_position.q[3] = q(3); + + if (_vision_position_pub < 0) { + _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); + + } else { + orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); + } +} + +void +MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) +{ + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == set_attitude_target.target_system || + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0)) { + for (int i = 0; i < 4; i++) { + offboard_control_sp.attitude[i] = set_attitude_target.q[i]; + } + offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; + offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; + + /* set correct ignore flags for body rate fields: copy from mavlink message */ + for (int i = 0; i < 3; i++) { + offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; + } + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); - offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); @@ -394,6 +754,57 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ + if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + mavlink_quaternion_to_euler(set_attitude_target.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + att_sp.R_valid = true; + att_sp.thrust = set_attitude_target.thrust; + memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); + att_sp.q_d_valid = true; + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + } + + /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ + ///XXX add support for ignoring individual axes + if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { + struct vehicle_rates_setpoint_s rates_sp; + rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = set_attitude_target.body_roll_rate; + rates_sp.pitch = set_attitude_target.body_pitch_rate; + rates_sp.yaw = set_attitude_target.body_yaw_rate; + rates_sp.thrust = set_attitude_target.thrust; + + if (_att_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + } + } + + } } } @@ -418,6 +829,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.remote_noise = rstatus.remnoise; tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; + tstatus.system_id = msg->sysid; + tstatus.component_id = msg->compid; if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); @@ -425,9 +838,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } } @@ -497,25 +907,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - hrt_abstime tnow = hrt_absolute_time(); - - /* always set heartbeat, publish only if telemetry link not up */ - tstatus.heartbeat_time = tnow; - - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; - tstatus.timestamp = tnow; - /* telem_time indicates the timestamp of a telemetry status packet and we got none */ - tstatus.telem_time = 0; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } } } |