fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Septentrio GNSS resilience reporting (#25012)

Browse Source 
Co-authored-by: Tory9 <vvpost05@gmail.com>
This commit is contained in:
Louis-max-H 2025-09-24 17:08:10 +02:00 committed by GitHub
parent 8fe2a2218e
commit e71faf38a0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
15 changed files with 360 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -195,6 +195,7 @@ set(msg_files
SensorCombined.msg
SensorCorrection.msg
SensorGnssRelative.msg
SensorGnssStatus.msg
SensorGps.msg
SensorGyro.msg
SensorGyroFft.msg

10
msg/SensorGnssStatus.msg Normal file
View File

@ -0,0 +1,10 @@
# Gnss quality indicators
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
bool quality_available # Set to true if quality indicators are available
uint8 quality_corrections # Corrections quality from 0 to 10, or 255 if not available
uint8 quality_receiver # Overall receiver operating status from 0 to 10, or 255 if not available
uint8 quality_gnss_signals # Quality of GNSS signals from 0 to 10, or 255 if not available
uint8 quality_post_processing # Expected post processing quality from 0 to 10, or 255 if not available

40
msg/SensorGps.msg
View File

@ -30,18 +30,26 @@ float32 vdop # Vertical dilution of precision
int32 noise_per_ms # GPS noise per millisecond
uint16 automatic_gain_control # Automatic gain control monitor
uint8 JAMMING_STATE_UNKNOWN = 0
uint8 JAMMING_STATE_OK = 1
uint8 JAMMING_STATE_WARNING = 2
uint8 JAMMING_STATE_CRITICAL = 3
uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
int32 jamming_indicator # indicates jamming is occurring
uint8 JAMMING_STATE_UNKNOWN = 0 #default
uint8 JAMMING_STATE_OK = 1
uint8 JAMMING_STATE_MITIGATED = 2
uint8 JAMMING_STATE_DETECTED = 3
uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Mitigated, 3: Detected
int32 jamming_indicator # indicates jamming is occurring
uint8 SPOOFING_STATE_UNKNOWN = 0
uint8 SPOOFING_STATE_NONE = 1
uint8 SPOOFING_STATE_INDICATED = 2
uint8 SPOOFING_STATE_MULTIPLE = 3
uint8 spoofing_state # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
uint8 SPOOFING_STATE_UNKNOWN = 0 #default
uint8 SPOOFING_STATE_OK = 1
uint8 SPOOFING_STATE_MITIGATED = 2
uint8 SPOOFING_STATE_DETECTED = 3
uint8 spoofing_state # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Mitigated, 3: Detected
# Combined authentication state (e.g. Galileo OSNMA)
uint8 AUTHENTICATION_STATE_UNKNOWN = 0 #default
uint8 AUTHENTICATION_STATE_INITIALIZING = 1
uint8 AUTHENTICATION_STATE_ERROR = 2
uint8 AUTHENTICATION_STATE_OK = 3
uint8 AUTHENTICATION_STATE_DISABLED = 4
uint8 authentication_state # GPS signal authentication state
float32 vel_m_s # GPS ground speed, (metres/sec)
float32 vel_n_m_s # GPS North velocity, (metres/sec)
@ -55,6 +63,16 @@ uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp whi
uint8 satellites_used # Number of satellites used
uint32 SYSTEM_ERROR_OK = 0 #default
uint32 SYSTEM_ERROR_INCOMING_CORRECTIONS = 1
uint32 SYSTEM_ERROR_CONFIGURATION = 2
uint32 SYSTEM_ERROR_SOFTWARE = 4
uint32 SYSTEM_ERROR_ANTENNA = 8
uint32 SYSTEM_ERROR_EVENT_CONGESTION = 16
uint32 SYSTEM_ERROR_CPU_OVERLOAD = 32
uint32 SYSTEM_ERROR_OUTPUT_CONGESTION = 64
uint32 system_error # General errors with the connected GPS receiver
float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])
float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI])
float32 heading_accuracy # heading accuracy (rad, [0, 2PI])

