fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 14:27:35 +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
15 changed files with 360 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+3 -3
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");
}
}
}
src/modules/ekf2/EKF2.cpp
+1 -1
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);
src/modules/mavlink/mavlink_main.cpp
+4
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);
src/modules/mavlink/mavlink_messages.cpp
+6
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
src/modules/mavlink/streams/GNSS_INTEGRITY.hpp
+115
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