mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-16 20:00:35 +08:00
move ecl L1, TECS, and data validator to PX4/Firmware
This commit is contained in:
Daniel Agar
@@ -39,11 +39,11 @@ px4_add_module(
|
||||
MAIN fw_pos_control_l1
|
||||
SRCS
|
||||
FixedwingPositionControl.cpp
|
||||
FixedwingPositionControl.hpp
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_l1
|
||||
ecl_tecs
|
||||
l1
|
||||
launchdetection
|
||||
landing_slope
|
||||
runway_takeoff
|
||||
tecs
|
||||
)
|
||||
|
||||
@@ -55,8 +55,8 @@
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/ecl/l1/ecl_l1_pos_controller.h>
|
||||
#include <lib/ecl/tecs/tecs.h>
|
||||
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
|
||||
#include <lib/tecs/TECS.hpp>
|
||||
#include <lib/landing_slope/Landingslope.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
@@ -35,8 +35,8 @@ px4_add_module(
|
||||
MAIN rover_pos_control
|
||||
SRCS
|
||||
RoverPositionControl.cpp
|
||||
RoverPositionControl.hpp
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_l1
|
||||
l1
|
||||
pid
|
||||
)
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/ecl/l1/ecl_l1_pos_controller.h>
|
||||
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/pid/pid.h>
|
||||
|
||||
@@ -31,6 +31,8 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(data_validator)
|
||||
|
||||
add_subdirectory(sensor_calibration) # used by vehicle_{acceleration, angular_velocity, imu}
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
add_subdirectory(vehicle_acceleration)
|
||||
@@ -48,9 +50,9 @@ px4_add_module(
|
||||
DEPENDS
|
||||
airspeed
|
||||
conversion
|
||||
data_validator
|
||||
drivers__device
|
||||
git_ecl
|
||||
ecl_validation
|
||||
mathlib
|
||||
vehicle_acceleration
|
||||
vehicle_angular_velocity
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018-2020 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(data_validator
|
||||
DataValidator.cpp
|
||||
DataValidator.hpp
|
||||
DataValidatorGroup.cpp
|
||||
DataValidatorGroup.hpp
|
||||
)
|
||||
@@ -0,0 +1,153 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2020 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 DataValidator.cpp
|
||||
*
|
||||
* A data validation class to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include "DataValidator.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
void DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in)
|
||||
{
|
||||
float data[dimensions] = {val}; // sets the first value and all others to 0
|
||||
put(timestamp, data, error_count_in, priority_in);
|
||||
}
|
||||
|
||||
void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t error_count_in, int priority_in)
|
||||
{
|
||||
|
||||
_event_count++;
|
||||
|
||||
if (error_count_in > _error_count) {
|
||||
_error_density += (error_count_in - _error_count);
|
||||
|
||||
} else if (_error_density > 0) {
|
||||
_error_density--;
|
||||
}
|
||||
|
||||
_error_count = error_count_in;
|
||||
_priority = priority_in;
|
||||
|
||||
for (unsigned i = 0; i < dimensions; i++) {
|
||||
if (_time_last == 0) {
|
||||
_mean[i] = 0;
|
||||
_lp[i] = val[i];
|
||||
_M2[i] = 0;
|
||||
|
||||
} else {
|
||||
float lp_val = val[i] - _lp[i];
|
||||
|
||||
float delta_val = lp_val - _mean[i];
|
||||
_mean[i] += delta_val / _event_count;
|
||||
_M2[i] += delta_val * (lp_val - _mean[i]);
|
||||
_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
|
||||
|
||||
if (fabsf(_value[i] - val[i]) < 0.000001f) {
|
||||
_value_equal_count++;
|
||||
|
||||
} else {
|
||||
_value_equal_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// XXX replace with better filter, make it auto-tune to update rate
|
||||
_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
|
||||
|
||||
_value[i] = val[i];
|
||||
}
|
||||
|
||||
_time_last = timestamp;
|
||||
}
|
||||
|
||||
float DataValidator::confidence(uint64_t timestamp)
|
||||
{
|
||||
|
||||
float ret = 1.0f;
|
||||
|
||||
/* check if we have any data */
|
||||
if (_time_last == 0) {
|
||||
_error_mask |= ERROR_FLAG_NO_DATA;
|
||||
ret = 0.0f;
|
||||
|
||||
} else if (timestamp - _time_last > _timeout_interval) {
|
||||
/* timed out - that's it */
|
||||
_error_mask |= ERROR_FLAG_TIMEOUT;
|
||||
ret = 0.0f;
|
||||
|
||||
} else if (_value_equal_count > _value_equal_count_threshold) {
|
||||
/* we got the exact same sensor value N times in a row */
|
||||
_error_mask |= ERROR_FLAG_STALE_DATA;
|
||||
ret = 0.0f;
|
||||
|
||||
} else if (_error_count > NORETURN_ERRCOUNT) {
|
||||
/* check error count limit */
|
||||
_error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
|
||||
ret = 0.0f;
|
||||
|
||||
} else if (_error_density > ERROR_DENSITY_WINDOW) {
|
||||
/* cap error density counter at window size */
|
||||
_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
|
||||
_error_density = ERROR_DENSITY_WINDOW;
|
||||
}
|
||||
|
||||
/* no critical errors */
|
||||
if (ret > 0.0f) {
|
||||
/* return local error density for last N measurements */
|
||||
ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
|
||||
|
||||
if (ret > 0.0f) {
|
||||
_error_mask = ERROR_FLAG_NO_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void DataValidator::print()
|
||||
{
|
||||
if (_time_last == 0) {
|
||||
PX4_INFO("\tno data");
|
||||
return;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < dimensions; i++) {
|
||||
PX4_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", (double)_value[i],
|
||||
(double)_lp[i], (double)_mean[i], (double)_rms[i], (double)confidence(hrt_absolute_time()));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,200 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2020 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 DataValidator.hpp
|
||||
*
|
||||
* A data validation class to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
class DataValidator
|
||||
{
|
||||
public:
|
||||
static const unsigned dimensions = 3;
|
||||
|
||||
DataValidator() = default;
|
||||
~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, const float val[dimensions], uint64_t error_count, int priority);
|
||||
|
||||
/**
|
||||
* Get the next sibling in the group
|
||||
*
|
||||
* @return the next sibling
|
||||
*/
|
||||
DataValidator *sibling() { return _sibling; }
|
||||
|
||||
/**
|
||||
* Set the sibling to the next node in the group
|
||||
*
|
||||
*/
|
||||
void setSibling(DataValidator *new_sibling) { _sibling = new_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() const { 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() const { return (_time_last > 0); }
|
||||
|
||||
/**
|
||||
* Get the priority of this validator
|
||||
* @return the stored priority
|
||||
*/
|
||||
int priority() const { return _priority; }
|
||||
|
||||
/**
|
||||
* Get the error state of this validator
|
||||
* @return the bitmask with the error status
|
||||
*/
|
||||
uint32_t state() const { 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; }
|
||||
|
||||
/**
|
||||
* 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{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 */
|
||||
|
||||
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 */
|
||||
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;
|
||||
};
|
||||
@@ -0,0 +1,307 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2020 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 DataValidatorGroup.cpp
|
||||
*
|
||||
* A data validation group to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include "DataValidatorGroup.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include <float.h>
|
||||
|
||||
DataValidatorGroup::DataValidatorGroup(unsigned siblings)
|
||||
{
|
||||
|
||||
DataValidator *next = nullptr;
|
||||
DataValidator *prev = nullptr;
|
||||
|
||||
for (unsigned i = 0; i < siblings; i++) {
|
||||
next = new DataValidator();
|
||||
|
||||
if (i == 0) {
|
||||
_first = next;
|
||||
|
||||
} else {
|
||||
prev->setSibling(next);
|
||||
}
|
||||
|
||||
prev = next;
|
||||
}
|
||||
|
||||
_last = next;
|
||||
|
||||
if (_first) {
|
||||
_timeout_interval_us = _first->get_timeout();
|
||||
}
|
||||
}
|
||||
|
||||
DataValidatorGroup::~DataValidatorGroup()
|
||||
{
|
||||
while (_first) {
|
||||
DataValidator *next = _first->sibling();
|
||||
delete (_first);
|
||||
_first = next;
|
||||
}
|
||||
}
|
||||
|
||||
DataValidator *DataValidatorGroup::add_new_validator()
|
||||
{
|
||||
|
||||
DataValidator *validator = new DataValidator();
|
||||
|
||||
if (!validator) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
_last->setSibling(validator);
|
||||
_last = validator;
|
||||
_last->set_timeout(_timeout_interval_us);
|
||||
return _last;
|
||||
}
|
||||
|
||||
void DataValidatorGroup::set_timeout(uint32_t timeout_interval_us)
|
||||
{
|
||||
|
||||
DataValidator *next = _first;
|
||||
|
||||
while (next != nullptr) {
|
||||
next->set_timeout(timeout_interval_us);
|
||||
next = next->sibling();
|
||||
}
|
||||
|
||||
_timeout_interval_us = timeout_interval_us;
|
||||
}
|
||||
|
||||
void DataValidatorGroup::set_equal_value_threshold(uint32_t threshold)
|
||||
{
|
||||
|
||||
DataValidator *next = _first;
|
||||
|
||||
while (next != nullptr) {
|
||||
next->set_equal_value_threshold(threshold);
|
||||
next = next->sibling();
|
||||
}
|
||||
}
|
||||
|
||||
void DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count,
|
||||
int priority)
|
||||
{
|
||||
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
if (i == index) {
|
||||
next->put(timestamp, val, error_count, priority);
|
||||
break;
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
float *DataValidatorGroup::get_best(uint64_t timestamp, int *index)
|
||||
{
|
||||
|
||||
DataValidator *next = _first;
|
||||
|
||||
// XXX This should eventually also include voting
|
||||
int pre_check_best = _curr_best;
|
||||
float pre_check_confidence = 1.0f;
|
||||
int pre_check_prio = -1;
|
||||
float max_confidence = -1.0f;
|
||||
int max_priority = -1000;
|
||||
int max_index = -1;
|
||||
DataValidator *best = nullptr;
|
||||
|
||||
int i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
float confidence = next->confidence(timestamp);
|
||||
|
||||
if (i == pre_check_best) {
|
||||
pre_check_prio = next->priority();
|
||||
pre_check_confidence = confidence;
|
||||
}
|
||||
|
||||
/*
|
||||
* Switch if:
|
||||
* 1) the confidence is higher and priority is equal or higher
|
||||
* 2) the confidence is no less than 1% different and the priority is higher
|
||||
*/
|
||||
if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
|
||||
(confidence > max_confidence && (next->priority() >= max_priority)) ||
|
||||
(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))) &&
|
||||
(confidence > 0.0f)) {
|
||||
max_index = i;
|
||||
max_confidence = confidence;
|
||||
max_priority = next->priority();
|
||||
best = next;
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
|
||||
/* the current best sensor is not matching the previous best sensor,
|
||||
* or the only sensor went bad */
|
||||
if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) {
|
||||
bool true_failsafe = true;
|
||||
|
||||
/* 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 */
|
||||
if (best != nullptr) {
|
||||
best->reset_state();
|
||||
}
|
||||
}
|
||||
|
||||
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
|
||||
if (_curr_best < 0) {
|
||||
_prev_best = max_index;
|
||||
|
||||
} else {
|
||||
/* we were initialized before, this is a real failsafe */
|
||||
_prev_best = pre_check_best;
|
||||
|
||||
if (true_failsafe) {
|
||||
_toggle_count++;
|
||||
|
||||
/* if this is the first time, log when we failed */
|
||||
if (_first_failover_time == 0) {
|
||||
_first_failover_time = timestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* for all cases we want to keep a record of the best index */
|
||||
_curr_best = max_index;
|
||||
}
|
||||
|
||||
*index = max_index;
|
||||
return (best) ? best->value() : nullptr;
|
||||
}
|
||||
|
||||
void DataValidatorGroup::print()
|
||||
{
|
||||
PX4_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best,
|
||||
(_toggle_count > 0) ? "YES" : "NO", _toggle_count);
|
||||
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
if (next->used()) {
|
||||
uint32_t flags = next->state();
|
||||
|
||||
PX4_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(),
|
||||
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
|
||||
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
|
||||
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""),
|
||||
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""),
|
||||
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""),
|
||||
((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : ""));
|
||||
|
||||
next->print();
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
int DataValidatorGroup::failover_index()
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) &&
|
||||
(i == (unsigned)_prev_best)) {
|
||||
return i;
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint32_t DataValidatorGroup::failover_state()
|
||||
{
|
||||
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) &&
|
||||
(i == (unsigned)_prev_best)) {
|
||||
return next->state();
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
|
||||
return DataValidator::ERROR_FLAG_NO_ERROR;
|
||||
}
|
||||
|
||||
uint32_t DataValidatorGroup::get_sensor_state(unsigned index)
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
if (i == index) {
|
||||
return next->state();
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
|
||||
// sensor index not found
|
||||
return UINT32_MAX;
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2020 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 DataValidatorGroup.hpp
|
||||
*
|
||||
* A data validation group to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "DataValidator.hpp"
|
||||
|
||||
class DataValidatorGroup
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @param siblings initial number of DataValidator's. Must be > 0.
|
||||
*/
|
||||
DataValidatorGroup(unsigned siblings);
|
||||
~DataValidatorGroup();
|
||||
|
||||
/**
|
||||
* Create a new Validator (with index equal to the number of currently existing validators)
|
||||
* @return the newly created DataValidator or nullptr on error
|
||||
*/
|
||||
DataValidator *add_new_validator();
|
||||
|
||||
/**
|
||||
* Put an item into the validator group.
|
||||
*
|
||||
* @param index Sensor index
|
||||
* @param timestamp The timestamp of the measurement
|
||||
* @param val The 3D vector
|
||||
* @param error_count The current error count of the sensor
|
||||
* @param priority The priority of the sensor
|
||||
*/
|
||||
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
|
||||
*
|
||||
* @return pointer to the array of best values
|
||||
*/
|
||||
float *get_best(uint64_t timestamp, int *index);
|
||||
|
||||
/**
|
||||
* Get the number of failover events
|
||||
*
|
||||
* @return the number of failovers
|
||||
*/
|
||||
unsigned failover_count() const { return _toggle_count; }
|
||||
|
||||
/**
|
||||
* Get the index of the failed sensor in the group
|
||||
*
|
||||
* @return index of the failed sensor
|
||||
*/
|
||||
int failover_index();
|
||||
|
||||
/**
|
||||
* Get the error state of the failed sensor in the group
|
||||
*
|
||||
* @return bitmask with erro states of the failed sensor
|
||||
*/
|
||||
uint32_t failover_state();
|
||||
|
||||
/**
|
||||
* Get the error state of the sensor with the specified index
|
||||
*
|
||||
* @return bitmask with error states of the sensor
|
||||
*/
|
||||
uint32_t get_sensor_state(unsigned index);
|
||||
|
||||
/**
|
||||
* Print the validator value
|
||||
*
|
||||
*/
|
||||
void print();
|
||||
|
||||
/**
|
||||
* Set the timeout value for the whole group
|
||||
*
|
||||
* @param timeout_interval_us The timeout interval in microseconds
|
||||
*/
|
||||
void set_timeout(uint32_t timeout_interval_us);
|
||||
|
||||
/**
|
||||
* Set the equal count threshold for the whole group
|
||||
*
|
||||
* @param threshold The number of equal values before considering the sensor stale
|
||||
*/
|
||||
void set_equal_value_threshold(uint32_t threshold);
|
||||
|
||||
private:
|
||||
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 */
|
||||
DataValidatorGroup(const DataValidatorGroup &);
|
||||
DataValidatorGroup operator=(const DataValidatorGroup &);
|
||||
};
|
||||
@@ -0,0 +1,46 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_executable(ecl_tests_data_validator test_data_validator.cpp tests_common.cpp)
|
||||
target_link_libraries(ecl_tests_data_validator ecl_validation)
|
||||
|
||||
add_test(NAME ecl_tests_data_validator
|
||||
COMMAND ecl_tests_data_validator
|
||||
)
|
||||
|
||||
add_executable(ecl_tests_data_validator_group test_data_validator_group.cpp tests_common.cpp)
|
||||
target_link_libraries(ecl_tests_data_validator_group ecl_validation)
|
||||
|
||||
add_test(NAME ecl_tests_data_validator_group
|
||||
COMMAND ecl_tests_data_validator_group
|
||||
)
|
||||
@@ -0,0 +1,302 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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 test_data_validator.cpp
|
||||
* Testing the DataValidator class
|
||||
*
|
||||
* @author Todd Stellanova
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <validation/data_validator.h>
|
||||
#include <validation/tests/tests_common.h>
|
||||
|
||||
|
||||
void test_init()
|
||||
{
|
||||
printf("\n--- test_init ---\n");
|
||||
|
||||
uint64_t fake_timestamp = 666;
|
||||
const uint32_t timeout_usec = 2000;//from original private value
|
||||
|
||||
DataValidator *validator = new DataValidator;
|
||||
// initially there should be no siblings
|
||||
assert(nullptr == validator->sibling());
|
||||
// initially we should have zero confidence
|
||||
assert(0.0f == validator->confidence(fake_timestamp));
|
||||
// initially the error count should be zero
|
||||
assert(0 == validator->error_count());
|
||||
// initially unused
|
||||
assert(!validator->used());
|
||||
// initially no priority
|
||||
assert(0 == validator->priority());
|
||||
validator->set_timeout(timeout_usec);
|
||||
assert(validator->get_timeout() == timeout_usec);
|
||||
|
||||
|
||||
DataValidator *sibling_validator = new DataValidator;
|
||||
validator->setSibling(sibling_validator);
|
||||
assert(sibling_validator == validator->sibling());
|
||||
|
||||
//verify that with no data, confidence is zero and error mask is set
|
||||
assert(0.0f == validator->confidence(fake_timestamp + 1));
|
||||
uint32_t state = validator->state();
|
||||
assert(DataValidator::ERROR_FLAG_NO_DATA == (DataValidator::ERROR_FLAG_NO_DATA & state));
|
||||
|
||||
//verify that calling print doesn't crash tests
|
||||
validator->print();
|
||||
|
||||
delete validator; //force delete
|
||||
}
|
||||
|
||||
void test_put()
|
||||
{
|
||||
printf("\n--- test_put ---\n");
|
||||
|
||||
uint64_t timestamp = 500;
|
||||
const uint32_t timeout_usec = 2000;//derived from class-private value
|
||||
float val = 3.14159f;
|
||||
//derived from class-private value: this is min change needed to avoid stale detection
|
||||
const float sufficient_incr_value = (1.1f * 1E-6f);
|
||||
|
||||
DataValidator *validator = new DataValidator;
|
||||
fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp);
|
||||
|
||||
assert(validator->used());
|
||||
//verify that the last value we inserted is the current validator value
|
||||
float last_val = val - sufficient_incr_value;
|
||||
assert(validator->value()[0] == last_val);
|
||||
|
||||
// we've just provided a bunch of valid data: should be fully confident
|
||||
float conf = validator->confidence(timestamp);
|
||||
|
||||
if (1.0f != conf) {
|
||||
printf("conf: %f\n", (double)conf);
|
||||
dump_validator_state(validator);
|
||||
}
|
||||
|
||||
assert(1.0f == conf);
|
||||
// should be no errors
|
||||
assert(0 == validator->state());
|
||||
|
||||
//now check confidence much beyond the timeout window-- should timeout
|
||||
conf = validator->confidence(timestamp + (1.1 * timeout_usec));
|
||||
|
||||
if (0.0f != conf) {
|
||||
printf("conf: %f\n", (double)conf);
|
||||
dump_validator_state(validator);
|
||||
}
|
||||
|
||||
assert(0.0f == conf);
|
||||
assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state()));
|
||||
|
||||
delete validator; //force delete
|
||||
}
|
||||
|
||||
/**
|
||||
* Verify that the DataValidator detects sensor data that does not vary sufficiently
|
||||
*/
|
||||
void test_stale_detector()
|
||||
{
|
||||
printf("\n--- test_stale_detector ---\n");
|
||||
|
||||
uint64_t timestamp = 500;
|
||||
float val = 3.14159f;
|
||||
//derived from class-private value, this is insufficient to avoid stale detection:
|
||||
const float insufficient_incr_value = (0.99 * 1E-6f);
|
||||
|
||||
DataValidator *validator = new DataValidator;
|
||||
fill_validator_with_samples(validator, insufficient_incr_value, &val, ×tamp);
|
||||
|
||||
// data is stale: should have no confidence
|
||||
assert(0.0f == validator->confidence(timestamp));
|
||||
|
||||
// should be a stale error
|
||||
uint32_t state = validator->state();
|
||||
|
||||
if (DataValidator::ERROR_FLAG_STALE_DATA != state) {
|
||||
dump_validator_state(validator);
|
||||
}
|
||||
|
||||
assert(DataValidator::ERROR_FLAG_STALE_DATA == (DataValidator::ERROR_FLAG_STALE_DATA & state));
|
||||
|
||||
delete validator; //force delete
|
||||
}
|
||||
|
||||
/**
|
||||
* Verify the RMS error calculated by the DataValidator for a series of samples
|
||||
*/
|
||||
void test_rms_calculation()
|
||||
{
|
||||
printf("\n--- test_rms_calculation ---\n");
|
||||
const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
|
||||
const float mean_value = 3.14159f;
|
||||
const uint32_t sample_count = 1000;
|
||||
float expected_rms_err = 0.0f;
|
||||
uint64_t timestamp = 500;
|
||||
|
||||
DataValidator *validator = new DataValidator;
|
||||
validator->set_equal_value_threshold(equal_value_count);
|
||||
|
||||
insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, ×tamp);
|
||||
float *rms = validator->rms();
|
||||
assert(nullptr != rms);
|
||||
float calc_rms_err = rms[0];
|
||||
float diff = fabsf(calc_rms_err - expected_rms_err);
|
||||
float diff_frac = (diff / expected_rms_err);
|
||||
printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err,
|
||||
(double)diff, (double)diff_frac);
|
||||
assert(diff_frac < 0.03f);
|
||||
|
||||
delete validator; //force delete
|
||||
}
|
||||
|
||||
/**
|
||||
* Verify error tracking performed by DataValidator::put
|
||||
*/
|
||||
void test_error_tracking()
|
||||
{
|
||||
printf("\n--- test_error_tracking ---\n");
|
||||
uint64_t timestamp = 500;
|
||||
uint64_t timestamp_incr = 5;
|
||||
const uint32_t timeout_usec = 2000;//from original private value
|
||||
float val = 3.14159f;
|
||||
uint64_t error_count = 0;
|
||||
int expected_error_density = 0;
|
||||
int priority = 50;
|
||||
//from private value: this is min change needed to avoid stale detection
|
||||
const float sufficient_incr_value = (1.1f * 1E-6f);
|
||||
//default is private VALUE_EQUAL_COUNT_DEFAULT
|
||||
const int equal_value_count = 50000;
|
||||
//should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT
|
||||
const int total_iterations = 1000;
|
||||
|
||||
DataValidator *validator = new DataValidator;
|
||||
validator->set_timeout(timeout_usec);
|
||||
validator->set_equal_value_threshold(equal_value_count);
|
||||
|
||||
//put a bunch of values that are all different
|
||||
for (int i = 0; i < total_iterations; i++, val += sufficient_incr_value) {
|
||||
timestamp += timestamp_incr;
|
||||
|
||||
//up to a 50% random error rate appears to pass the error density filter
|
||||
if ((((float)rand() / (float)RAND_MAX)) < 0.500f) {
|
||||
error_count += 1;
|
||||
expected_error_density += 1;
|
||||
|
||||
} else if (expected_error_density > 0) {
|
||||
expected_error_density -= 1;
|
||||
}
|
||||
|
||||
validator->put(timestamp, val, error_count, priority);
|
||||
}
|
||||
|
||||
assert(validator->used());
|
||||
//at this point, error_count should be less than NORETURN_ERRCOUNT
|
||||
assert(validator->error_count() == error_count);
|
||||
|
||||
// we've just provided a bunch of valid data with some errors:
|
||||
// confidence should be reduced by the number of errors
|
||||
float conf = validator->confidence(timestamp);
|
||||
printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf);
|
||||
assert(1.0f != conf); //we should not be fully confident
|
||||
assert(0.0f != conf); //neither should we be completely unconfident
|
||||
// should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT
|
||||
assert(0 == validator->state());
|
||||
|
||||
// the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW)
|
||||
// ERROR_DENSITY_WINDOW is currently private, but == 100.0f
|
||||
float reduced_conf = 1.0f - ((float)expected_error_density / 100.0f);
|
||||
double diff = fabs(reduced_conf - conf);
|
||||
|
||||
if (reduced_conf != conf) {
|
||||
printf("conf: %f reduced_conf: %f diff: %f\n",
|
||||
(double)conf, (double)reduced_conf, diff);
|
||||
dump_validator_state(validator);
|
||||
}
|
||||
|
||||
assert(diff < 1E-6f);
|
||||
|
||||
//Now, insert a series of errors and ensure we trip the error detector
|
||||
for (int i = 0; i < 250; i++, val += sufficient_incr_value) {
|
||||
timestamp += timestamp_incr;
|
||||
//100% error rate
|
||||
error_count += 1;
|
||||
expected_error_density += 1;
|
||||
validator->put(timestamp, val, error_count, priority);
|
||||
}
|
||||
|
||||
conf = validator->confidence(timestamp);
|
||||
assert(0.0f == conf); // should we be completely unconfident
|
||||
// we should have triggered the high error density detector
|
||||
assert(DataValidator::ERROR_FLAG_HIGH_ERRDENSITY == (DataValidator::ERROR_FLAG_HIGH_ERRDENSITY & validator->state()));
|
||||
|
||||
|
||||
validator->reset_state();
|
||||
|
||||
//Now insert so many errors that we exceed private NORETURN_ERRCOUNT
|
||||
for (int i = 0; i < 10000; i++, val += sufficient_incr_value) {
|
||||
timestamp += timestamp_incr;
|
||||
//100% error rate
|
||||
error_count += 1;
|
||||
expected_error_density += 1;
|
||||
validator->put(timestamp, val, error_count, priority);
|
||||
}
|
||||
|
||||
conf = validator->confidence(timestamp);
|
||||
assert(0.0f == conf); // should we be completely unconfident
|
||||
// we should have triggered the high error count detector
|
||||
assert(DataValidator::ERROR_FLAG_HIGH_ERRCOUNT == (DataValidator::ERROR_FLAG_HIGH_ERRCOUNT & validator->state()));
|
||||
|
||||
delete validator; //force delete
|
||||
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
(void)argc; // unused
|
||||
(void)argv; // unused
|
||||
|
||||
srand(666);
|
||||
test_init();
|
||||
test_put();
|
||||
test_stale_detector();
|
||||
test_rms_calculation();
|
||||
test_error_tracking();
|
||||
|
||||
return 0; //passed
|
||||
}
|
||||
@@ -0,0 +1,385 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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 test_data_validator_group.cpp
|
||||
* Testing the DataValidatorGroup class
|
||||
*
|
||||
* @author Todd Stellanova
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <validation/data_validator.h>
|
||||
#include <validation/data_validator_group.h>
|
||||
#include <validation/tests/tests_common.h>
|
||||
|
||||
|
||||
const uint32_t base_timeout_usec = 2000;//from original private value
|
||||
const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
|
||||
const uint64_t base_timestamp = 666;
|
||||
const unsigned base_num_siblings = 4;
|
||||
|
||||
|
||||
/**
|
||||
* Initialize a DataValidatorGroup with some common settings;
|
||||
* @param sibling_count (out) the number of siblings initialized
|
||||
*/
|
||||
DataValidatorGroup *setup_base_group(unsigned *sibling_count)
|
||||
{
|
||||
unsigned num_siblings = base_num_siblings;
|
||||
|
||||
DataValidatorGroup *group = new DataValidatorGroup(num_siblings);
|
||||
assert(nullptr != group);
|
||||
//verify that calling print doesn't crash the tests
|
||||
group->print();
|
||||
printf("\n");
|
||||
|
||||
//should be no failovers yet
|
||||
assert(0 == group->failover_count());
|
||||
assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state());
|
||||
assert(-1 == group->failover_index());
|
||||
|
||||
//this sets the timeout on all current members of the group, as well as members added later
|
||||
group->set_timeout(base_timeout_usec);
|
||||
//the following sets the threshold on all CURRENT members of the group, but not any added later
|
||||
//TODO This is likely a bug in DataValidatorGroup
|
||||
group->set_equal_value_threshold(equal_value_count);
|
||||
|
||||
//return values
|
||||
*sibling_count = num_siblings;
|
||||
|
||||
return group;
|
||||
}
|
||||
|
||||
/**
|
||||
* Fill one DataValidator with samples, by index.
|
||||
*
|
||||
* @param group
|
||||
* @param val1_idx Index of the validator to fill with samples
|
||||
* @param num_samples
|
||||
*/
|
||||
void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples)
|
||||
{
|
||||
uint64_t timestamp = base_timestamp;
|
||||
uint64_t error_count = 0;
|
||||
float last_best_val = 0.0f;
|
||||
|
||||
for (uint32_t i = 0; i < num_samples; i++) {
|
||||
float val = ((float) rand() / (float) RAND_MAX);
|
||||
float data[DataValidator::dimensions] = {val};
|
||||
group->put(val1_idx, timestamp, data, error_count, 100);
|
||||
last_best_val = val;
|
||||
}
|
||||
|
||||
int best_idx = 0;
|
||||
float *best_data = group->get_best(timestamp, &best_idx);
|
||||
assert(last_best_val == best_data[0]);
|
||||
assert(best_idx == val1_idx);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Fill two validators in the group with samples, by index.
|
||||
* Both validators will be filled with the same data, but
|
||||
* the priority of the first validator will be higher than the second.
|
||||
*
|
||||
* @param group
|
||||
* @param val1_idx index of the first validator to fill
|
||||
* @param val2_idx index of the second validator to fill
|
||||
* @param num_samples
|
||||
*/
|
||||
void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples)
|
||||
{
|
||||
uint64_t timestamp = base_timestamp;
|
||||
uint64_t error_count = 0;
|
||||
float last_best_val = 0.0f;
|
||||
|
||||
for (uint32_t i = 0; i < num_samples; i++) {
|
||||
float val = ((float) rand() / (float) RAND_MAX);
|
||||
float data[DataValidator::dimensions] = {val};
|
||||
//two sensors with identical values, but different priorities
|
||||
group->put(val1_idx, timestamp, data, error_count, 100);
|
||||
group->put(val2_idx, timestamp, data, error_count, 10);
|
||||
last_best_val = val;
|
||||
}
|
||||
|
||||
int best_idx = 0;
|
||||
float *best_data = group->get_best(timestamp, &best_idx);
|
||||
assert(last_best_val == best_data[0]);
|
||||
assert(best_idx == val1_idx);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Dynamically add a validator to the group after construction
|
||||
* @param group
|
||||
* @return
|
||||
*/
|
||||
DataValidator *add_validator_to_group(DataValidatorGroup *group)
|
||||
{
|
||||
DataValidator *validator = group->add_new_validator();
|
||||
//verify the previously set timeout applies to the new group member
|
||||
assert(validator->get_timeout() == base_timeout_usec);
|
||||
//for testing purposes, ensure this newly added member is consistent with the rest of the group
|
||||
//TODO this is likely a bug in DataValidatorGroup
|
||||
validator->set_equal_value_threshold(equal_value_count);
|
||||
|
||||
return validator;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a DataValidatorGroup and tack on two additional DataValidators
|
||||
*
|
||||
* @param validator1_handle (out) first DataValidator added to the group
|
||||
* @param validator2_handle (out) second DataValidator added to the group
|
||||
* @param sibling_count (in/out) in: number of initial siblings to create, out: total
|
||||
* @return
|
||||
*/
|
||||
DataValidatorGroup *setup_group_with_two_validator_handles(
|
||||
DataValidator **validator1_handle,
|
||||
DataValidator **validator2_handle,
|
||||
unsigned *sibling_count)
|
||||
{
|
||||
DataValidatorGroup *group = setup_base_group(sibling_count);
|
||||
|
||||
//now we add validators
|
||||
*validator1_handle = add_validator_to_group(group);
|
||||
*validator2_handle = add_validator_to_group(group);
|
||||
*sibling_count += 2;
|
||||
|
||||
return group;
|
||||
}
|
||||
|
||||
|
||||
void test_init()
|
||||
{
|
||||
unsigned num_siblings = 0;
|
||||
|
||||
DataValidatorGroup *group = setup_base_group(&num_siblings);
|
||||
|
||||
//should not yet be any best value
|
||||
int best_index = -1;
|
||||
assert(nullptr == group->get_best(base_timestamp, &best_index));
|
||||
|
||||
delete group; //force cleanup
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Happy path test of put method -- ensure the "best" sensor selected is the one with highest priority
|
||||
*/
|
||||
void test_put()
|
||||
{
|
||||
unsigned num_siblings = 0;
|
||||
DataValidator *validator1 = nullptr;
|
||||
DataValidator *validator2 = nullptr;
|
||||
|
||||
uint64_t timestamp = base_timestamp;
|
||||
|
||||
DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
|
||||
printf("num_siblings: %d \n", num_siblings);
|
||||
unsigned val1_idx = num_siblings - 2;
|
||||
unsigned val2_idx = num_siblings - 1;
|
||||
|
||||
fill_two_with_valid_data(group, val1_idx, val2_idx, 500);
|
||||
int best_idx = -1;
|
||||
float *best_data = group->get_best(timestamp, &best_idx);
|
||||
assert(nullptr != best_data);
|
||||
float best_val = best_data[0];
|
||||
|
||||
float *cur_val1 = validator1->value();
|
||||
assert(nullptr != cur_val1);
|
||||
//printf("cur_val1 %p \n", cur_val1);
|
||||
assert(best_val == cur_val1[0]);
|
||||
|
||||
float *cur_val2 = validator2->value();
|
||||
assert(nullptr != cur_val2);
|
||||
//printf("cur_val12 %p \n", cur_val2);
|
||||
assert(best_val == cur_val2[0]);
|
||||
|
||||
delete group; //force cleanup
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best".
|
||||
*/
|
||||
void test_priority_switch()
|
||||
{
|
||||
unsigned num_siblings = 0;
|
||||
DataValidator *validator1 = nullptr;
|
||||
DataValidator *validator2 = nullptr;
|
||||
|
||||
uint64_t timestamp = base_timestamp;
|
||||
|
||||
DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
|
||||
//printf("num_siblings: %d \n",num_siblings);
|
||||
int val1_idx = (int)num_siblings - 2;
|
||||
int val2_idx = (int)num_siblings - 1;
|
||||
uint64_t error_count = 0;
|
||||
|
||||
fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
|
||||
|
||||
int best_idx = -1;
|
||||
float *best_data = nullptr;
|
||||
//now, switch the priorities, which switches "best" but doesn't trigger a failover
|
||||
float new_best_val = 3.14159f;
|
||||
float data[DataValidator::dimensions] = {new_best_val};
|
||||
//a single sample insertion should be sufficient to trigger a priority switch
|
||||
group->put(val1_idx, timestamp, data, error_count, 1);
|
||||
group->put(val2_idx, timestamp, data, error_count, 100);
|
||||
best_data = group->get_best(timestamp, &best_idx);
|
||||
assert(new_best_val == best_data[0]);
|
||||
//the new best sensor should now be the sensor with the higher priority
|
||||
assert(best_idx == val2_idx);
|
||||
//should not have detected a real failover
|
||||
assert(0 == group->failover_count());
|
||||
|
||||
delete group; //cleanup
|
||||
}
|
||||
|
||||
/**
|
||||
* Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors
|
||||
*/
|
||||
void test_simple_failover()
|
||||
{
|
||||
unsigned num_siblings = 0;
|
||||
DataValidator *validator1 = nullptr;
|
||||
DataValidator *validator2 = nullptr;
|
||||
|
||||
uint64_t timestamp = base_timestamp;
|
||||
|
||||
DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
|
||||
//printf("num_siblings: %d \n",num_siblings);
|
||||
int val1_idx = (int)num_siblings - 2;
|
||||
int val2_idx = (int)num_siblings - 1;
|
||||
|
||||
|
||||
fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
|
||||
|
||||
int best_idx = -1;
|
||||
float *best_data = nullptr;
|
||||
|
||||
//trigger a real failover
|
||||
float new_best_val = 3.14159f;
|
||||
float data[DataValidator::dimensions] = {new_best_val};
|
||||
//trigger a bunch of errors on the previous best sensor
|
||||
unsigned val1_err_count = 0;
|
||||
|
||||
for (int i = 0; i < 25; i++) {
|
||||
group->put(val1_idx, timestamp, data, ++val1_err_count, 100);
|
||||
group->put(val2_idx, timestamp, data, 0, 10);
|
||||
}
|
||||
|
||||
assert(validator1->error_count() == val1_err_count);
|
||||
|
||||
//since validator1 is experiencing errors, we should see a failover to validator2
|
||||
best_data = group->get_best(timestamp + 1, &best_idx);
|
||||
assert(nullptr != best_data);
|
||||
assert(new_best_val == best_data[0]);
|
||||
assert(best_idx == val2_idx);
|
||||
//should have detected a real failover
|
||||
printf("failover_count: %d \n", group->failover_count());
|
||||
assert(1 == group->failover_count());
|
||||
|
||||
//even though validator1 has encountered a bunch of errors, it hasn't failed
|
||||
assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state());
|
||||
|
||||
// although we failed over from one sensor to another, this is not the same thing tracked by failover_index
|
||||
int fail_idx = group->failover_index();
|
||||
assert(-1 == fail_idx);//no failed sensor
|
||||
|
||||
//since no sensor has actually hard-failed, the group failover state is NO_ERROR
|
||||
assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state());
|
||||
|
||||
|
||||
delete group; //cleanup
|
||||
}
|
||||
|
||||
/**
|
||||
* Force once sensor to fail and ensure that we detect it
|
||||
*/
|
||||
void test_sensor_failure()
|
||||
{
|
||||
unsigned num_siblings = 0;
|
||||
uint64_t timestamp = base_timestamp;
|
||||
const float sufficient_incr_value = (1.1f * 1E-6f);
|
||||
const uint32_t timeout_usec = 2000;//derived from class-private value
|
||||
|
||||
float val = 3.14159f;
|
||||
|
||||
DataValidatorGroup *group = setup_base_group(&num_siblings);
|
||||
|
||||
//now we add validators
|
||||
DataValidator *validator = add_validator_to_group(group);
|
||||
assert(nullptr != validator);
|
||||
num_siblings++;
|
||||
int val_idx = num_siblings - 1;
|
||||
|
||||
fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp);
|
||||
//the best should now be the one validator we've filled with samples
|
||||
|
||||
int best_idx = -1;
|
||||
float *best_data = group->get_best(timestamp, &best_idx);
|
||||
assert(nullptr != best_data);
|
||||
//printf("best_idx: %d val_idx: %d\n", best_idx, val_idx);
|
||||
assert(best_idx == val_idx);
|
||||
|
||||
//now force a timeout failure in the one validator, by checking confidence long past timeout
|
||||
validator->confidence(timestamp + (1.1 * timeout_usec));
|
||||
assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state()));
|
||||
|
||||
//now that the one sensor has failed, the group should detect this as well
|
||||
int fail_idx = group->failover_index();
|
||||
assert(val_idx == fail_idx);
|
||||
|
||||
delete group;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
(void)argc; // unused
|
||||
(void)argv; // unused
|
||||
|
||||
test_init();
|
||||
test_put();
|
||||
test_simple_failover();
|
||||
test_priority_switch();
|
||||
test_sensor_failure();
|
||||
|
||||
return 0; //passed
|
||||
}
|
||||
@@ -0,0 +1,103 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include "tests_common.h"
|
||||
|
||||
|
||||
void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err,
|
||||
uint64_t *timestamp_io)
|
||||
{
|
||||
uint64_t timestamp = *timestamp_io;
|
||||
uint64_t timestamp_incr = 5;
|
||||
const uint64_t error_count = 0;
|
||||
const int priority = 50;
|
||||
const float swing = 1E-2f;
|
||||
double sum_dev_squares = 0.0f;
|
||||
|
||||
//insert a series of values that swing around the mean
|
||||
for (uint32_t i = 0; i < count; i++) {
|
||||
float iter_swing = (0 == (i % 2)) ? swing : -swing;
|
||||
float iter_val = mean + iter_swing;
|
||||
float iter_dev = iter_val - mean;
|
||||
sum_dev_squares += (iter_dev * iter_dev);
|
||||
timestamp += timestamp_incr;
|
||||
validator->put(timestamp, iter_val, error_count, priority);
|
||||
}
|
||||
|
||||
double rms = sqrt(sum_dev_squares / (double)count);
|
||||
//note: this should be approximately equal to "swing"
|
||||
*rms_err = (float)rms;
|
||||
*timestamp_io = timestamp;
|
||||
}
|
||||
|
||||
void dump_validator_state(DataValidator *validator)
|
||||
{
|
||||
uint32_t state = validator->state();
|
||||
printf("state: 0x%x no_data: %d stale: %d timeout:%d\n",
|
||||
validator->state(),
|
||||
DataValidator::ERROR_FLAG_NO_DATA & state,
|
||||
DataValidator::ERROR_FLAG_STALE_DATA & state,
|
||||
DataValidator::ERROR_FLAG_TIMEOUT & state
|
||||
);
|
||||
validator->print();
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
void fill_validator_with_samples(DataValidator *validator,
|
||||
const float incr_value,
|
||||
float *value_io,
|
||||
uint64_t *timestamp_io)
|
||||
{
|
||||
uint64_t timestamp = *timestamp_io;
|
||||
const uint64_t timestamp_incr = 5; //usec
|
||||
const uint32_t timeout_usec = 2000;//derived from class-private value
|
||||
|
||||
float val = *value_io;
|
||||
const uint64_t error_count = 0;
|
||||
const int priority = 50; //"medium" priority
|
||||
const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
|
||||
|
||||
validator->set_equal_value_threshold(equal_value_count);
|
||||
validator->set_timeout(timeout_usec);
|
||||
|
||||
//put a bunch of values that are all different
|
||||
for (int i = 0; i < equal_value_count; i++, val += incr_value) {
|
||||
timestamp += timestamp_incr;
|
||||
validator->put(timestamp, val, error_count, priority);
|
||||
}
|
||||
|
||||
*timestamp_io = timestamp;
|
||||
*value_io = val;
|
||||
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef ECL_TESTS_COMMON_H
|
||||
#define ECL_TESTS_COMMON_H
|
||||
|
||||
#include <validation/data_validator.h>
|
||||
|
||||
/**
|
||||
* Insert a series of samples around a mean value
|
||||
* @param validator The validator under test
|
||||
* @param mean The mean value
|
||||
* @param count The number of samples to insert in the validator
|
||||
* @param rms_err (out) calculated rms error of the inserted samples
|
||||
* @param timestamp_io (in/out) in: start timestamp, out: last timestamp
|
||||
*/
|
||||
void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err,
|
||||
uint64_t *timestamp_io);
|
||||
|
||||
/**
|
||||
* Print out the state of a DataValidator
|
||||
* @param validator
|
||||
*/
|
||||
void dump_validator_state(DataValidator *validator);
|
||||
|
||||
/**
|
||||
* Insert a time series of samples into the validator
|
||||
* @param validator
|
||||
* @param incr_value The amount to increment the value by on each iteration
|
||||
* @param value_io (in/out) in: initial value, out: final value
|
||||
* @param timestamp_io (in/out) in: initial timestamp, out: final timestamp
|
||||
*/
|
||||
void fill_validator_with_samples(DataValidator *validator,
|
||||
const float incr_value,
|
||||
float *value_io,
|
||||
uint64_t *timestamp_io);
|
||||
|
||||
#endif //ECL_TESTS_COMMON_H
|
||||
@@ -33,7 +33,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/ecl/validation/data_validator_group.h>
|
||||
#include "data_validator/DataValidatorGroup.hpp"
|
||||
|
||||
#include <lib/mathlib/math/Limits.hpp>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
@@ -41,14 +41,15 @@
|
||||
|
||||
#include "parameters.h"
|
||||
|
||||
#include "data_validator/DataValidator.hpp"
|
||||
#include "data_validator/DataValidatorGroup.hpp"
|
||||
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#include <lib/ecl/validation/data_validator.h>
|
||||
#include <lib/ecl/validation/data_validator_group.h>
|
||||
#include <lib/mag_compensation/MagCompensation.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
Reference in New Issue
Block a user