2
src/drivers/gnss/septentrio/CMakeLists.txt
View File

@ -37,7 +37,7 @@ px4_add_module(
COMPILE_FLAGS
# -DDEBUG_BUILD # Enable during development of the module
-DSEP_LOG_ERROR # Enable module-level error logs
# -DSEP_LOG_WARN # Enable module-level warning logs
-DSEP_LOG_WARN # Enable module-level warning logs
# -DSEP_LOG_INFO # Enable module-level info logs
# -DSEP_LOG_TRACE_PARSING # Tracing of parsing steps
SRCS

2
src/drivers/gnss/septentrio/sbf/decoder.cpp
View File

@ -163,7 +163,7 @@ int Decoder::parse(QualityInd *message) const
int Decoder::parse(RFStatus *message) const
{
if (can_parse() && id() == BlockID::PVTGeodetic) {
if (can_parse() && id() == BlockID::RFStatus) {
static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
memcpy(message, _message.payload, sizeof(RFStatus) - sizeof(RFStatus::rf_band));

18
src/drivers/gnss/septentrio/sbf/messages.h
View File

@ -243,9 +243,14 @@ struct QualityInd {
};
struct RFBand {
enum class InfoMode : uint8_t {
Suppressed = 1,
Mitigated = 2,
Interference = 8
};
uint32_t frequency;
uint16_t bandwidth;
uint8_t info_mode: 4;
uint8_t info_mode: 4;
uint8_t info_reserved: 2;
uint8_t info_antenna_id: 2;
};
@ -261,6 +266,15 @@ struct RFStatus {
};
struct GALAuthStatus {
enum class OSNMAStatus : uint16_t {
Disabled = 0,
Initializing = 1,
AwaitingTrustedTimeInfo = 2,
InitFailedInconsistentTime = 3,
InitFailedKROOTInvalid = 4,
InitFailedInvalidParam = 5,
Authenticating = 6,
};
uint16_t osnma_status_status: 3;
uint16_t osnma_status_initialization_progress: 8;
uint16_t osnma_status_trusted_time_source: 3;
@ -271,6 +285,8 @@ struct GALAuthStatus {
uint64_t gal_authentic_mask;
uint64_t gps_active_mask;
uint64_t gps_authentic_mask;
OSNMAStatus osnmaStatus() const { return static_cast<OSNMAStatus>(osnma_status_status); }
};
struct VelCovGeodetic {

150
src/drivers/gnss/septentrio/septentrio.cpp
View File

@ -92,7 +92,7 @@ constexpr size_t k_min_receiver_read_bytes = 32;
*/
constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200;
constexpr uint8_t k_max_command_size = 120;
constexpr uint8_t k_max_command_size = 140;
constexpr uint16_t k_timeout_5hz = 500;
constexpr uint32_t k_read_buffer_size = 150;
constexpr time_t k_gps_epoch_secs = 1234567890ULL; // TODO: This seems wrong
@ -112,7 +112,7 @@ constexpr const char *k_command_reset_hot = "erst,soft,none\n";
constexpr const char *k_command_reset_warm = "erst,soft,PVTData\n";
constexpr const char *k_command_reset_cold = "erst,hard,SatData\n";
constexpr const char *k_command_sbf_output_pvt =
"sso,Stream%" PRIu32 ",%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus,%s\n";
"sso,Stream%lu,%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus+GALAuthStatus+RFStatus+QualityInd,%s\n";
constexpr const char *k_command_set_sbf_output =
"sso,Stream%" PRIu32 ",%s,%s%s,%s\n";
constexpr const char *k_command_clear_sbf = "sso,Stream%" PRIu32 ",%s,none,off\n";
@ -967,7 +967,7 @@ SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
}
// Output a set of SBF blocks on a given connection at a regular interval.
snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, (long unsigned int) _receiver_stream_main, com_port, sbf_frequency);
if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
SEP_WARN("CONFIG: Failed to configure SBF");
return ConfigureResult::FailedCompletely;
@ -1191,20 +1191,138 @@ int SeptentrioDriver::process_message()
if (_sbf_decoder.parse(&receiver_status) == PX4_OK) {
_sensor_gps.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED;
_time_synced = receiver_status.rx_state_wn_set && receiver_status.rx_state_tow_set;
_sensor_gps.system_error = sensor_gps_s::SYSTEM_ERROR_OK;
if (receiver_status.rx_error_cpu_overload) {
_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_CPU_OVERLOAD;
}
if (receiver_status.rx_error_antenna) {
_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_ANTENNA;
}
if (receiver_status.ext_error_diff_corr_error) {
_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_INCOMING_CORRECTIONS;
}
if (receiver_status.ext_error_setup_error) {
_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_CONFIGURATION;
}
if (receiver_status.rx_error_software) {
_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_SOFTWARE;
}
if (receiver_status.rx_error_congestion) {
_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_OUTPUT_CONGESTION;
}
if (receiver_status.rx_error_missed_event) {
_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_EVENT_CONGESTION;
}
}
break;
}
case BlockID::QualityInd: {
using Type = QualityIndicator::Type;
SEP_TRACE_PARSING("Processing QualityInd SBF message");
QualityInd quality_ind;
if (_sbf_decoder.parse(&quality_ind) == PX4_OK) {
_message_sensor_gnss_status.quality_available = true;
_message_sensor_gnss_status.device_id = get_device_id();
_message_sensor_gnss_status.quality_corrections = 255;
_message_sensor_gnss_status.quality_receiver = 255;
_message_sensor_gnss_status.quality_post_processing = 255;
_message_sensor_gnss_status.quality_gnss_signals = 255;
for (int i = 0; i < math::min(quality_ind.n, static_cast<uint8_t>(sizeof(quality_ind.indicators) / sizeof(quality_ind.indicators[0]))); i++) {
int quality = quality_ind.indicators[i].value;
switch (quality_ind.indicators[i].type) {
case Type::BaseStationMeasurements:
_message_sensor_gnss_status.quality_corrections = quality;
break;
case Type::Overall:
_message_sensor_gnss_status.quality_receiver = quality;
break;
case Type::RTKPostProcessing:
_message_sensor_gnss_status.quality_post_processing = quality;
break;
case Type::GNSSSignalsMainAntenna:
_message_sensor_gnss_status.quality_gnss_signals = quality;
break;
default:
break;
}
}
_message_sensor_gnss_status.timestamp = hrt_absolute_time();
_time_last_qualityind_received = hrt_absolute_time();
_sensor_gnss_status_pub.publish(_message_sensor_gnss_status);
}
break;
}
case BlockID::RFStatus: {
using InfoMode = RFBand::InfoMode;
SEP_TRACE_PARSING("Processing RFStatus SBF message");
RFStatus rf_status;
if (_sbf_decoder.parse(&rf_status) == PX4_OK) {
_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_OK;
_sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_OK;
for (int i = 0; i < math::min(rf_status.n, static_cast<uint8_t>(sizeof(rf_status.rf_band) / sizeof(rf_status.rf_band[0]))); i++) {
InfoMode status = static_cast<InfoMode>(rf_status.rf_band[i].info_mode);
if(status == InfoMode::Interference){
_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_DETECTED;
break; // Worst case, we don't need to check the other bands
}
if(status == InfoMode::Suppressed || status == InfoMode::Mitigated){
_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_MITIGATED;
}
}
if (rf_status.flags_inauthentic_gnss_signals || rf_status.flags_inauthentic_navigation_message) {
_sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_DETECTED;
}
_time_last_resilience_received = hrt_absolute_time();
}
break;
}
case BlockID::GALAuthStatus: {
using OSNMAStatus = GALAuthStatus::OSNMAStatus;
SEP_TRACE_PARSING("Processing GALAuthStatus SBF message");
GALAuthStatus gal_auth_status;
if (_sbf_decoder.parse(&gal_auth_status) == PX4_OK) {
switch (gal_auth_status.osnmaStatus()) {
case OSNMAStatus::Disabled:
_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_DISABLED;
break;
case OSNMAStatus::AwaitingTrustedTimeInfo:
case OSNMAStatus::Initializing:
_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_INITIALIZING;
break;
case OSNMAStatus::InitFailedInconsistentTime:
case OSNMAStatus::InitFailedKROOTInvalid:
case OSNMAStatus::InitFailedInvalidParam:
_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_ERROR;
break;
case OSNMAStatus::Authenticating:
_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_OK;
break;
}
}
break;
}
case BlockID::EndOfPVT: {
@ -1277,6 +1395,31 @@ int SeptentrioDriver::process_message()
}
}
//Check for how recent the resilience data for reciever is, if outdated set to unknown
if ((_time_last_resilience_received != 0) && (hrt_elapsed_time(&_time_last_resilience_received) > 5_s)) {
_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_UNKNOWN;
_sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_UNKNOWN;
_time_last_resilience_received = 0; // Reset
}
// Check for how recent the status data for receiver is, if outdated set to unknown
if ((_time_last_qualityind_received != 0) && (hrt_elapsed_time(&_time_last_qualityind_received) > 5_s)) {
_message_sensor_gnss_status.quality_available = false;
_message_sensor_gnss_status.device_id = get_device_id();
_message_sensor_gnss_status.timestamp = hrt_absolute_time();
_message_sensor_gnss_status.quality_corrections = 255;
_message_sensor_gnss_status.quality_receiver = 255;
_message_sensor_gnss_status.quality_post_processing = 255;
_message_sensor_gnss_status.quality_gnss_signals = 255;
_sensor_gnss_status_pub.publish(_message_sensor_gnss_status);
_time_last_qualityind_received = 0; // Reset
}
break;
}
case DecodingStatus::RTCMv3: {
@ -1532,6 +1675,7 @@ void SeptentrioDriver::publish()
_sensor_gps.device_id = get_device_id();
_sensor_gps.selected_rtcm_instance = _selected_rtcm_instance;
_sensor_gps.rtcm_injection_rate = rtcm_injection_frequency();
_sensor_gps.timestamp = hrt_absolute_time();
_sensor_gps_pub.publish(_sensor_gps);
}

23
src/drivers/gnss/septentrio/septentrio.h
View File

@ -53,6 +53,7 @@
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_gnss_status.h>
#include <uORB/topics/gps_dump.h>
#include <uORB/topics/gps_inject_data.h>
#include <drivers/drv_hrt.h>
@ -710,12 +711,16 @@ private:
char _port[20] {}; ///< The path of the used serial device
hrt_abstime _last_rtcm_injection_time {0}; ///< Time of last RTCM injection
uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections
uint8_t _spoofing_state {0}; ///< Receiver spoofing state
uint8_t _jamming_state {0}; ///< Receiver jamming state
bool _time_synced {false}; ///< Receiver time in sync with GPS time
const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls
uint32_t _chosen_baud_rate {0}; ///< The baud rate requested by the user
static px4::atomic<SeptentrioDriver *> _secondary_instance;
hrt_abstime _sleep_end {0}; ///< End time for sleeping
State _resume_state {State::OpeningSerialPort}; ///< Resume state after sleep
hrt_abstime _time_last_qualityind_received{0}; ///< Time of last quality message reception
hrt_abstime _time_last_resilience_received{0}; ///< Time of last resilience message reception
// Module configuration
float _heading_offset {0.0f}; ///< The heading offset given by the `SEP_YAW_OFFS` parameter
@ -737,14 +742,16 @@ private:
rtcm::Decoder *_rtcm_decoder {nullptr}; ///< RTCM message decoder
// uORB topics and subscriptions
sensor_gps_s _sensor_gps{}; ///< uORB topic for position
gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver
gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver
satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position
uORB::Publication<gps_dump_s> _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver
uORB::PublicationMulti<satellite_info_s> _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info
sensor_gps_s _sensor_gps {}; ///< uORB topic for position
sensor_gnss_status_s _message_sensor_gnss_status {}; ///< uORB topic for gps status
gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver
gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver
satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position
uORB::PublicationMulti<sensor_gnss_status_s> _sensor_gnss_status_pub {ORB_ID(sensor_gnss_status)}; ///< uORB publication for gnss status
uORB::Publication<gps_dump_s> _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver
uORB::PublicationMulti<satellite_info_s> _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info
uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _gps_inject_data_sub {ORB_ID::gps_inject_data}; ///< uORB subscription about data to inject to the receiver
// Data about update frequencies of various bits of information like RTCM message injection frequency, received data rate...

8
src/examples/fake_gps/FakeGps.cpp
View File

@ -87,6 +87,14 @@ void FakeGps::Run()
sensor_gps.satellites_used = 14;
sensor_gps.timestamp = hrt_absolute_time();
_sensor_gps_pub.publish(sensor_gps);
sensor_gnss_status_s sensor_gnss_status{};
sensor_gnss_status.quality_corrections = 0;
sensor_gnss_status.quality_receiver = 9;
sensor_gnss_status.quality_gnss_signals = 10;
sensor_gnss_status.quality_post_processing = 255;
sensor_gnss_status.timestamp = hrt_absolute_time();
_sensor_gnss_status_pub.publish(sensor_gnss_status);
}
int FakeGps::task_spawn(int argc, char *argv[])

