From 87f74b7ea61d001b46a6db268acab116408ffa2d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 14 Aug 2021 12:53:46 -0400 Subject: [PATCH] WIP: move GPS status to sensors --- .ci/Jenkinsfile-hardware | 1 + msg/CMakeLists.txt | 1 + msg/sensors_status_gps.msg | 28 ++++ src/modules/logger/logged_topics.cpp | 1 + .../VehicleGPSPosition.cpp | 132 +++++++++++++++++- .../VehicleGPSPosition.hpp | 44 +++++- .../sensors/vehicle_gps_position/params.c | 107 ++++++++++++++ 7 files changed, 310 insertions(+), 4 deletions(-) create mode 100644 msg/sensors_status_gps.msg diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index f336504548..b58882dc8e 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -943,6 +943,7 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_gps"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info"' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6904bfce54..25fc85ae15 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -140,6 +140,7 @@ set(msg_files sensor_preflight_mag.msg sensor_selection.msg sensors_status_imu.msg + sensors_status_gps.msg system_power.msg takeoff_status.msg task_stack_info.msg diff --git a/msg/sensors_status_gps.msg b/msg/sensors_status_gps.msg new file mode 100644 index 0000000000..de9be9164c --- /dev/null +++ b/msg/sensors_status_gps.msg @@ -0,0 +1,28 @@ +# +# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated. +# +uint64 timestamp # time since system start (microseconds) + +uint32[2] device_ids +float32[2] inconsistency +bool[2] healthy +uint8[2] priority + + +# drift metrics +float32[2] drift_rate_hpos # Horizontal position rate magnitude checked using EKF2_REQ_HDRIFT (m/s) +float32[2] drift_rate_vpos # Vertical position rate magnitude checked using EKF2_REQ_VDRIFT (m/s) +float32[2] drift_hspd # Filtered horizontal velocity magnitude checked using EKF2_REQ_HDRIFT (m/s) + +float32[2] error_norm # normalised gps error + +bool[2] fail_fix # true if the fix type is insufficient (no 3D solution) +bool[2] fail_nsats # true if number of satellites used is insufficient +bool[2] fail_pdop # true if position dilution of precision is insufficient +bool[2] fail_hacc # true if reported horizontal accuracy is insufficient +bool[2] fail_vacc # true if reported vertical accuracy is insufficient +bool[2] fail_sacc # true if reported speed accuracy is insufficient +bool[2] fail_hdrift # true if horizontal drift is excessive (can only be used when stationary on ground) +bool[2] fail_vdrift # true if vertical drift is excessive (can only be used when stationary on ground) +bool[2] fail_hspeed # true if horizontal speed is excessive (can only be used when stationary on ground) +bool[2] fail_vspeed # true if vertical speed error is excessive diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e0ce45bff3..da9747b466 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -86,6 +86,7 @@ void LoggedTopics::add_default_topics() add_topic("sensor_correction"); add_topic("sensor_gyro_fft", 50); add_topic("sensor_selection"); + add_topic("sensors_status_gps", 500); add_topic("sensors_status_imu", 200); add_topic("system_power", 500); add_topic("takeoff_status", 1000); diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 9f65125ef4..22ebc13eb0 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -98,6 +98,14 @@ void VehicleGPSPosition::ParametersUpdate(bool force) } } +void VehicleGPSPosition::resetGpsDriftCheckFilters() +{ + for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { + _gps_velNE_filt[i].setZero(); + _gps_pos_deriv_filt[i].setZero(); + } +} + void VehicleGPSPosition::Run() { perf_begin(_cycle_perf); @@ -113,21 +121,134 @@ void VehicleGPSPosition::Run() for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { gps_updated = _sensor_gps_sub[i].updated(); - sensor_gps_s gps_data; + sensor_gps_s gps; if (gps_updated) { any_gps_updated = true; - _sensor_gps_sub[i].copy(&gps_data); - _gps_blending.setGpsData(gps_data, i); + _sensor_gps_sub[i].copy(&gps); + _gps_blending.setGpsData(gps, i); if (!_sensor_gps_sub[i].registered()) { _sensor_gps_sub[i].registerCallback(); } + + // update drift metrics + if (gps.timestamp > _timestamp_prev[i]) { + + // check if GPS quality is degraded + _sensors_status_gps.error_norm[i] = fmaxf((gps.eph / _param_sens_gps_rq_eph.get()), + (gps.epv / _param_sens_gps_rq_epv.get())); + _sensors_status_gps.error_norm[i] = fmaxf(_sensors_status_gps.error_norm[i], + (gps.s_variance_m_s / _param_sens_gps_rq_sacc.get())); + + _sensors_status_gps.device_ids[i] = gps.device_id; + + // Check the fix type + _sensors_status_gps.fail_fix[i] = (gps.fix_type < 3); + + // Check the number of satellites + _sensors_status_gps.fail_nsats[i] = (gps.satellites_used < _param_sens_gps_rq_nsat.get()); + + // Check the position dilution of precision + float pdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop); + _sensors_status_gps.fail_pdop[i] = (pdop > _param_sens_gps_rq_pdop.get()); + + // Check the reported horizontal and vertical position accuracy + _sensors_status_gps.fail_hacc[i] = (gps.eph > _param_sens_gps_rq_eph.get()); + _sensors_status_gps.fail_vacc[i] = (gps.epv > _param_sens_gps_rq_epv.get()); + + // Check the reported speed accuracy + _sensors_status_gps.fail_sacc[i] = (gps.s_variance_m_s > _param_sens_gps_rq_sacc.get()); + + + if (_landed && _at_rest) { + + // The following checks are only valid when the vehicle is at rest + const double lat = gps.lat * 1.0e-7; + const double lon = gps.lon * 1.0e-7; + + // Calculate position movement since last measurement + float delta_pos_n = 0.0f; + float delta_pos_e = 0.0f; + + // calculate position movement since last GPS fix + if (_gps_pos_prev[i].timestamp > 0) { + map_projection_project(&_gps_pos_prev[i], lat, lon, &delta_pos_n, &delta_pos_e); + + } else { + // no previous position has been set + map_projection_init_timestamped(&_gps_pos_prev[i], lat, lon, gps.timestamp); + _gps_alt_prev[i] = 1e-3f * (float)gps.alt; + } + + if (_timestamp_prev[i] != 0) { + static constexpr float filt_time_const = 10.0f; + const float dt = math::constrain((gps.timestamp - _timestamp_prev[i]) * 1e-6f, 0.001f, filt_time_const); + const float filter_coef = dt / filt_time_const; + + // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold + const matrix::Vector3f vel_limit{_param_sens_gps_rq_hdrf.get(), _param_sens_gps_rq_hdrf.get(), _param_sens_gps_rq_vdrf.get()}; + matrix::Vector3f pos_derived{delta_pos_n, delta_pos_e, (_gps_alt_prev[i] - 1e-3f * (float)gps.alt)}; + pos_derived = matrix::constrain(pos_derived / dt, -10.f * vel_limit, 10.f * vel_limit); + + // Apply a low pass filter + _gps_pos_deriv_filt[i] = pos_derived * filter_coef + _gps_pos_deriv_filt[i] * (1.f - filter_coef); + + // Calculate the horizontal drift speed and fail if too high + _sensors_status_gps.drift_rate_hpos[i] = Vector2f(_gps_pos_deriv_filt[i].xy()).norm(); + _sensors_status_gps.fail_hdrift[i] = (_gps_drift_metrics[i][0] > _param_sens_gps_rq_hdrf.get()); + + // Fail if the vertical drift speed is too high + _sensors_status_gps.drift_rate_vpos[i] = fabsf(_gps_pos_deriv_filt[i](2)); + _sensors_status_gps.fail_vdrift[i] = (_gps_drift_metrics[i][1] > _param_sens_gps_rq_vdrf.get()); + + // Check the magnitude of the filtered horizontal GPS velocity + + // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient + const Vector2f gps_velNE = matrix::constrain(Vector2f{gps.vel_n_m_s, gps.vel_e_m_s}, + -10.f * _param_sens_gps_rq_hdrf.get(), 10.f * _param_sens_gps_rq_hdrf.get()); + _gps_velNE_filt[i] = gps_velNE * filter_coef + _gps_velNE_filt[i] * (1.f - filter_coef); + _sensors_status_gps.drift_hspd[i] = _gps_velNE_filt[i].norm(); + _sensors_status_gps.fail_hspeed[i] = (_gps_drift_metrics[i][2] > _param_sens_gps_rq_hdrf.get()); + } + + } else if (!_landed) { + // These checks are always declared as passed when flying + // If on ground and moving, the last result before movement commenced is kept + // _gps_check_fail_status.flags.hdrift = false; + // _gps_check_fail_status.flags.vdrift = false; + // _gps_check_fail_status.flags.hspeed = false; + // _gps_drift_updated = false; + + resetGpsDriftCheckFilters(); + + } else { + // This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation + //_gps_drift_updated = true; + + resetGpsDriftCheckFilters(); + } + + _sensors_status_gps.healthy[i] = (_sensors_status_gps.fail_fix[i] && (_param_sens_gps_checks.get() & MASK_GPS_NSATS)) + && (_sensors_status_gps.fail_fix[i] && (_param_sens_gps_checks.get() & MASK_GPS_PDOP)) + && (_sensors_status_gps.fail_pdop[i] && (_param_sens_gps_checks.get() & MASK_GPS_HACC)) + && (_sensors_status_gps.fail_vacc[i] && (_param_sens_gps_checks.get() & MASK_GPS_VACC)) + && (_sensors_status_gps.fail_sacc[i] && (_param_sens_gps_checks.get() & MASK_GPS_SACC)) + && (_sensors_status_gps.fail_hdrift[i] && (_param_sens_gps_checks.get() & MASK_GPS_HDRIFT)) + && (_sensors_status_gps.fail_vdrift[i] && (_param_sens_gps_checks.get() & MASK_GPS_VDRIFT)) + && (_sensors_status_gps.fail_hspeed[i] && (_param_sens_gps_checks.get() & MASK_GPS_HSPD)) + && (_sensors_status_gps.fail_vspeed[i] && (_param_sens_gps_checks.get() & MASK_GPS_VSPD)); + } + + _timestamp_prev[i] = gps.timestamp; } } if (any_gps_updated) { + _sensors_status_gps.timestamp = hrt_absolute_time(); + _sensors_status_gps_pub.publish(_sensors_status_gps); + _gps_blending.update(hrt_absolute_time()); if (_gps_blending.isNewOutputDataAvailable()) { @@ -177,6 +298,11 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected) void VehicleGPSPosition::PrintStatus() { //PX4_INFO("selected GPS: %d", _gps_select_index); + for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_timestamp_prev[i] != 0) { + PX4_INFO_RAW("GPS %d healthy: %d", i, _sensors_status_gps.healthy[i]); + } + } } }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 8126fd5efd..4dd7e04cd9 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include "gps_blending.hpp" @@ -71,6 +72,8 @@ private: void ParametersUpdate(bool force = false); void Publish(const sensor_gps_s &gps, uint8_t selected); + void resetGpsDriftCheckFilters(); + // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm static constexpr uint8_t BLEND_MASK_USE_SPD_ACC = 1; static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2; @@ -81,6 +84,7 @@ private: static_assert(GPS_MAX_RECEIVERS == GpsBlending::GPS_MAX_RECEIVERS_BLEND, "GPS_MAX_RECEIVERS must match to GPS_MAX_RECEIVERS_BLEND"); + uORB::Publication _sensors_status_gps_pub{ORB_ID(sensors_status_gps)}; uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -90,6 +94,34 @@ private: {this, ORB_ID(sensor_gps), 1}, }; + bool _landed{true}; + bool _at_rest{true}; + + sensors_status_gps_s _sensors_status_gps{}; + + // variables used for the GPS quality checks + hrt_abstime _timestamp_prev[GPS_MAX_RECEIVERS] {}; + map_projection_reference_s _gps_pos_prev[GPS_MAX_RECEIVERS] {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message + float _gps_alt_prev[GPS_MAX_RECEIVERS] {}; // height from the previous GPS message (m) + matrix::Vector3f _gps_pos_deriv_filt[GPS_MAX_RECEIVERS] {}; ///< GPS NED position derivative (m/sec) + matrix::Vector2f _gps_velNE_filt[GPS_MAX_RECEIVERS] {}; ///< filtered GPS North and East velocity (m/sec) + + float _gps_drift_metrics[GPS_MAX_RECEIVERS][3] {}; // Array containing GPS drift metrics + // [0] Horizontal position drift rate (m/s) + // [1] Vertical position drift rate (m/s) + // [2] Filtered horizontal velocity (m/s) + + // GPS pre-flight check bit locations + static constexpr uint32_t MASK_GPS_NSATS = (1 << 0); + static constexpr uint32_t MASK_GPS_PDOP = (1 << 1); + static constexpr uint32_t MASK_GPS_HACC = (1 << 2); + static constexpr uint32_t MASK_GPS_VACC = (1 << 3); + static constexpr uint32_t MASK_GPS_SACC = (1 << 4); + static constexpr uint32_t MASK_GPS_HDRIFT = (1 << 5); + static constexpr uint32_t MASK_GPS_VDRIFT = (1 << 6); + static constexpr uint32_t MASK_GPS_HSPD = (1 << 7); + static constexpr uint32_t MASK_GPS_VSPD = (1 << 8); + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; GpsBlending _gps_blending; @@ -97,7 +129,17 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_sens_gps_mask, (ParamFloat) _param_sens_gps_tau, - (ParamInt) _param_sens_gps_prime + (ParamInt) _param_sens_gps_prime, + + (ParamFloat) _param_sens_gps_rq_eph, + (ParamFloat) _param_sens_gps_rq_epv, + (ParamFloat) _param_sens_gps_rq_sacc, + (ParamInt) _param_sens_gps_rq_nsat, + (ParamFloat) _param_sens_gps_rq_pdop, + (ParamFloat) _param_sens_gps_rq_hdrf, + (ParamFloat) _param_sens_gps_rq_vdrf, + + (ParamInt) _param_sens_gps_checks ) }; }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/params.c b/src/modules/sensors/vehicle_gps_position/params.c index 0136ad52d8..7998cd42c5 100644 --- a/src/modules/sensors/vehicle_gps_position/params.c +++ b/src/modules/sensors/vehicle_gps_position/params.c @@ -80,3 +80,110 @@ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f); * @max 1 */ PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0); + + + + + +/** + * Required EPH to use GPS. + * + * @group Sensors + * @min 2 + * @max 100 + * @unit m + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(SENS_GPS_RQ_EPH, 3.0f); + +/** + * Required EPV to use GPS. + * + * @group Sensors + * @min 2 + * @max 100 + * @unit m + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(SENS_GPS_RQ_EPV, 5.0f); + +/** + * Required speed accuracy to use GPS. + * + * @group Sensors + * @min 0.5 + * @max 5.0 + * @unit m/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(SENS_GPS_RQ_SACC, 0.5f); + +/** + * Required satellite count to use GPS. + * + * @group Sensors + * @min 4 + * @max 12 + */ +PARAM_DEFINE_INT32(SENS_GPS_RQ_NSAT, 6); + +/** + * Maximum PDOP to use GPS. + * + * @group Sensors + * @min 1.5 + * @max 5.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(SENS_GPS_RQ_PDOP, 2.5f); + +/** + * Maximum horizontal drift speed to use GPS. + * + * @group Sensors + * @min 0.1 + * @max 1.0 + * @unit m/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(SENS_GPS_RQ_HDRF, 0.1f); + +/** + * Maximum vertical drift speed to use GPS. + * + * @group Sensors + * @min 0.1 + * @max 1.5 + * @decimal 2 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(SENS_GPS_RQ_VDRF, 0.2f); + +/** + * Integer bitmask controlling GPS checks. + * + * Set bits to 1 to enable checks. Checks enabled by the following bit positions + * 0 : Minimum required sat count set by SENS_GPS_RQ_NSATS + * 1 : Maximum allowed PDOP set by SENS_GPS_RQ_PDOP + * 2 : Maximum allowed horizontal position error set by SENS_GPS_RQ_EPH + * 3 : Maximum allowed vertical position error set by SENS_GPS_RQ_EPV + * 4 : Maximum allowed speed error set by SENS_GPS_RQ_SACC + * 5 : Maximum allowed horizontal position rate set by SENS_GPS_RQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detection of the stationary condition is controlled by the EKF2_MOVE_TEST parameter. + * 6 : Maximum allowed vertical position rate set by SENS_GPS_RQ_VDRIFT. This check will only run when the vehicle is on ground and stationary. Detection of the stationary condition is controlled by the EKF2_MOVE_TEST parameter. + * 7 : Maximum allowed horizontal speed set by SENS_GPS_RQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detection of the stationary condition is controlled by the EKF2_MOVE_TEST parameter. + * 8 : Maximum allowed vertical velocity discrepancy set by SENS_GPS_RQ_VDRIFT + * + * @group Sensors + * @min 0 + * @max 511 + * @bit 0 Min sat count (SENS_GPS_RQ_NSAT) + * @bit 1 Max PDOP (SENS_GPS_RQ_PDOP) + * @bit 2 Max horizontal position error (SENS_GPS_RQ_EPH) + * @bit 3 Max vertical position error (SENS_GPS_RQ_EPV) + * @bit 4 Max speed error (SENS_GPS_RQ_SACC) + * @bit 5 Max horizontal position rate (SENS_GPS_RQ_HDRF) + * @bit 6 Max vertical position rate (SENS_GPS_RQ_VDRF) + * @bit 7 Max horizontal speed (SENS_GPS_RQ_HDRIFT) + * @bit 8 Max vertical velocity discrepancy (SENS_GPS_RQ_VDRF) + */ +PARAM_DEFINE_INT32(SENS_GPS_CHECKS, 245);