From 442cd50d31aee08b2c8e5e66e427882eecc0efe9 Mon Sep 17 00:00:00 2001 From: oystub Date: Wed, 27 Aug 2025 13:08:32 +0000 Subject: [PATCH 1/2] sensors: add vehicle_gnss_heading to sensor bridge Modelled after vehicle_gps_position, it processes incoming sensor_gnss_relative messages and outputs vehicle_gps_yaw with a validated body-frame heading measurement. In the future, this can be used in the EKF, instead of the existing hacky solution which only works with blending enabled. Note that only one instance is supported. This is because PX4 only has support for two GNSS receivers anyway, so there isn't "room" for multiple rovers that output heading. --- msg/CMakeLists.txt | 1 + msg/vehicle_gnss_heading.msg | 5 + src/modules/logger/logged_topics.cpp | 2 + src/modules/sensors/CMakeLists.txt | 2 + src/modules/sensors/sensor_params.c | 36 ++++ src/modules/sensors/sensors.cpp | 28 +++ .../vehicle_gnss_heading/CMakeLists.txt | 8 + .../VehicleGNSSHeading.cpp | 169 ++++++++++++++++++ .../VehicleGNSSHeading.hpp | 56 ++++++ .../sensors/vehicle_gnss_heading/params.c | 24 +++ 10 files changed, 331 insertions(+) create mode 100644 msg/vehicle_gnss_heading.msg create mode 100644 src/modules/sensors/vehicle_gnss_heading/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.cpp create mode 100644 src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.hpp create mode 100644 src/modules/sensors/vehicle_gnss_heading/params.c diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 423dc6130b57..f7c537135519 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -186,6 +186,7 @@ set(msg_files vehicle_control_mode.msg vehicle_global_position.msg vehicle_gps_position.msg + vehicle_gnss_heading.msg vehicle_imu.msg vehicle_imu_status.msg vehicle_land_detected.msg diff --git a/msg/vehicle_gnss_heading.msg b/msg/vehicle_gnss_heading.msg new file mode 100644 index 000000000000..e1dd4c47cbcb --- /dev/null +++ b/msg/vehicle_gnss_heading.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start(microseconds) +uint64 timestamp_sample # time since system start(microseconds) + +float32 heading # Heading angle of XYZ body frame rel to NED.(radians) +float32 heading_accuracy # Accuracy of heading of the relative position vector(radians) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index dc635653e049..f0b123e200d1 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -107,6 +107,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_control_mode"); add_topic("vehicle_global_position", 200); add_topic("vehicle_gps_position", 500); + add_topic("vehicle_gnss_heading", 500); add_topic("vehicle_land_detected"); add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); @@ -280,6 +281,7 @@ void LoggedTopics::add_estimator_replay_topics() add_topic("sensor_selection"); add_topic("vehicle_air_data"); add_topic("vehicle_gps_position"); + add_topic("vehicle_gnss_heading"); add_topic("vehicle_land_detected"); add_topic("vehicle_magnetometer"); add_topic("vehicle_status"); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0eb8a7e0a968..fbbcc245d5c0 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -38,6 +38,7 @@ add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) add_subdirectory(vehicle_air_data) add_subdirectory(vehicle_gps_position) +add_subdirectory(vehicle_gnss_heading) add_subdirectory(vehicle_imu) add_subdirectory(vehicle_magnetometer) @@ -60,6 +61,7 @@ px4_add_module( vehicle_angular_velocity vehicle_air_data vehicle_gps_position + vehicle_gnss_heading vehicle_imu vehicle_magnetometer ) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fd93e18dff7a..58010423e3ed 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -233,3 +233,39 @@ PARAM_DEFINE_INT32(SENS_IMU_MODE, 1); * @group Sensors */ PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1); + +/** + * GPS Relative X offset + * + * The body-frame offset between the GPS moving baseline and the rover. + * A positive value means the rover antenna is in front of the moving baseline. + * + * @group Sensors + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(SENS_GNSSREL_PX, 0); + +/** + * GPS Relative Y offset + * + * The body-frame offset between the GPS moving baseline and the rover. + * A positive value means the rover antenna is to the right of the moving baseline. + * + * @group Sensors + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(SENS_GNSSREL_PY, 0); + +/** + * GPS Relative Z offset + * + * The body-frame offset between the GPS moving baseline and the rover. + * A positive value means the rover antenna is below the moving baseline. + * + * @group Sensors + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(SENS_GNSSREL_PZ, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 30ba6aeaac1e..7abc8c0cdb9d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -76,6 +76,7 @@ #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" #include "vehicle_air_data/VehicleAirData.hpp" #include "vehicle_gps_position/VehicleGPSPosition.hpp" +#include "vehicle_gnss_heading/VehicleGNSSHeading.hpp" #include "vehicle_imu/VehicleIMU.hpp" #include "vehicle_magnetometer/VehicleMagnetometer.hpp" @@ -181,6 +182,7 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch VehicleAirData *_vehicle_air_data{nullptr}; VehicleMagnetometer *_vehicle_magnetometer{nullptr}; VehicleGPSPosition *_vehicle_gps_position{nullptr}; + VehicleGNSSHeading *_vehicle_gnss_heading{nullptr}; VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; @@ -218,6 +220,7 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch void InitializeVehicleAirData(); void InitializeVehicleGPSPosition(); + void InitializeVehicleGNSSHeading(); void InitializeVehicleIMU(); void InitializeVehicleMagnetometer(); @@ -267,6 +270,7 @@ Sensors::Sensors(bool hil_enabled) : InitializeVehicleAirData(); InitializeVehicleGPSPosition(); + InitializeVehicleGNSSHeading(); InitializeVehicleIMU(); InitializeVehicleMagnetometer(); } @@ -291,6 +295,11 @@ Sensors::~Sensors() delete _vehicle_gps_position; } + if (_vehicle_gnss_heading) { + _vehicle_gnss_heading->Stop(); + delete _vehicle_gnss_heading; + } + if (_vehicle_magnetometer) { _vehicle_magnetometer->Stop(); delete _vehicle_magnetometer; @@ -385,6 +394,7 @@ int Sensors::parameters_update() InitializeVehicleAirData(); InitializeVehicleGPSPosition(); + InitializeVehicleGNSSHeading(); InitializeVehicleMagnetometer(); return PX4_OK; @@ -589,6 +599,19 @@ void Sensors::InitializeVehicleGPSPosition() } } +void Sensors::InitializeVehicleGNSSHeading() +{ + if (_param_sys_has_gps.get()) { + if (_vehicle_gnss_heading == nullptr) { + _vehicle_gnss_heading = new VehicleGNSSHeading(); + + if (_vehicle_gnss_heading) { + _vehicle_gnss_heading->Start(); + } + } + } +} + void Sensors::InitializeVehicleIMU() { // create a VehicleIMU instance for each accel/gyro pair @@ -791,6 +814,11 @@ int Sensors::print_status() _vehicle_gps_position->PrintStatus(); } + if (_vehicle_gnss_heading) { + PX4_INFO_RAW("\n"); + _vehicle_gnss_heading->PrintStatus(); + } + PX4_INFO_RAW("\n"); for (auto &i : _vehicle_imu_list) { diff --git a/src/modules/sensors/vehicle_gnss_heading/CMakeLists.txt b/src/modules/sensors/vehicle_gnss_heading/CMakeLists.txt new file mode 100644 index 000000000000..e216d6e2d5d1 --- /dev/null +++ b/src/modules/sensors/vehicle_gnss_heading/CMakeLists.txt @@ -0,0 +1,8 @@ +px4_add_library(vehicle_gnss_heading + VehicleGNSSHeading.cpp + VehicleGNSSHeading.hpp +) +target_link_libraries(vehicle_gnss_heading + PRIVATE + px4_work_queue +) diff --git a/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.cpp b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.cpp new file mode 100644 index 000000000000..3152b37cfe9f --- /dev/null +++ b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.cpp @@ -0,0 +1,169 @@ +#include "VehicleGNSSHeading.hpp" + +#include +#include +#include +#include + +namespace sensors +{ +VehicleGNSSHeading::VehicleGNSSHeading() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + _vehicle_gnss_heading_pub.advertise(); +} + +VehicleGNSSHeading::~VehicleGNSSHeading() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleGNSSHeading::Start() +{ + // force initial updates + ParametersUpdate(true); + + _sensor_gnss_relative_sub.registerCallback(); + + ScheduleNow(); + + return true; +} + +void VehicleGNSSHeading::Stop() +{ + Deinit(); + _sensor_gnss_relative_sub.unregisterCallback(); +} + +void VehicleGNSSHeading::ParametersUpdate(bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + const float bx = _params_gpsrel_px.get(); + const float by = _params_gpsrel_py.get(); + const float bz = _params_gpsrel_pz.get(); + + const float baseline_length = sqrtf(bx * bx + by * by + bz * bz); + + if (baseline_length < 0.3f) { + PX4_ERR("[vehicle_gnss_heading]Expected baseline length is too small: %.2f m", (double)baseline_length); + _expected_baseline_length_m = NAN; + _baseline_body_angle = NAN; + return; + } + + _expected_baseline_length_m = baseline_length; + _baseline_body_angle = atan2f(by, bx); +} + +void VehicleGNSSHeading::Run() +{ + perf_begin(_cycle_perf); + ParametersUpdate(); + + if (PX4_ISFINITE(_expected_baseline_length_m) && PX4_ISFINITE(_baseline_body_angle) + && _sensor_gnss_relative_sub.updated()) { + sensor_gnss_relative_s d{}; + _sensor_gnss_relative_sub.copy(&d); + + // Basic validity + if (!PX4_ISFINITE(d.heading) || !PX4_ISFINITE(d.heading_accuracy) || + !PX4_ISFINITE(d.position_length) || !d.heading_valid || !d.relative_position_valid) { + perf_end(_cycle_perf); + return; + } + + if (_params_gpsyaw_crq.get() == 1 && (!d.carrier_solution_floating || !d.carrier_solution_fixed)) { + perf_end(_cycle_perf); + return; + } + + if (_params_gpsyaw_crq.get() == 2 && !d.carrier_solution_fixed) { + perf_end(_cycle_perf); + return; + } + + // The reported baseline length compared to expected is a good indication of accuracy. + // Additionally, it prevents potentially large errors if the wrong RTCM stream is sent to the receiver. + const float gate_len = _params_gpsyaw_lrq.get(); + + if (PX4_ISFINITE(gate_len) && gate_len >= 0.0f) { + const float len_diff = fabsf(d.position_length - _expected_baseline_length_m); + + if (len_diff > gate_len) { + perf_end(_cycle_perf); + return; + } + } + + const hrt_abstime prev_valid = _last_valid_timestamp; + _last_valid_timestamp = d.timestamp; + + // Reject if the delta time to previous valid update is too large, ublox receivers can output stale data + // when in rover mode. F9P rovers publish every 125 ms, we accept up to 200 ms additional delay. + if (prev_valid == 0 || d.timestamp - prev_valid > 325_ms) { + perf_end(_cycle_perf); + return; + } + + // Rotate into vehicle frame using configured antenna position + const float vehicle_yaw = matrix::wrap_pi(d.heading - _baseline_body_angle); + + // publish + vehicle_gnss_heading_s out{ + .timestamp = d.timestamp, + .timestamp_sample = d.timestamp_sample, + .heading = vehicle_yaw, + .heading_accuracy = d.heading_accuracy + }; + + _vehicle_gnss_heading_pub.publish(out); + } + + perf_end(_cycle_perf); +} + +void VehicleGNSSHeading::PrintStatus() +{ + PX4_INFO("VehicleGNSSHeading status:"); + + const float bx = _params_gpsrel_px.get(); + const float by = _params_gpsrel_py.get(); + const float bz = _params_gpsrel_pz.get(); + const float gate_len = _params_gpsyaw_lrq.get(); + const int32_t crq = _params_gpsyaw_crq.get(); + + PX4_INFO(" Antenna baseline (body) [m]: bx=%.2f by=%.2f bz=%.2f", (double)bx, (double)by, (double)bz); + PX4_INFO(" Expected baseline length [m]: %s%.2f", + isfinite(_expected_baseline_length_m) ? "" : "(invalid) ", + (double)_expected_baseline_length_m); + + if (PX4_ISFINITE(gate_len) && gate_len >= 0.f) { + PX4_INFO(" Baseline length gate [m]: %.2f m", (double)gate_len); + + } else { + PX4_INFO(" Baseline length gate: disabled"); + } + + PX4_INFO(" Required carrier solution: %d (0=None, 1=Floating or Fixed, 2=Fixed)", (int)crq); + + if (_last_valid_timestamp != 0) { + const float since_last_valid_s = (hrt_absolute_time() - _last_valid_timestamp) * 1e-6f; + PX4_INFO(" Last valid GNSS-relative message: %.3f s ago", (double)since_last_valid_s); + + } else { + PX4_INFO(" Last valid GNSS-relative message: none yet"); + } + + perf_print_counter(_cycle_perf); +} + +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.hpp b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.hpp new file mode 100644 index 000000000000..d800ea760d84 --- /dev/null +++ b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ +class VehicleGNSSHeading : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VehicleGNSSHeading(); + ~VehicleGNSSHeading() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + + uORB::Publication _vehicle_gnss_heading_pub{ORB_ID(vehicle_gnss_heading)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + // Only one subscription callback for GNSS relative, since PX4 currently only supports two receivers, + // i.e. one base/rover pair. + uORB::SubscriptionCallbackWorkItem _sensor_gnss_relative_sub{this, ORB_ID(sensor_gnss_relative), 0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamFloat) _params_gpsrel_px, + (ParamFloat) _params_gpsrel_py, + (ParamFloat) _params_gpsrel_pz, + (ParamFloat) _params_gpsyaw_lrq, + (ParamInt) _params_gpsyaw_crq + ) + + float _expected_baseline_length_m{NAN}; + float _baseline_body_angle{NAN}; + hrt_abstime _last_valid_timestamp{0}; +}; +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_gnss_heading/params.c b/src/modules/sensors/vehicle_gnss_heading/params.c new file mode 100644 index 000000000000..9b90f41bd9f6 --- /dev/null +++ b/src/modules/sensors/vehicle_gnss_heading/params.c @@ -0,0 +1,24 @@ +/** + * GPS Yaw Baseline Length Accuracy Requirement + * + * Discard any GPS yaw data if the baseline length deviates from the expected length by more than this value. + * Expected length is calculated from the SENS_GNSSREL_P{X,Y,Z} parameters. + * A value of 0 disables the gate. (This is not recommended.) + * + * @group Sensors + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(SENS_GNSSHDG_LRQ, 0.1f); + +/** + * Required carrier phase range solution for GPS Yaw + * + * @value 0 None + * @value 1 Floating or Fixed + * @value 2 Fixed + * @group Sensors + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(SENS_GNSSHDG_CRQ, 2); From c45d7704d96591014fcb91258f04b6b981431f5b Mon Sep 17 00:00:00 2001 From: oystub Date: Wed, 27 Aug 2025 13:13:28 +0000 Subject: [PATCH 2/2] ekf2: use vehicle_gnss_heading as source for rtk heading measurements --- src/modules/ekf2/EKF/common.h | 20 +++++-- src/modules/ekf2/EKF/control.cpp | 36 +++++------- src/modules/ekf2/EKF/ekf.h | 8 ++- src/modules/ekf2/EKF/ekf_helper.cpp | 4 -- src/modules/ekf2/EKF/estimator_interface.cpp | 40 +++++++++++++ src/modules/ekf2/EKF/estimator_interface.h | 5 ++ src/modules/ekf2/EKF/gps_control.cpp | 2 - src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 30 ++++------ src/modules/ekf2/EKF2.cpp | 26 +++++++++ src/modules/ekf2/EKF2.hpp | 4 ++ src/modules/ekf2/test/CMakeLists.txt | 2 +- .../ekf2/test/sensor_simulator/CMakeLists.txt | 1 + .../test/sensor_simulator/ekf_wrapper.cpp | 2 +- .../ekf2/test/sensor_simulator/ekf_wrapper.h | 2 +- .../test/sensor_simulator/gnss_heading.cpp | 58 +++++++++++++++++++ .../ekf2/test/sensor_simulator/gnss_heading.h | 31 ++++++++++ .../sensor_simulator/sensor_simulator.cpp | 18 +++++- .../test/sensor_simulator/sensor_simulator.h | 7 ++- ..._gps_yaw.cpp => test_EKF_gnss_heading.cpp} | 57 +++++++++--------- 19 files changed, 266 insertions(+), 87 deletions(-) create mode 100644 src/modules/ekf2/test/sensor_simulator/gnss_heading.cpp create mode 100644 src/modules/ekf2/test/sensor_simulator/gnss_heading.h rename src/modules/ekf2/test/{test_EKF_gps_yaw.cpp => test_EKF_gnss_heading.cpp} (88%) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b107c43d1e05..322f728fc0bf 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -79,6 +79,12 @@ struct gps_message { float pdop; ///< position dilution of precision }; +struct gnss_heading_message { + uint64_t time_usec{0}; ///< timestamp of the measurement (uSec) + float heading; ///< heading angle in radians + float heading_accuracy; ///< heading accuracy in radians +}; + struct outputSample { uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Quatf quat_nominal; ///< nominal quaternion describing vehicle attitude @@ -112,6 +118,11 @@ struct gpsSample { float vacc; ///< 1-std vertical position error (m) float sacc; ///< 1-std speed error (m/sec) }; +struct gnssHeadingSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) + float heading; ///< heading angle in radians + float heading_accuracy; ///< heading accuracy in radians +}; struct magSample { uint64_t time_us{0}; ///< timestamp of the measurement (uSec) @@ -202,10 +213,11 @@ enum TerrainFusionMask : int32_t { #define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter. // Maximum sensor intervals in usec -#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec) -#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec) -#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec) -#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) +#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec) +#define GNSS_HEADING_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GNSS heading measurements (uSec) +#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec) +#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec) +#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) // bad accelerometer detection and mitigation #define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 0682e1214ac7..29407b629c03 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -123,6 +123,12 @@ void Ekf::controlFusionModes() } } + if (_gnss_heading_buffer) { + // check for arrival of new sensor data at the fusion time horizon + _time_prev_gnss_heading_us = _gnss_heading_sample_delayed.time_us; + _gnss_heading_data_ready = _gnss_heading_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gnss_heading_sample_delayed); + } + if (_range_buffer) { // Get range data from buffer and check validity bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); @@ -179,6 +185,7 @@ void Ekf::controlFusionModes() controlMagFusion(); controlOpticalFlowFusion(); controlGpsFusion(); + controlGNSSHeadingFusion(); controlAirDataFusion(); controlBetaFusion(); controlDragFusion(); @@ -554,7 +561,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks() _time_good_motion_us = _imu_sample_delayed.time_us; } -void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) +void Ekf::controlGNSSHeadingFusion() { if (!(_params.fusion_mode & MASK_USE_GPSYAW) || _control_status.flags.gps_yaw_fault) { @@ -563,29 +570,20 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) return; } - const bool is_new_data_available = PX4_ISFINITE(_gps_sample_delayed.yaw); - - if (is_new_data_available) { - - const bool continuing_conditions_passing = !gps_checks_failing; + if (_gnss_heading_data_ready) { - const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL); + const bool is_gnss_heading_data_intermittent = !isRecent(_time_last_gnss_heading_data, 2 * GNSS_HEADING_MAX_INTERVAL); - const bool starting_conditions_passing = continuing_conditions_passing - && _control_status.flags.tilt_align - && gps_checks_passing - && !is_gps_yaw_data_intermittent - && !_gps_intermittent; + const bool starting_conditions_passing = _control_status.flags.tilt_align + && !is_gnss_heading_data_intermittent; - _time_last_gps_yaw_data = _time_last_imu; + _time_last_gnss_heading_data = _time_last_imu; if (_control_status.flags.gps_yaw) { - if (continuing_conditions_passing) { - fuseGpsYaw(); - const bool is_fusion_failing = isTimedOut(_time_last_gps_yaw_fuse, _params.reset_timeout_max); + const bool is_fusion_failing = isTimedOut(_time_last_gnss_heading_fuse, _params.reset_timeout_max); if (is_fusion_failing) { if (_nb_gps_yaw_reset_available > 0) { @@ -611,10 +609,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) // TODO: should we give a new reset credit when the fusion does not fail for some time? } - } else { - // Stop GPS yaw fusion but do not declare it faulty - stopGpsYawFusion(); - } } else { if (starting_conditions_passing) { @@ -627,7 +621,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } } - } else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) { + } else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gnss_heading_data, _params.reset_timeout_max)) { // No yaw data in the message anymore. Stop until it comes back. stopGpsYawFusion(); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 6eeb52fb1af8..3d25ff3b56ce 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -376,6 +376,7 @@ class Ekf final : public EstimatorInterface // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused + bool _gnss_heading_data_ready{false};///< true when new GNSS heading data has fallen behind the fusion time horizon and is available to be fused bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused @@ -383,6 +384,7 @@ class Ekf final : public EstimatorInterface bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec) + uint64_t _time_prev_gnss_heading_us{0}; ///< time stamp of previous GNSS heading data retrieved from the buffer (uSec) uint64_t _time_last_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift @@ -396,8 +398,8 @@ class Ekf final : public EstimatorInterface uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_fake_pos_fuse{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) - uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) - uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec) + uint64_t _time_last_gnss_heading_fuse{0}; ///< time the last fusion of GNSS heading measurements were performed (uSec) + uint64_t _time_last_gnss_heading_data{0}; ///< time the last GNSS heading measurement was available (uSec) uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source @@ -833,7 +835,7 @@ class Ekf final : public EstimatorInterface bool hasHorizontalAidingTimedOut() const; bool isYawFailure() const; - void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing); + void controlGNSSHeadingFusion(); // control fusion of magnetometer observations void controlMagFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e16f66551dac..4ab6a178e08d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1559,10 +1559,6 @@ void Ekf::stopGpsFusion() stopGpsVelFusion(); } - if (_control_status.flags.gps_yaw) { - stopGpsYawFusion(); - } - // We do not need to know the true North anymore // EV yaw can start again _inhibit_ev_yaw_use = false; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index d94b2a076a61..8614f2b9f2b3 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -47,6 +47,7 @@ EstimatorInterface::~EstimatorInterface() { delete _gps_buffer; + delete _gnss_heading_buffer; delete _mag_buffer; delete _baro_buffer; delete _range_buffer; @@ -193,6 +194,41 @@ void EstimatorInterface::setGpsData(const gps_message &gps) } } +void EstimatorInterface::setGnssHeadingData(const gnss_heading_message &gnss_heading) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_gnss_heading_buffer == nullptr) { + _gnss_heading_buffer = new RingBuffer(_obs_buffer_length); + + if (_gnss_heading_buffer == nullptr || !_gnss_heading_buffer->valid()) { + delete _gnss_heading_buffer; + _gnss_heading_buffer = nullptr; + printBufferAllocationFailed("GNSS heading"); + return; + } + } + + if ((gnss_heading.time_usec - _time_last_gnss_heading) > _min_obs_interval_us) { + _time_last_gnss_heading = gnss_heading.time_usec; + + gnssHeadingSample gnss_heading_sample_new; + + gnss_heading_sample_new.time_us = gnss_heading.time_usec - static_cast(_params.gps_delay_ms * 1000); + gnss_heading_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + + gnss_heading_sample_new.heading = gnss_heading.heading; + gnss_heading_sample_new.heading_accuracy = gnss_heading.heading_accuracy; + + _gnss_heading_buffer->push(gnss_heading_sample_new); + } else { + ECL_ERR("GNSS heading data too fast %" PRIu64, gnss_heading.time_usec - _time_last_gnss_heading); + } +} + void EstimatorInterface::setBaroData(const baroSample &baro_sample) { if (!_initialised) { @@ -564,6 +600,10 @@ void EstimatorInterface::print_status() printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); } + if (_gnss_heading_buffer) { + printf("gnss heading buffer: %d/%d (%d Bytes)\n", _gnss_heading_buffer->entries(), _gnss_heading_buffer->get_length(), _gnss_heading_buffer->get_total_size()); + } + if (_mag_buffer) { printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f98d3394d90e..aef8525e746f 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -86,6 +86,8 @@ class EstimatorInterface void setGpsData(const gps_message &gps); + void setGnssHeadingData(const gnss_heading_message &gnss_heading_sample); + void setBaroData(const baroSample &baro_sample); void setAirspeedData(const airspeedSample &airspeed_sample); @@ -294,6 +296,7 @@ class EstimatorInterface // measurement samples capturing measurements on the delayed time horizon baroSample _baro_sample_delayed{}; gpsSample _gps_sample_delayed{}; + gnssHeadingSample _gnss_heading_sample_delayed{}; sensor::SensorRangeFinder _range_sensor{}; airspeedSample _airspeed_sample_delayed{}; flowSample _flow_sample_delayed{}; @@ -363,6 +366,7 @@ class EstimatorInterface RingBuffer _output_vert_buffer{12}; RingBuffer *_gps_buffer{nullptr}; + RingBuffer *_gnss_heading_buffer{nullptr}; RingBuffer *_mag_buffer{nullptr}; RingBuffer *_baro_buffer{nullptr}; RingBuffer *_range_buffer{nullptr}; @@ -375,6 +379,7 @@ class EstimatorInterface // timestamps of latest in buffer saved measurement in microseconds uint64_t _time_last_imu{0}; uint64_t _time_last_gps{0}; + uint64_t _time_last_gnss_heading{0}; uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec) uint64_t _time_last_baro{0}; uint64_t _time_last_range{0}; diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index c78eea087167..aa254238240c 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -51,8 +51,6 @@ void Ekf::controlGpsFusion() const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); - controlGpsYawFusion(gps_checks_passing, gps_checks_failing); - // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently const bool mandatory_conditions_passing = _control_status.flags.tilt_align diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 317744da17e8..9b3cf5cbc690 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -53,16 +53,12 @@ void Ekf::fuseGpsYaw() const float &q2 = _state.quat_nominal(2); const float &q3 = _state.quat_nominal(3); - // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset); - - // define the predicted antenna array vector and rotate into earth frame - const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; - const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; + // First column of rotation matrix = body x-axis in earth frame + const Vector3f ant_vec_ef = _R_to_earth.col(0); // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading - if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { - return; + if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + return; } // calculate predicted antenna yaw angle @@ -138,7 +134,7 @@ void Ekf::fuseGpsYaw() // calculate the innovation and define the innovation gate const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - _heading_innov = predicted_hdg - measured_hdg; + _heading_innov = predicted_hdg - _gnss_heading_sample_delayed.heading; // wrap the innovation to the interval between +-pi _heading_innov = wrap_pi(_heading_innov); @@ -191,28 +187,26 @@ void Ekf::fuseGpsYaw() _fault_status.flags.bad_hdg = !is_fused; if (is_fused) { - _time_last_gps_yaw_fuse = _time_last_imu; + _time_last_gnss_heading_fuse = _time_last_imu; } } bool Ekf::resetYawToGps() { - // define the predicted antenna array vector and rotate into earth frame - const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; - const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; + // First column of rotation matrix = body x-axis in earth frame + const Vector3f ant_vec_ef = _R_to_earth.col(0); // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading - if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { return false; } - // GPS yaw measurement is alreday compensated for antenna offset in the driver - const float measured_yaw = _gps_sample_delayed.yaw; + // TODO: Consider using the reported variance const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); - resetQuatStateYaw(measured_yaw, yaw_variance, true); + resetQuatStateYaw(_gnss_heading_sample_delayed.heading, yaw_variance, true); - _time_last_gps_yaw_fuse = _time_last_imu; + _time_last_gnss_heading_fuse = _time_last_imu; _yaw_signed_test_ratio_lpf.reset(0.f); return true; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 166a08f1afe6..326085d026c9 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -188,6 +188,7 @@ EKF2::~EKF2() perf_free(_msg_missed_airspeed_perf); perf_free(_msg_missed_distance_sensor_perf); perf_free(_msg_missed_gps_perf); + perf_free(_msg_missed_gnss_heading_perf); perf_free(_msg_missed_landing_target_pose_perf); perf_free(_msg_missed_magnetometer_perf); perf_free(_msg_missed_odometry_perf); @@ -252,6 +253,7 @@ int EKF2::print_status() perf_print_counter(_msg_missed_airspeed_perf); perf_print_counter(_msg_missed_distance_sensor_perf); perf_print_counter(_msg_missed_gps_perf); + perf_print_counter(_msg_missed_gnss_heading_perf); perf_print_counter(_msg_missed_landing_target_pose_perf); perf_print_counter(_msg_missed_magnetometer_perf); perf_print_counter(_msg_missed_odometry_perf); @@ -579,6 +581,7 @@ void EKF2::Run() UpdateBaroSample(ekf2_timestamps); UpdateFlowSample(ekf2_timestamps); UpdateGpsSample(ekf2_timestamps); + UpdateGNSSHeadingSample(ekf2_timestamps); UpdateMagSample(ekf2_timestamps); UpdateRangeSample(ekf2_timestamps); @@ -1764,6 +1767,29 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) } } +void EKF2::UpdateGNSSHeadingSample(ekf2_timestamps_s &ekf2_timestamps) +{ + const unsigned last_generation = _vehicle_gnss_heading_sub.get_last_generation(); + vehicle_gnss_heading_s vehicle_gnss_heading; + + if (_vehicle_gnss_heading_sub.update(&vehicle_gnss_heading)) { + if (_msg_missed_gnss_heading_perf == nullptr) { + _msg_missed_gnss_heading_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_gnss_heading messages missed"); + + } else if (_vehicle_gnss_heading_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_gnss_heading_perf); + } + + gnss_heading_message gps_yaw_msg{ + .time_usec = vehicle_gnss_heading.timestamp, + .heading = vehicle_gnss_heading.heading, + .heading_accuracy = vehicle_gnss_heading.heading_accuracy, + }; + _ekf.setGnssHeadingData(gps_yaw_msg); + } + +} + void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) { const unsigned last_generation = _magnetometer_sub.get_last_generation(); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index fe7fbdca636c..abadafd1314e 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -86,6 +86,7 @@ #include #include #include +#include #include #include #include @@ -160,6 +161,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom); bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateGNSSHeadingSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); @@ -194,6 +196,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem perf_counter_t _msg_missed_airspeed_validated_perf{nullptr}; perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; perf_counter_t _msg_missed_gps_perf{nullptr}; + perf_counter_t _msg_missed_gnss_heading_perf{nullptr}; perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; @@ -257,6 +260,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_gnss_heading_sub{ORB_ID(vehicle_gnss_heading)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 6a2d9dc2296b..67b760cf8772 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -42,7 +42,7 @@ px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_s px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) -px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_gnss_heading.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_initialization.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt index 58bdcee73388..6916484f42c3 100644 --- a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt +++ b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt @@ -41,6 +41,7 @@ set(SRCS mag.cpp baro.cpp gps.cpp + gnss_heading.cpp flow.cpp range_finder.cpp vio.cpp diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index e90e01282fbb..38dd0ab0e751 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -65,7 +65,7 @@ bool EkfWrapper::isIntendingGpsFusion() const return _ekf->control_status_flags().gps; } -void EkfWrapper::enableGpsHeadingFusion() +void EkfWrapper::enableGnssHeadingFusion() { _ekf_params->fusion_mode |= MASK_USE_GPSYAW; } diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 80b4cc064436..00347940f250 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -65,7 +65,7 @@ class EkfWrapper void disableGpsFusion(); bool isIntendingGpsFusion() const; - void enableGpsHeadingFusion(); + void enableGnssHeadingFusion(); void disableGpsHeadingFusion(); bool isIntendingGpsHeadingFusion() const; diff --git a/src/modules/ekf2/test/sensor_simulator/gnss_heading.cpp b/src/modules/ekf2/test/sensor_simulator/gnss_heading.cpp new file mode 100644 index 000000000000..20f04618b4e9 --- /dev/null +++ b/src/modules/ekf2/test/sensor_simulator/gnss_heading.cpp @@ -0,0 +1,58 @@ +#include "gnss_heading.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +GnssHeading::GnssHeading(std::shared_ptr ekf) : Sensor(ekf) +{ +} + +GnssHeading::~GnssHeading() +{ +} + +void GnssHeading::send(const uint64_t time) +{ + if (!PX4_ISFINITE(_gnss_heading_data.heading)) { + // if heading is not set, don't send, as in reality, this data wouldn't be published to vehicle_gnss_heading + return; + } + + // keep dt in case you want to evolve heading with a rate later + const float dt = static_cast(time - _gnss_heading_data.time_usec) * 1e-6f; + (void)dt; + + _gnss_heading_data.time_usec = time; + + // hand the sample to the EKF + _ekf->setGnssHeadingData(_gnss_heading_data); +} + +void GnssHeading::setData(const gnss_heading_message &gps_yaw) +{ + _gnss_heading_data = gps_yaw; +} + +void GnssHeading::setHeading(const float heading) +{ + _gnss_heading_data.heading = heading; +} + +void GnssHeading::setHeadingAccuracy(const float heading_accuracy) +{ + _gnss_heading_data.heading_accuracy = heading_accuracy; +} + +gnss_heading_message GnssHeading::getDefaultGnssHeadingData() +{ + gnss_heading_message gps_yaw{}; + gps_yaw.time_usec = 0; + gps_yaw.heading = NAN; + gps_yaw.heading_accuracy = 0.1f; + return gps_yaw; +} + +} // namespace sensor +} // namespace sensor_simulator diff --git a/src/modules/ekf2/test/sensor_simulator/gnss_heading.h b/src/modules/ekf2/test/sensor_simulator/gnss_heading.h new file mode 100644 index 000000000000..d6a3253b8ef9 --- /dev/null +++ b/src/modules/ekf2/test/sensor_simulator/gnss_heading.h @@ -0,0 +1,31 @@ +#ifndef EKF_GNSS_HEADING_H +#define EKF_GNSS_HEADING_H + +#include "sensor.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +class GnssHeading: public Sensor +{ +public: + GnssHeading(std::shared_ptr ekf); + ~GnssHeading(); + + void setData(const gnss_heading_message &gps_yaw); + void setHeading(const float heading); + void setHeadingAccuracy(const float heading_accuracy); + + gnss_heading_message getDefaultGnssHeadingData(); + +private: + void send(uint64_t time) override; + + gnss_heading_message _gnss_heading_data{}; +}; + +} // namespace sensor +} // namespace sensor_simulator +#endif // EKF_GNSS_HEADING_H diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp index a489a79831dc..cb1fd4a14fe7 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp @@ -1,11 +1,12 @@ #include "sensor_simulator.h" - +#include SensorSimulator::SensorSimulator(std::shared_ptr ekf): _airspeed(ekf), _baro(ekf), _flow(ekf), _gps(ekf), + _gps_yaw(ekf), _imu(ekf), _mag(ekf), _rng(ekf), @@ -59,6 +60,9 @@ void SensorSimulator::loadSensorDataFromFile(std::string file_name) } else if (!sensor_type.compare("gps")) { sensor_sample.sensor_type = sensor_info::GPS; + } else if (!sensor_type.compare("gps_yaw")) { + sensor_sample.sensor_type = sensor_info::GPS_YAW; + } else if (!sensor_type.compare("airspeed")) { sensor_sample.sensor_type = sensor_info::AIRSPEED; @@ -113,6 +117,7 @@ void SensorSimulator::setSensorRateToDefault() _mag.setRateHz(80); _baro.setRateHz(80); _gps.setRateHz(5); + _gps_yaw.setRateHz(5); _flow.setRateHz(50); _rng.setRateHz(30); _vio.setRateHz(30); @@ -125,6 +130,7 @@ void SensorSimulator::setSensorDataToDefault() _baro.setData(122.2f); _flow.setData(_flow.dataAtRest()); _gps.setData(_gps.getDefaultGpsData()); + _gps_yaw.setData(_gps_yaw.getDefaultGnssHeadingData()); _imu.setData(Vector3f{0.0f, 0.0f, -CONSTANTS_ONE_G}, Vector3f{0.0f, 0.0f, 0.0f}); _mag.setData(Vector3f{0.2f, 0.0f, 0.4f}); _rng.setData(0.2f, 100); @@ -169,6 +175,7 @@ void SensorSimulator::updateSensors() _mag.update(_time); _baro.update(_time); _gps.update(_time); + _gps_yaw.update(_time); _flow.update(_time); _rng.update(_time); _vio.update(_time); @@ -258,6 +265,9 @@ void SensorSimulator::setSingleReplaySample(const sensor_info &sample) (float) sample.sensor_data[4], (float) sample.sensor_data[5])); + } else if (sample.sensor_type == sensor_info::GPS_YAW) { + _gps_yaw.setHeading((float) sample.sensor_data[0]); + } else if (sample.sensor_type == sensor_info::AIRSPEED) { _airspeed.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]); @@ -385,6 +395,12 @@ void SensorSimulator::setSensorDataFromTrajectory() /* _gps.setLongitude(); */ _gps.setVelocity(vel_world); } + + if (_gps_yaw.isRunning()) { + // Heading derived from body X-axis projected in world (NED) frame + const float heading = std::atan2(_R_body_to_world(1, 0), _R_body_to_world(0, 0)); + _gps_yaw.setHeading(heading); + } } void SensorSimulator::setGpsLatitude(const double latitude) diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h index b80901ff9f3a..c628be6b6a99 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h @@ -55,6 +55,7 @@ #include "mag.h" #include "baro.h" #include "gps.h" +#include "gnss_heading.h" #include "flow.h" #include "range_finder.h" #include "vio.h" @@ -65,7 +66,7 @@ using namespace sensor_simulator::sensor; struct sensor_info { uint64_t timestamp{}; - enum measurement_t {IMU, MAG, BARO, GPS, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU; + enum measurement_t {IMU, MAG, BARO, GPS, GPS_YAW, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU; std::array sensor_data{}; }; @@ -94,6 +95,9 @@ class SensorSimulator void startGps() { _gps.start(); } void stopGps() { _gps.stop(); } + void startGnssHeading() { _gps_yaw.start(); } + void stopGnssHeading() { _gps_yaw.stop(); } + void startFlow() { _flow.start(); } void stopFlow() { _flow.stop(); } @@ -122,6 +126,7 @@ class SensorSimulator Baro _baro; Flow _flow; Gps _gps; + GnssHeading _gps_yaw; Imu _imu; Mag _mag; RangeFinder _rng; diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gnss_heading.cpp similarity index 88% rename from src/modules/ekf2/test/test_EKF_gps_yaw.cpp rename to src/modules/ekf2/test/test_EKF_gnss_heading.cpp index de74025d162e..6d79e1103c6e 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gnss_heading.cpp @@ -55,7 +55,7 @@ class EkfGpsHeadingTest : public ::testing::Test SensorSimulator _sensor_simulator; EkfWrapper _ekf_wrapper; - void runConvergenceScenario(float yaw_offset_rad = 0.f, float antenna_offset_rad = 0.f); + void runConvergenceScenario(float yaw_offset_rad = 0.f); void checkConvergence(float truth, float tolerance = FLT_EPSILON); // Setup the Ekf with synthetic measurements @@ -69,26 +69,27 @@ class EkfGpsHeadingTest : public ::testing::Test _sensor_simulator.runSeconds(_init_duration_s); _sensor_simulator._gps.setYaw(NAN); + _sensor_simulator._gps_yaw.setHeading(NAN); _sensor_simulator.runSeconds(2); _ekf_wrapper.enableGpsFusion(); - _ekf_wrapper.enableGpsHeadingFusion(); + _ekf_wrapper.enableGnssHeadingFusion(); _sensor_simulator.startGps(); + _sensor_simulator.startGnssHeading(); _sensor_simulator.runSeconds(11); } const uint32_t _init_duration_s{4}; }; -void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad) +void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad) { // GIVEN: an initial GPS yaw, not aligned with the current one float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad); - _sensor_simulator._gps.setYaw(gps_heading); - _sensor_simulator._gps.setYawOffset(antenna_offset_rad); + _sensor_simulator._gps_yaw.setHeading(gps_heading); // WHEN: the GPS yaw fusion is activated - _ekf_wrapper.enableGpsHeadingFusion(); + _ekf_wrapper.enableGnssHeadingFusion(); _sensor_simulator.runSeconds(5); // THEN: the estimate is reset and stays close to the measurement @@ -108,8 +109,8 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset) // WHEN: enabling GPS heading fusion and heading difference is bigger than 15 degrees const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(20.f); - _sensor_simulator._gps.setYaw(gps_heading); - _ekf_wrapper.enableGpsHeadingFusion(); + _sensor_simulator._gps_yaw.setHeading(gps_heading); + _ekf_wrapper.enableGnssHeadingFusion(); const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); _sensor_simulator.runSeconds(0.4); @@ -121,7 +122,7 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset) EXPECT_NEAR(_ekf_wrapper.getYawAngle(), gps_heading, 0.001); // WHEN: GPS heading is disabled - _sensor_simulator._gps.stop(); + _sensor_simulator._gps_yaw.stop(); _sensor_simulator.runSeconds(11); // THEN: after a while the fusion should be stopped @@ -134,10 +135,10 @@ TEST_F(EkfGpsHeadingTest, yawConvergence) const float initial_yaw = math::radians(10.f); float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + initial_yaw); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); // WHEN: the GPS yaw fusion is activated - _ekf_wrapper.enableGpsHeadingFusion(); + _ekf_wrapper.enableGnssHeadingFusion(); _sensor_simulator.runSeconds(5); // THEN: the estimate is reset and stays close to the measurement @@ -145,7 +146,7 @@ TEST_F(EkfGpsHeadingTest, yawConvergence) // AND WHEN: the the measurement changes gps_heading += math::radians(2.f); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(20); // THEN: the estimate slowly converges to the new measurement @@ -161,29 +162,25 @@ TEST_F(EkfGpsHeadingTest, yaw0) TEST_F(EkfGpsHeadingTest, yaw60) { const float yaw_offset_rad = math::radians(60.f); - const float antenna_offset_rad = math::radians(80.f); - runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); + runConvergenceScenario(yaw_offset_rad); } TEST_F(EkfGpsHeadingTest, yaw180) { const float yaw_offset_rad = math::radians(180.f); - const float antenna_offset_rad = math::radians(-20.f); - runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); + runConvergenceScenario(yaw_offset_rad); } TEST_F(EkfGpsHeadingTest, yawMinus120) { const float yaw_offset_rad = math::radians(120.f); - const float antenna_offset_rad = math::radians(-42.f); - runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); + runConvergenceScenario(yaw_offset_rad); } TEST_F(EkfGpsHeadingTest, yawMinus30) { const float yaw_offset_rad = math::radians(-30.f); - const float antenna_offset_rad = math::radians(10.f); - runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); + runConvergenceScenario(yaw_offset_rad); } TEST_F(EkfGpsHeadingTest, fallBackToMag) @@ -193,7 +190,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag) // to the filter _sensor_simulator.runSeconds(6); float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(10.f)); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); // WHEN: the GPS yaw fusion is activated _sensor_simulator.runSeconds(1); @@ -208,7 +205,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag) // BUT WHEN: the GPS yaw is suddenly invalid gps_heading = NAN; - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(7.5); // THEN: after a few seconds, the fusion should stop and @@ -227,7 +224,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator) float gps_heading = math::radians(90.f); const float true_heading = math::radians(-20.f); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(10); const Vector3f accel_frd{-1.0, -1.5f, 0.f}; @@ -265,14 +262,14 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround) { // GIVEN: the GPS yaw fusion activated float gps_heading = _ekf_wrapper.getYawAngle(); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(1); _ekf->set_in_air_status(false); // WHEN: the measurement suddenly changes const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f)); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(8); // THEN: the fusion should reset @@ -285,14 +282,14 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) { // GIVEN: the GPS yaw fusion activated float gps_heading = _ekf_wrapper.getYawAngle(); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(5); _ekf->set_in_air_status(true); // WHEN: the measurement suddenly changes const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f)); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(7.5); // THEN: the fusion should reset @@ -300,7 +297,7 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) // BUT WHEN: the measurement jumps a 2nd time gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f)); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(7.5); // THEN: after a few seconds, the fusion should stop and @@ -314,12 +311,12 @@ TEST_F(EkfGpsHeadingTest, stopOnGround) // GIVEN: the GPS yaw fusion activated and there is no mag data _sensor_simulator._mag.stop(); float gps_heading = _ekf_wrapper.getYawAngle(); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(5); // WHEN: the measurement stops gps_heading = NAN; - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps_yaw.setHeading(gps_heading); _sensor_simulator.runSeconds(7.5); // THEN: the fusion should stop and the GPS pos/vel aiding