diff options
author | Julian Oes <julian@oes.ch> | 2014-06-19 09:33:47 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-06-19 09:33:47 +0200 |
commit | 32e2df3d08ee3178de193336c30ae712bcb39dc6 (patch) | |
tree | c9858cc102ab13aeb838cc402ef7ed9afa5010c9 /src | |
parent | eb1af6096e51531189f1cc6a1da748ab7101ba06 (diff) | |
parent | 94e004955df3f467b7e67b3fac0d968b9a68e091 (diff) | |
download | px4-firmware-32e2df3d08ee3178de193336c30ae712bcb39dc6.tar.gz px4-firmware-32e2df3d08ee3178de193336c30ae712bcb39dc6.tar.bz2 px4-firmware-32e2df3d08ee3178de193336c30ae712bcb39dc6.zip |
Merge branch 'navigator_rewrite' into navigator_rewrite_estimator
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 22 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 1 |
2 files changed, 23 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 32c5e51dd..7a6922bfa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -107,6 +107,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rc_pub(-1), _manual_pub(-1), _telemetry_heartbeat_time(0), + _radio_status_available(false), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -432,6 +433,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } + + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; } void @@ -467,6 +471,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { _telemetry_heartbeat_time = hrt_absolute_time(); } + + /* 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.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + } } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ab3dc81c6..cd1dab365 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -140,6 +140,7 @@ private: orb_advert_t _rc_pub; orb_advert_t _manual_pub; hrt_abstime _telemetry_heartbeat_time; + bool _radio_status_available; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; |