diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-19 15:56:37 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-23 08:18:55 +0200 |
| commit | 91becef344833dbb19230ec3f3c69750bce216e1 (patch) | |
| tree | 5abfeafe7a37e062ca69e5111d1cf0386214a7f0 /src/modules/mavlink/mavlink_receiver.cpp | |
| parent | 0c0b1a4c666671e964d295fe6aea64d7b6a98fc9 (diff) | |
| download | px4-firmware-91becef344833dbb19230ec3f3c69750bce216e1.tar.gz px4-firmware-91becef344833dbb19230ec3f3c69750bce216e1.tar.bz2 px4-firmware-91becef344833dbb19230ec3f3c69750bce216e1.zip | |
Cleanup of heartbeat handling and status printing. Ready to go mainline
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 20 |
1 files changed, 12 insertions, 8 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 528c8b72a..454d730d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _telemetry_heartbeat_time(0), _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), @@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) /* ignore own heartbeats, accept only heartbeats from GCS */ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + + 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) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; + 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) { |