diff --git a/EKF/ekf.h b/EKF/ekf.h index 18bb0d795c..33ad1a07d9 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -348,7 +348,6 @@ private: Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) - float _flow_gnd_spd_max{0.0f}; ///< maximum ground speed that the flow sensor can reliably measure (m/s) float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e18c904a01..d619bcac93 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1039,30 +1039,25 @@ limit_hagl : Boolean true when height above ground needs to be controlled to rem */ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) { + float flow_gnd_spd_max; bool flow_limit_hagl; // If relying on optical flow for navigation we need to keep within flow and range sensor limits bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos); if (relying_on_optical_flow) { // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - _flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); - _flow_gnd_spd_max = fmaxf(_flow_gnd_spd_max , 0.0f); + flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); + flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f); + flow_limit_hagl = true; - } else if (_control_status.flags.opt_flow) { - // We are using optical flow but not reliant on it - // Release the speed limit as the vehicle climbs, but do not reduce it when it descends - float lower_limit = fmaxf(0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)) , 0.0f); - _flow_gnd_spd_max = fmaxf(_flow_gnd_spd_max , lower_limit); - flow_limit_hagl = false; - } else { - _flow_gnd_spd_max = NAN; + flow_gnd_spd_max = NAN; flow_limit_hagl = false; } - memcpy(vxy_max, &_flow_gnd_spd_max, sizeof(float)); + memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float)); memcpy(limit_hagl, &flow_limit_hagl, sizeof(bool)); } diff --git a/validation/data_validator.cpp b/validation/data_validator.cpp index 0fe4dbc8fe..f510cd9b53 100644 --- a/validation/data_validator.cpp +++ b/validation/data_validator.cpp @@ -42,7 +42,7 @@ #include "data_validator.h" #include -DataValidator::DataValidator() : +DataValidator::DataValidator(DataValidator *prev_sibling) : _error_mask(ERROR_FLAG_NO_ERROR), _timeout_interval(20000), _time_last(0), @@ -58,7 +58,7 @@ DataValidator::DataValidator() : _vibe{0.0f}, _value_equal_count(0), _value_equal_count_threshold(VALUE_EQUAL_COUNT_DEFAULT), - _sibling(nullptr) + _sibling(prev_sibling) { } diff --git a/validation/data_validator.h b/validation/data_validator.h index 84b5ecfd27..013e78851d 100644 --- a/validation/data_validator.h +++ b/validation/data_validator.h @@ -48,7 +48,7 @@ class __EXPORT DataValidator { public: static const unsigned dimensions = 3; - DataValidator(); + DataValidator(DataValidator *prev_sibling = nullptr); virtual ~DataValidator() = default; /** @@ -72,12 +72,6 @@ public: */ DataValidator* sibling() { return _sibling; } - /** - * Set the sibling to the next node in the group - * - */ - void setSibling(DataValidator* sibling) { _sibling = sibling; } - /** * Get the confidence of this validator * @return the confidence between 0 and 1 diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp index 4f6fcd0dcf..321231a486 100644 --- a/validation/data_validator_group.cpp +++ b/validation/data_validator_group.cpp @@ -45,30 +45,19 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) : _first(nullptr), - _last(nullptr), _curr_best(-1), _prev_best(-1), _first_failover_time(0), _toggle_count(0) { - DataValidator *next = nullptr; - DataValidator *prev = nullptr; + DataValidator *next = _first; for (unsigned i = 0; i < siblings; i++) { - next = new DataValidator(); - if(i == 0) { - _first = next; - } else { - prev->setSibling(next); - } - prev = next; + next = new DataValidator(next); } - _last = next; - - if(_first) { - _timeout_interval_us = _first->get_timeout(); - } + _first = next; + _timeout_interval_us = _first->get_timeout(); } DataValidatorGroup::~DataValidatorGroup() @@ -82,14 +71,13 @@ DataValidatorGroup::~DataValidatorGroup() DataValidator *DataValidatorGroup::add_new_validator() { - DataValidator *validator = new DataValidator(); + DataValidator *validator = new DataValidator(_first); if (!validator) { return nullptr; } - _last->setSibling(validator); - _last = validator; - _last->set_timeout(_timeout_interval_us); - return _last; + _first = validator; + _first->set_timeout(_timeout_interval_us); + return _first; } void diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h index 4ffebda7be..13bb427431 100644 --- a/validation/data_validator_group.h +++ b/validation/data_validator_group.h @@ -133,8 +133,7 @@ public: private: - DataValidator *_first; /**< first node in the group */ - DataValidator *_last; /**< last node in the group */ + DataValidator *_first; /**< sibling in the group */ uint32_t _timeout_interval_us; /**< currently set timeout */ int _curr_best; /**< currently best index */ int _prev_best; /**< the previous best index */