From 481c624975a23f8ab189c3770001abf53c1d7ebb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 10 May 2016 10:21:45 +1000 Subject: [PATCH 1/2] EKF: Remove use of vehicle arm status Use single externally set in-air status for all decisions --- EKF/control.cpp | 61 +++++++++---------------------------- EKF/ekf.cpp | 2 +- EKF/ekf.h | 3 -- EKF/estimator_interface.cpp | 2 -- EKF/estimator_interface.h | 7 +---- EKF/gps_checks.cpp | 6 ++-- EKF/terrain_estimator.cpp | 2 +- 7 files changed, 21 insertions(+), 62 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index e5a1e5aa8f..2830d696dc 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -45,8 +45,6 @@ void Ekf::controlFusionModes() { // Store the status to enable change detection _control_status_prev.value = _control_status.value; - // Determine the vehicle status - calculateVehicleStatus(); // Get the magnetic declination calcMagDeclination(); @@ -109,7 +107,7 @@ void Ekf::controlFusionModes() // reset the horizontal velocity variance using the optical flow noise variance P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); - if (!_in_air) { + if (!_control_status.flags.in_air) { // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; @@ -370,29 +368,23 @@ void Ekf::controlFusionModes() // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances // or the more accurate 3-axis fusion if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { - if (!_control_status.flags.armed) { - // use heading fusion for initial startup - _control_status.flags.mag_hdg = true; - _control_status.flags.mag_3D = false; - - } else { - if (_control_status.flags.in_air) { - // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states - if (!_control_status.flags.mag_3D) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - } - - // use 3D mag fusion when airborne - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_3D = true; - } else { - // use heading fusion when on the ground - _control_status.flags.mag_hdg = true; - _control_status.flags.mag_3D = false; - } + if (_control_status.flags.in_air) { + // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states + if (!_control_status.flags.mag_3D) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } + // use 3D mag fusion when airborne + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = true; + + } else { + // use heading fusion when on the ground + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + } + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { // always use heading fusion _control_status.flags.mag_hdg = true; @@ -448,26 +440,3 @@ void Ekf::controlFusionModes() } } - -void Ekf::calculateVehicleStatus() -{ - // determine if the vehicle is armed - _control_status.flags.armed = _vehicle_armed; - - // record vertical position whilst disarmed to use as a height change reference - if (!_control_status.flags.armed) { - _last_disarmed_posD = _state.pos(2); - } - - // Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming - bool in_air = _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f; - - if (!_control_status.flags.in_air && in_air) { - _control_status.flags.in_air = true; - } - - // Transition to on-ground occurs when disarmed or if the land detector indicated landed state - if (_control_status.flags.in_air && (!_control_status.flags.armed || !_in_air)) { - _control_status.flags.in_air = false; - } -} diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index db1356f470..e660014498 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -253,7 +253,7 @@ bool Ekf::update() } else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) { // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements // and are on the ground, then synthesise a measurement at the expected on ground value - if (!_in_air) { + if (!_control_status.flags.in_air) { _range_sample_delayed.rng = _params.rng_gnd_clearance; _range_sample_delayed.time_us = _imu_sample_delayed.time_us; diff --git a/EKF/ekf.h b/EKF/ekf.h index c22df787ad..7c12ac7475 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -332,9 +332,6 @@ private: // Control the filter fusion modes void controlFusionModes(); - // Determine if we are airborne or motors are armed - void calculateVehicleStatus(); - // return the square of two floating point numbers - used in auto coded sections inline float sq(float var) { diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 162cfaba21..65bfa295c7 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -52,8 +52,6 @@ EstimatorInterface::EstimatorInterface(): _imu_ticks(0), _imu_updated(false), _initialised(false), - _vehicle_armed(false), - _in_air(false), _NED_origin_initialised(false), _gps_speed_valid(false), _gps_origin_eph(0.0f), diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index a8e537a8cd..bd2ecf40a0 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -149,11 +149,8 @@ public: // in order to give access to the application parameters *getParamHandle() {return &_params;} - // set vehicle arm status data - void set_arm_status(bool data) { _vehicle_armed = data; } - // set vehicle landed status data - void set_in_air_status(bool in_air) {_in_air = in_air;} + void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;} // return true if the global position estimate is valid virtual bool global_position_is_valid() = 0; @@ -258,8 +255,6 @@ protected: bool _imu_updated; // true if the ekf should update (completed downsampling process) bool _initialised; // true if the ekf interface instance (data buffering) is initialized - bool _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground - bool _in_air; // we assume vehicle is in the air, set by the given landing detector bool _NED_origin_initialised = false; bool _gps_speed_valid = false; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 45f903dfec..184b3c4edf 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -162,7 +162,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) // Calculate the horizontal drift speed and fail if too high // This check can only be used if the vehicle is stationary during alignment - if (!_control_status.flags.armed) { + if (!_control_status.flags.in_air) { float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift); @@ -186,7 +186,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) // Fail if the vertical drift speed is too high // This check can only be used if the vehicle is stationary during alignment - if (!_control_status.flags.armed) { + if (!_control_status.flags.in_air) { _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift); } else { @@ -195,7 +195,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) // Check the magnitude of the filtered horizontal GPS velocity // This check can only be used if the vehicle is stationary during alignment - if (!_control_status.flags.armed) { + if (!_control_status.flags.in_air) { vel_limit = 10.0f * _params.req_hdrift; float gps_velN = fminf(fmaxf(gps->vel_ned[0], -vel_limit), vel_limit); float gps_velE = fminf(fmaxf(gps->vel_ned[1], -vel_limit), vel_limit); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 6c3fa82f5b..f812ad2110 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -55,7 +55,7 @@ bool Ekf::initHagl() // success return true; - } else if (!_in_air) { + } else if (!_control_status.flags.in_air) { // if on ground we assume a ground clearance _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; // Use the ground clearance value as our uncertainty From 1a24ec5f507eb48b25966633ba20f88fb93a8ff7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 10 May 2016 10:45:06 +1000 Subject: [PATCH 2/2] EKF: remove unused flag from control status message --- EKF/common.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 3b8df55ff5..25e6f2374f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -371,11 +371,10 @@ union filter_control_status_u { uint16_t mag_3D : 1; // 5 - true if 3-axis magnetometer measurement are being fused uint16_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused uint16_t in_air : 1; // 7 - true when the vehicle is airborne - uint16_t armed : 1; // 8 - true when the vehicle motors are armed - uint16_t wind : 1; // 9 - true when wind velocity is being estimated - uint16_t baro_hgt : 1; // 10 - true when baro height is being fused as a primary height reference - uint16_t rng_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference - uint16_t gps_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference + uint16_t wind : 1; // 8 - true when wind velocity is being estimated + uint16_t baro_hgt : 1; // 9 - true when baro height is being fused as a primary height reference + uint16_t rng_hgt : 1; // 10 - true when range finder height is being fused as a primary height reference + uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference } flags; uint16_t value; };