fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 01:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: Keep sending GPS_RAW_INT/GPS2_RAW.hpp streams on GPS failure

Browse Source 
* 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 <marcell@auterion.com>
This commit is contained in:
Marcell Rausch
2022-12-14 15:12:39 +01:00
committed by GitHub
parent 96362bfb52
commit 6c7702b906
2 changed files with 38 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/streams/GPS2_RAW.hpp
+19 -1
View File
@@ -36,6 +36,8 @@
#include <uORB/topics/sensor_gps.h>
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;
}
src/modules/mavlink/streams/GPS_RAW_INT.hpp
+19 -2
View File
@@ -36,6 +36,8 @@
#include <uORB/topics/sensor_gps.h>
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;
}