Merge branch 'master' into pr-ekfOptFlowGyro

EKF/control.cpp

@@ -635,7 +635,7 @@ void Ekf::controlGpsFusion()
 			_hvelInnovGate = fmaxf(_params.vel_innov_gate, 1.0f);
 		}
 
-	} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) {
+	} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
 		_control_status.flags.gps = false;
 		ECL_WARN("EKF GPS data stopped");
 	}

validation/data_validator.cpp

@@ -40,28 +40,8 @@
  */
 
 #include "data_validator.h"
-#include <ecl.h>
-
-DataValidator::DataValidator() :
-	_error_mask(ERROR_FLAG_NO_ERROR),
-	_timeout_interval(20000),
-	_time_last(0),
-	_event_count(0),
-	_error_count(0),
-	_error_density(0),
-	_priority(0),
-	_mean{0.0f},
-	_lp{0.0f},
-	_M2{0.0f},
-	_rms{0.0f},
-	_value{0.0f},
-	_vibe{0.0f},
-	_value_equal_count(0),
-	_value_equal_count_threshold(VALUE_EQUAL_COUNT_DEFAULT),
-	_sibling(nullptr)
-{
 
-}
+#include <ecl.h>
 
 void
 DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in)
@@ -72,7 +52,7 @@ DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int p
 }
 
 void
-DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_count_in, int priority_in)
+DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t error_count_in, int priority_in)
 {
 	_event_count++;
 

validation/data_validator.h

@@ -49,8 +49,8 @@ class DataValidator
 public:
 	static const unsigned dimensions = 3;
 
-	DataValidator();
-	virtual ~DataValidator() = default;
+	DataValidator() = default;
+	~DataValidator() = default;
 
 	/**
 	 * Put an item into the validator.
@@ -64,7 +64,7 @@ public:
 	 *
 	 * @param val		Item to put
 	 */
-	void			put(uint64_t timestamp, float val[dimensions], uint64_t error_count, int priority);
+	void			put(uint64_t timestamp, const float val[dimensions], uint64_t error_count, int priority);
 
 	/**
 	 * Get the next sibling in the group
@@ -89,7 +89,7 @@ public:
 	 * Get the error count of this validator
 	 * @return		the error count
 	 */
-	uint64_t		error_count() { return _error_count; }
+	uint64_t		error_count() const { return _error_count; }
 
 	/**
 	 * Get the values of this validator
@@ -101,19 +101,19 @@ public:
 	 * Get the used status of this validator
 	 * @return		true if this validator ever saw data
 	 */
-	bool			used() { return (_time_last > 0); }
+	bool			used() const { return (_time_last > 0); }
 
 	/**
 	 * Get the priority of this validator
 	 * @return		the stored priority
 	 */
-	int			priority() { return (_priority); }
+	int			priority() const { return _priority; }
 
 	/**
 	 * Get the error state of this validator
 	 * @return		the bitmask with the error status
 	 */
-	uint32_t		state() { return _error_mask; }
+	uint32_t		state() const { return _error_mask; }
 
 	/**
 	 * Reset the error state of this validator
@@ -157,7 +157,7 @@ public:
 	 *
 	 * @return The timeout interval in microseconds
 	 */
-	uint32_t			get_timeout() const { return _timeout_interval; }
+	uint32_t		get_timeout() const { return _timeout_interval; }
 
 	/**
 	 * Data validator error states
@@ -170,24 +170,29 @@ public:
 	static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY 	= (0x00000001U << 4);
 
 private:
-	uint32_t _error_mask;			/**< sensor error state */
-	uint32_t _timeout_interval;		/**< interval in which the datastream times out in us */
-	uint64_t _time_last;			/**< last timestamp */
-	uint64_t _event_count;			/**< total data counter */
-	uint64_t _error_count;			/**< error count */
-	int _error_density;			/**< ratio between successful reads and errors */
-	int _priority;				/**< sensor nominal priority */
-	float _mean[dimensions];		/**< mean of value */
-	float _lp[dimensions];			/**< low pass value */
-	float _M2[dimensions];			/**< RMS component value */
-	float _rms[dimensions];			/**< root mean square error */
-	float _value[dimensions];		/**< last value */
-	float _vibe[dimensions];		/**< vibration level, in sensor unit */
-
-	unsigned _value_equal_count;		/**< equal values in a row */
-	unsigned _value_equal_count_threshold;	/**< when to consider an equal count as a problem */
-
-	DataValidator *_sibling;		/**< sibling in the group */
+	uint32_t _error_mask{ERROR_FLAG_NO_ERROR};	/**< sensor error state */
+
+	uint32_t _timeout_interval{20000};		/**< interval in which the datastream times out in us */
+
+	uint64_t _time_last{0};				/**< last timestamp */
+	uint64_t _event_count{0};			/**< total data counter */
+	uint64_t _error_count{0};			/**< error count */
+
+	int _error_density{0};				/**< ratio between successful reads and errors */
+
+	int _priority{0};				/**< sensor nominal priority */
+
+	float _mean[dimensions] {};			/**< mean of value */
+	float _lp[dimensions] {};			/**< low pass value */
+	float _M2[dimensions] {};			/**< RMS component value */
+	float _rms[dimensions] {};			/**< root mean square error */
+	float _value[dimensions] {};			/**< last value */
+	float _vibe[dimensions] {};			/**< vibration level, in sensor unit */
+
+	unsigned _value_equal_count{0};			/**< equal values in a row */
+	unsigned _value_equal_count_threshold{VALUE_EQUAL_COUNT_DEFAULT};	/**< when to consider an equal count as a problem */
+
+	DataValidator *_sibling{nullptr};		/**< sibling in the group */
 
 	static const constexpr unsigned NORETURN_ERRCOUNT = 10000;	/**< if the error count reaches this value, return sensor as invalid */
 	static const constexpr float ERROR_DENSITY_WINDOW = 100.0f; 	/**< window in measurement counts for errors */

validation/data_validator_group.cpp

@@ -43,13 +43,7 @@
 #include <ecl.h>
 #include <cfloat>
 
-DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
-	_first(nullptr),
-	_last(nullptr),
-	_curr_best(-1),
-	_prev_best(-1),
-	_first_failover_time(0),
-	_toggle_count(0)
+DataValidatorGroup::DataValidatorGroup(unsigned siblings)
 {
 	DataValidator *next = nullptr;
 	DataValidator *prev = nullptr;
@@ -123,7 +117,7 @@ DataValidatorGroup::set_equal_value_threshold(uint32_t threshold)
 
 
 void
-DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
+DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority)
 {
 	DataValidator *next = _first;
 	unsigned i = 0;
@@ -153,12 +147,12 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 	int max_index = -1;
 	DataValidator *best = nullptr;
 
-	unsigned i = 0;
+	int i = 0;
 
 	while (next != nullptr) {
 		float confidence = next->confidence(timestamp);
 
-		if (static_cast<int>(i) == pre_check_best) {
+		if (i == pre_check_best) {
 			pre_check_prio = next->priority();
 			pre_check_confidence = confidence;
 		}
@@ -192,10 +186,14 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 		/* check whether the switch was a failsafe or preferring a higher priority sensor */
 		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
 		    fabsf(pre_check_confidence - max_confidence) < 0.1f) {
+
 			/* this is not a failover */
 			true_failsafe = false;
+
 			/* reset error flags, this is likely a hotplug sensor coming online late */
-			best->reset_state();
+			if (best != nullptr) {
+				best->reset_state();
+			}
 		}
 
 		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
@@ -305,12 +303,6 @@ DataValidatorGroup::print()
 	}
 }
 
-unsigned
-DataValidatorGroup::failover_count()
-{
-	return _toggle_count;
-}
-
 int
 DataValidatorGroup::failover_index()
 {

validation/data_validator_group.h

@@ -50,7 +50,7 @@ public:
 	 * @param siblings initial number of DataValidator's. Must be > 0.
 	 */
 	DataValidatorGroup(unsigned siblings);
-	virtual ~DataValidatorGroup();
+	~DataValidatorGroup();
 
 	/**
 	 * Create a new Validator (with index equal to the number of currently existing validators)
@@ -67,7 +67,7 @@ public:
 	 * @param error_count	The current error count of the sensor
 	 * @param priority	The priority of the sensor
 	 */
-	void			put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority);
+	void			put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority);
 
 	/**
 	 * Get the best data triplet of the group
@@ -95,7 +95,7 @@ public:
 	 *
 	 * @return		the number of failovers
 	 */
-	unsigned		failover_count();
+	unsigned		failover_count() const { return _toggle_count; }
 
 	/**
 	 * Get the index of the failed sensor in the group
@@ -133,13 +133,18 @@ public:
 
 
 private:
-	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 */
-	uint64_t _first_failover_time;	/**< timestamp where the first failover occured or zero if none occured */
-	unsigned _toggle_count;		/**< number of back and forth switches between two sensors */
+	DataValidator *_first{nullptr};		/**< first node in the group */
+	DataValidator *_last{nullptr};		/**< last node in the group */
+
+	uint32_t _timeout_interval_us{0}; /**< currently set timeout */
+
+	int _curr_best{-1};		/**< currently best index */
+	int _prev_best{-1};		/**< the previous best index */
+
+	uint64_t _first_failover_time{0};	/**< timestamp where the first failover occured or zero if none occured */
+
+	unsigned _toggle_count{0};		/**< number of back and forth switches between two sensors */
+
 	static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;
 
 	/* we don't want this class to be copied */