diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-06-16 17:32:58 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-06-16 17:32:58 +0200 |
commit | 91f0b9eee41a8446c0a5ec455fbe3853c5c3eee3 (patch) | |
tree | 7808b63637064dae5c51db51921982cb59df0947 /src/modules/mavlink | |
parent | 9772aa5814eb2f0d864e89814f5be2cf9f136eda (diff) | |
download | px4-firmware-91f0b9eee41a8446c0a5ec455fbe3853c5c3eee3.tar.gz px4-firmware-91f0b9eee41a8446c0a5ec455fbe3853c5c3eee3.tar.bz2 px4-firmware-91f0b9eee41a8446c0a5ec455fbe3853c5c3eee3.zip |
mavlink: store last heartbeat time in telemetry_status topic
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 18 |
1 files changed, 18 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33358b7b6..32c5e51dd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,6 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _telemetry_heartbeat_time(0), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -150,6 +151,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(msg); + break; + default: break; } @@ -411,6 +416,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) memset(&tstatus, 0, sizeof(tstatus)); tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -452,6 +458,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) +{ + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); + + /* 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(); + } +} + +void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { mavlink_hil_sensor_t imu; |