Reverse the linked list of data_validator_group and maintain a first node

for search from index 0

Fixes issue PX4/Firmware#8644

validation/data_validator.cpp

@@ -42,7 +42,7 @@
 #include "data_validator.h"
 #include <ecl/ecl.h>
 
-DataValidator::DataValidator(DataValidator *prev_sibling) :
+DataValidator::DataValidator() :
 	_error_mask(ERROR_FLAG_NO_ERROR),
 	_timeout_interval(20000),
 	_time_last(0),
@@ -58,7 +58,7 @@ DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_vibe{0.0f},
 	_value_equal_count(0),
 	_value_equal_count_threshold(VALUE_EQUAL_COUNT_DEFAULT),
-	_sibling(prev_sibling)
+	_sibling(nullptr)
 {
 
 }

validation/data_validator.h

@@ -48,7 +48,7 @@ class __EXPORT DataValidator {
 public:
 	static const unsigned dimensions = 3;
 
-	DataValidator(DataValidator *prev_sibling = nullptr);
+	DataValidator();
 	virtual ~DataValidator() = default;
 
 	/**
@@ -72,6 +72,12 @@ public:
 	 */
 	DataValidator*		sibling() { return _sibling; }
 
+	/**
+	 * Set the sibling to the next node in the group
+	 *
+	 */
+	void			sibling(DataValidator* sibling) { _sibling = sibling; }
+
 	/**
 	 * Get the confidence of this validator
 	 * @return		the confidence between 0 and 1

validation/data_validator_group.cpp

@@ -45,19 +45,31 @@
 
 DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
 	_first(nullptr),
+	_last(nullptr),
 	_curr_best(-1),
 	_prev_best(-1),
 	_first_failover_time(0),
 	_toggle_count(0)
 {
-	DataValidator *next = _first;
+	DataValidator *next = nullptr;
+	DataValidator *prev = nullptr;
 
 	for (unsigned i = 0; i < siblings; i++) {
-		next = new DataValidator(next);
+		next = new DataValidator();
+		if(i == 0) {
+			_first = next;
+		}
+		else {
+			prev->sibling(next);
+		}
+		prev = next;
 	}
 
-	_first = next;
+	_last = next;
-	_timeout_interval_us = _first->get_timeout();
+
+	if(_first) {
+		_timeout_interval_us = _first->get_timeout();
+	}
 }
 
 DataValidatorGroup::~DataValidatorGroup()
@@ -71,13 +83,14 @@ DataValidatorGroup::~DataValidatorGroup()
 
 DataValidator *DataValidatorGroup::add_new_validator()
 {
-	DataValidator *validator = new DataValidator(_first);
+	DataValidator *validator = new DataValidator();
 	if (!validator) {
 		return nullptr;
 	}
-	_first = validator;
+	_last->sibling(validator);
-	_first->set_timeout(_timeout_interval_us);
+	_last = validator;
-	return _first;
+	_last->set_timeout(_timeout_interval_us);
+	return _last;
 }
 
 void

validation/data_validator_group.h

@@ -133,7 +133,8 @@ public:
 
 
 private:
-	DataValidator *_first;		/**< sibling in the group */
+	DataValidator *_first;		/**< first node in the group */
+	DataValidator *_last;		/**< last node in the group */
 	uint32_t _timeout_interval_us; /**< currently set timeout */
 	int _curr_best;		/**< currently best index */
 	int _prev_best;		/**< the previous best index */