Revert "EKF: Release flow speed limit with altitude gained" (#382)

* Revert "EKF: Release flow speed limit with altitude gained"

This reverts commit e70206f74b.

* Revert "fix code style"

This reverts commit 76bf70121c.

* Revert "Reverse the linked list of data_validator_group and maintain a first node"

This reverts commit 32482e7644.

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)
 

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_limit_hagl = true;
+		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);
 
-	} 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;
+		flow_limit_hagl = true;
 
 	} 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));
 
 }

validation/data_validator.cpp

@@ -42,7 +42,7 @@
 #include "data_validator.h"
 #include <ecl/ecl.h>
 
-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)
 {
 
 }

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

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

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 */