2
src/examples/fake_gps/FakeGps.hpp
View File

@ -41,6 +41,7 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_gnss_status.h>
class FakeGps : public ModuleBase<FakeGps>, public ModuleParams, public px4::ScheduledWorkItem
{
@ -66,6 +67,7 @@ private:
void Run() override;
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<sensor_gnss_status_s> _sensor_gnss_status_pub{ORB_ID(sensor_gnss_status)};
double _latitude{29.6603018}; // Latitude in degrees
double _longitude{-82.3160500}; // Longitude in degrees

6
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View File

@ -594,15 +594,15 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const
{
if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) {
if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_DETECTED) {
/* EVENT
*/
reporter.armingCheckFailure(NavModes::None, health_component_t::gps,
events::ID("check_estimator_gps_jamming_critical"),
events::Log::Critical, "GPS reports critical jamming state");
events::Log::Critical, "GPS jamming detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports critical jamming state\t");
mavlink_log_critical(reporter.mavlink_log_pub(), "GPS jamming detected\t");
}
}
}

2
src/modules/ekf2/EKF2.cpp
View File

@ -2467,7 +2467,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
.yaw = vehicle_gps_position.heading, //TODO: move to different message
.yaw_acc = vehicle_gps_position.heading_accuracy,
.yaw_offset = vehicle_gps_position.heading_offset,
.spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_MULTIPLE,
.spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED,
};
_ekf.setGpsData(gnss_sample);

