From 6c7702b906d7e9dde98e25a4d0cc7af6accc077e Mon Sep 17 00:00:00 2001 From: Marcell Rausch Date: Wed, 14 Dec 2022 15:12:39 +0100 Subject: [PATCH] mavlink: Keep sending GPS_RAW_INT/GPS2_RAW.hpp streams on GPS failure * GPS_RAW_INT: Only send no gps messages if gps has ever been present * GPS2_RAW: Keep in sync with GPS_RAW_INT Signed-off-by: Marcell Rausch --- src/modules/mavlink/streams/GPS2_RAW.hpp | 20 +++++++++++++++++++- src/modules/mavlink/streams/GPS_RAW_INT.hpp | 21 +++++++++++++++++++-- 2 files changed, 38 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index 3c2e06067d..eb77c8c319 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -36,6 +36,8 @@ #include +using namespace time_literals; + class MavlinkStreamGPS2Raw : public MavlinkStream { public: @@ -56,13 +58,16 @@ private: explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 1}; + hrt_abstime _last_send_ts {}; + static constexpr hrt_abstime kNoGpsSendInterval {1_s}; bool send() override { sensor_gps_s gps; + mavlink_gps2_raw_t msg{}; + hrt_abstime now{}; if (_sensor_gps_sub.update(&gps)) { - mavlink_gps2_raw_t msg{}; msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; @@ -99,6 +104,19 @@ private: } mavlink_msg_gps2_raw_send_struct(_mavlink->get_channel(), &msg); + _last_send_ts = gps.timestamp; + + return true; + + } else if (_last_send_ts != 0 && (now = hrt_absolute_time()) > _last_send_ts + kNoGpsSendInterval) { + msg.fix_type = GPS_FIX_TYPE_NO_GPS; + msg.eph = UINT16_MAX; + msg.epv = UINT16_MAX; + msg.vel = UINT16_MAX; + msg.cog = UINT16_MAX; + msg.satellites_visible = UINT8_MAX; + mavlink_msg_gps2_raw_send_struct(_mavlink->get_channel(), &msg); + _last_send_ts = now; return true; } diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index 8c3aeb12c9..f6887c73c9 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -36,6 +36,8 @@ #include +using namespace time_literals; + class MavlinkStreamGPSRawInt : public MavlinkStream { public: @@ -56,14 +58,16 @@ private: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 0}; + hrt_abstime _last_send_ts {}; + static constexpr hrt_abstime kNoGpsSendInterval {1_s}; bool send() override { sensor_gps_s gps; + mavlink_gps_raw_int_t msg{}; + hrt_abstime now{}; if (_sensor_gps_sub.update(&gps)) { - mavlink_gps_raw_int_t msg{}; - msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; msg.lat = gps.lat; @@ -100,6 +104,19 @@ private: } mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg); + _last_send_ts = gps.timestamp; + + return true; + + } else if (_last_send_ts != 0 && (now = hrt_absolute_time()) > _last_send_ts + kNoGpsSendInterval) { + msg.fix_type = GPS_FIX_TYPE_NO_GPS; + msg.eph = UINT16_MAX; + msg.epv = UINT16_MAX; + msg.vel = UINT16_MAX; + msg.cog = UINT16_MAX; + msg.satellites_visible = UINT8_MAX; + mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg); + _last_send_ts = now; return true; }