forked from Archive/PX4-Autopilot
190 lines
6.2 KiB
C++
190 lines
6.2 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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;
|
|
};
|