4
src/modules/mavlink/mavlink_main.cpp
View File

@ -1441,6 +1441,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
configure_stream_local("GLOBAL_POSITION", 5.0f);
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
configure_stream_local("GNSS_INTEGRITY", 1.0f);
configure_stream_local("GPS2_RAW", 1.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS_RAW_INT", 5.0f);
@ -1513,6 +1514,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
configure_stream_local("GNSS_INTEGRITY", 1.0f);
configure_stream_local("GPS2_RAW", unlimited_rate);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
@ -1673,6 +1675,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ESTIMATOR_STATUS", 5.0f);
configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GNSS_INTEGRITY", 1.0f);
configure_stream_local("GPS2_RAW", unlimited_rate);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
@ -1775,6 +1778,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GLOBAL_POSITION", 10.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GNSS_INTEGRITY", 1.0f);
configure_stream_local("GPS2_RAW", unlimited_rate);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
configure_stream_local("HOME_POSITION", 0.5f);

6
src/modules/mavlink/mavlink_messages.cpp
View File

@ -79,6 +79,9 @@
#include "streams/GLOBAL_POSITION.hpp"
#endif //MAVLINK_MSG_ID_GLOBAL_POSITION
#include "streams/GLOBAL_POSITION_INT.hpp"
#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
#include "streams/GNSS_INTEGRITY.hpp"
#endif
#include "streams/GPS_GLOBAL_ORIGIN.hpp"
#include "streams/GPS_RAW_INT.hpp"
#include "streams/GPS_RTCM_DATA.hpp"
@ -507,6 +510,9 @@ static const StreamListItem streams_list[] = {
#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
#if defined(GNSS_INTEGRITY_HPP)
create_stream_list_item<MavlinkStreamGNSSIntegrity>(),
#endif // GNSS_INTEGRITY_HPP
#if defined(AVAILABLE_MODES_HPP)
create_stream_list_item<MavlinkStreamAvailableModes>(),
#endif // AVAILABLE_MODES_HPP

115
src/modules/mavlink/streams/GNSS_INTEGRITY.hpp Normal file
View File

@ -0,0 +1,115 @@
/****************************************************************************
*
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef GNSS_INTEGRITY_HPP
#define GNSS_INTEGRITY_HPP
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_gnss_status.h>
#include <uORB/PublicationMulti.hpp>
using namespace time_literals;
class MavlinkStreamGNSSIntegrity : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGNSSIntegrity(mavlink); }
static constexpr const char *get_name_static() { return "GNSS_INTEGRITY"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GNSS_INTEGRITY; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _vehicle_gps_position_sub.advertised() ? (MAVLINK_MSG_ID_GNSS_INTEGRITY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
static constexpr int GPS_MAX_RECEIVERS = 2;
explicit MavlinkStreamGNSSIntegrity(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionMultiArray<sensor_gnss_status_s, GPS_MAX_RECEIVERS> _sensor_gnss_status_sub{ORB_ID::sensor_gnss_status};
bool send() override
{
sensor_gps_s vehicle_gps_position{};
if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
mavlink_gnss_integrity_t msg{};
msg.id = vehicle_gps_position.device_id;
msg.system_errors = vehicle_gps_position.system_error;
msg.authentication_state = vehicle_gps_position.authentication_state;
msg.jamming_state = vehicle_gps_position.jamming_state;
msg.spoofing_state = vehicle_gps_position.spoofing_state;
msg.corrections_quality = UINT8_MAX;
msg.system_status_summary = UINT8_MAX;
msg.gnss_signal_quality = UINT8_MAX;
msg.post_processing_quality = UINT8_MAX;
for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
sensor_gnss_status_s sensor_gnss_status{};
if (_sensor_gnss_status_sub[i].copy(&sensor_gnss_status)) {
if ((hrt_elapsed_time(&sensor_gnss_status.timestamp) < 3_s)
&& (sensor_gnss_status.device_id == vehicle_gps_position.device_id)
&& (sensor_gnss_status.quality_available)) {
msg.corrections_quality = sensor_gnss_status.quality_corrections;
msg.system_status_summary = sensor_gnss_status.quality_receiver;
msg.gnss_signal_quality = sensor_gnss_status.quality_gnss_signals;
msg.post_processing_quality = sensor_gnss_status.quality_post_processing;
break;
}
}
}
msg.raim_hfom = UINT16_MAX;
msg.raim_vfom = UINT16_MAX;
mavlink_msg_gnss_integrity_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // GNSS_INTEGRITY_HPP