diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-17 10:58:28 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-17 10:58:28 +0200 |
commit | c3522f85928c16d55e54dedc73eb4ed1420d527f (patch) | |
tree | de5ccb98efb436285fd8c5a77e70bd1aa8bcfa0b /src/modules/mavlink/mavlink_receiver.cpp | |
parent | 5a5e1a976ed74efdd3e660c8739d051fd1f847e4 (diff) | |
download | px4-firmware-c3522f85928c16d55e54dedc73eb4ed1420d527f.tar.gz px4-firmware-c3522f85928c16d55e54dedc73eb4ed1420d527f.tar.bz2 px4-firmware-c3522f85928c16d55e54dedc73eb4ed1420d527f.zip |
Publish telemetry status on telemetry update and on heartbeat update events to avoid inducing heartbeat update latencies resulting in spurious telemetry link dropped detections. Makes overall state handling simpler
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 30 |
1 files changed, 9 insertions, 21 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0fae0a2f..e9a9ff133 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -430,9 +429,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; } } @@ -474,25 +470,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); } } } |