px4_autopilot/validation/data_validator.h

/****************************************************************************
 *
 *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file data_validator.h
 *
 * A data validation class to identify anomalies in data streams
 *
 * @author Lorenz Meier <lorenz@px4.io>
 */

#pragma once

#include <cmath>
#include <stdint.h>

class __EXPORT DataValidator {
public:
	static const unsigned dimensions = 3;

	DataValidator(DataValidator *prev_sibling = nullptr);
	virtual ~DataValidator() = default;

	/**
	 * Put an item into the validator.
	 *
	 * @param val		Item to put
	 */
	void			put(uint64_t timestamp, float val, uint64_t error_count, int priority);

	/**
	 * Put a 3D item into the validator.
	 *
	 * @param val		Item to put
	 */
	void			put(uint64_t timestamp, float val[dimensions], uint64_t error_count, int priority);

	/**
	 * Get the next sibling in the group
	 *
	 * @return		the next sibling
	 */
	DataValidator*		sibling() { return _sibling; }

	/**
	 * Get the confidence of this validator
	 * @return		the confidence between 0 and 1
	 */
	float			confidence(uint64_t timestamp);

	/**
	 * Get the error count of this validator
	 * @return		the error count
	 */
	uint64_t		error_count() { return _error_count; }

	/**
	 * Get the values of this validator
	 * @return		the stored value
	 */
	float*			value() { return _value; }

	/**
	 * Get the used status of this validator
	 * @return		true if this validator ever saw data
	 */
	bool			used() { return (_time_last > 0); }

	/**
	 * Get the priority of this validator
	 * @return		the stored priority
	 */
	int			priority() { return (_priority); }

	/**
	 * Get the error state of this validator
	 * @return		the bitmask with the error status
	 */
	uint32_t		state() { return _error_mask; }

	/**
	 * Reset the error state of this validator
	 */
	void			reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }

	/**
	 * Get the RMS values of this validator
	 * @return		the stored RMS
	 */
	float*			rms() { return _rms; }

	/**
	 * Get the vibration offset
	 * @return		the stored vibration offset
	 */
	float*			vibration_offset() { return _vibe; }

	/**
	 * Print the validator value
	 *
	 */
	void			print();

	/**
	 * Set the timeout value
	 *
	 * @param timeout_interval_us The timeout interval in microseconds
	 */
	void			set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }

	/**
	 * Set the equal count threshold
	 *
	 * @param threshold The number of equal values before considering the sensor stale
	 */
	void			set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; }

	/**
	 * Get the timeout value
	 *
	 * @return The timeout interval in microseconds
	 */
	uint32_t			get_timeout() const { return _timeout_interval; }

	/**
	 * Data validator error states
	 */
	static constexpr uint32_t ERROR_FLAG_NO_ERROR      	= (0x00000000U);
	static constexpr uint32_t ERROR_FLAG_NO_DATA       	= (0x00000001U);
	static constexpr uint32_t ERROR_FLAG_STALE_DATA    	= (0x00000001U << 1);
	static constexpr uint32_t ERROR_FLAG_TIMEOUT 	   	= (0x00000001U << 2);
	static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT 	= (0x00000001U << 3);
	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 */
	float _value_equal_count;		/**< equal values in a row */
	float _value_equal_count_threshold; /**< when to consider an equal count as a problem */
	DataValidator *_sibling;		/**< 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 */
	static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = 100;	/**< if the sensor value is the same (accumulated also between axes) this many times, flag it */

	/* we don't want this class to be copied */
	DataValidator(const DataValidator&) = delete;
	DataValidator operator=(const DataValidator&) = delete;
};