dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

data validator: Move implementations to CPP files

This commit is contained in:
Lorenz Meier 2015-08-30 18:47:21 +02:00
parent ddb3cde1d7
commit 8da3f1f8f9
5 changed files with 330 additions and 249 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/ecl/module.mk
View File

@ -39,6 +39,8 @@ SRCS = attitude_fw/ecl_controller.cpp \
attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp \
l1/ecl_l1_pos_controller.cpp
l1/ecl_l1_pos_controller.cpp \
validation/data_validator.cpp \
validation/data_validator_group.cpp
MAXOPTIMIZATION = -Os

135
src/lib/ecl/validation/data_validator.cpp Normal file
View File

@ -0,0 +1,135 @@
DataValidator::DataValidator(DataValidator *prev_sibling) :
_time_last(0),
_timeout_interval(70000),
_event_count(0),
_error_count(0),
_mean{0.0f},
_lp{0.0f},
_M2{0.0f},
_rms{0.0f},
_value{0.0f},
_value_equal_count(0),
_sibling(prev_sibling)
{
}
DataValidator::~DataValidator()
{
}
void
DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
{
_event_count++;
_error_count = error_count_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.5f + val[i] * 0.5f;
_value[i] = val[i];
}
_time_last = timestamp;
}
/****************************************************************************
*
* 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.c
*
* A data validation class to identify anomalies in data streams
*
* @author Lorenz Meier <lorenz@px4.io>
*/
float
DataValidator::confidence(uint64_t timestamp)
{
/* check if we have any data */
if (_time_last == 0) {
return 0.0f;
}
/* check error count limit */
if (_error_count > NORETURN_ERRCOUNT) {
return 0.0f;
}
/* we got the exact same sensor value N times in a row */
if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
return 0.0f;
}
/* timed out - that's it */
if (timestamp - _time_last > _timeout_interval) {
return 0.0f;
}
return 1.0f;
}
void
DataValidator::print()
{
if (_time_last == 0) {
printf("\tno data\n");
return;
}
for (unsigned i = 0; i < _dimensions; i++) {
printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
}
}

96
src/lib/ecl/validation/data_validator.h
View File

@ -119,99 +119,3 @@ private:
DataValidator(const DataValidator&);
DataValidator operator=(const DataValidator&);
};
DataValidator::DataValidator(DataValidator *prev_sibling) :
_time_last(0),
_timeout_interval(70000),
_event_count(0),
_error_count(0),
_mean{0.0f},
_lp{0.0f},
_M2{0.0f},
_rms{0.0f},
_value{0.0f},
_value_equal_count(0),
_sibling(prev_sibling)
{
}
DataValidator::~DataValidator()
{
}
void
DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
{
_event_count++;
_error_count = error_count_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.5f + val[i] * 0.5f;
_value[i] = val[i];
}
_time_last = timestamp;
}
float
DataValidator::confidence(uint64_t timestamp)
{
/* check if we have any data */
if (_time_last == 0) {
return 0.0f;
}
/* check error count limit */
if (_error_count > NORETURN_ERRCOUNT) {
return 0.0f;
}
/* we got the exact same sensor value N times in a row */
if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
return 0.0f;
}
/* timed out - that's it */
if (timestamp - _time_last > _timeout_interval) {
return 0.0f;
}
return 1.0f;
}
void
DataValidator::print()
{
if (_time_last == 0) {
printf("\tno data\n");
return;
}
for (unsigned i = 0; i < _dimensions; i++) {
printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
}
}

192
src/lib/ecl/validation/data_validator_group.cpp Normal file
View File

@ -0,0 +1,192 @@
/****************************************************************************
*
* 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_group.cpp
*
* A data validation group to identify anomalies in data streams
*
* @author Lorenz Meier <lorenz@px4.io>
*/
DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
_first(nullptr),
_curr_best(-1),
_prev_best(-1),
_first_failover_time(0),
_toggle_count(0)
{
DataValidator *next = _first;
for (unsigned i = 0; i < siblings; i++) {
next = new DataValidator(next);
}
_first = next;
}
DataValidatorGroup::~DataValidatorGroup()
{
}
void
DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
{
DataValidator *next = _first;
while (next != nullptr) {
next->set_timeout(timeout_interval_us);
next = next->sibling();
}
}
void
DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count)
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (i == index) {
next->put(timestamp, val, error_count);
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 max_confidence = -1.0f;
int max_index = -1;
uint64_t min_error_count = 30000;
DataValidator *best = nullptr;
unsigned i = 0;
while (next != nullptr) {
float confidence = next->confidence(timestamp);
if (confidence > max_confidence ||
(fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) {
max_index = i;
max_confidence = confidence;
min_error_count = next->error_count();
best = next;
}
next = next->sibling();
i++;
}
/* the current best sensor is not matching the previous best sensor */
if (max_index != _curr_best) {
/* 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;
_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;
}
float
DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
{
DataValidator *next = _first;
float vibe = 0.0f;
/* find the best RMS value of a non-timed out sensor */
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float* rms = next->rms();
for (unsigned j = 0; j < 3; j++) {
if (rms[j] > vibe) {
vibe = rms[j];
}
}
}
next = next->sibling();
}
return vibe;
}
void
DataValidatorGroup::print()
{
/* print the group's state */
warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
printf("sensor #%u:\n", i);
next->print();
next = next->sibling();
i++;
}
}
unsigned
DataValidatorGroup::failover_count()
{
return _toggle_count;
}

152
src/lib/ecl/validation/data_validator_group.h
View File

@ -103,155 +103,3 @@ private:
DataValidatorGroup(const DataValidatorGroup&);
DataValidatorGroup operator=(const DataValidatorGroup&);
};
DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
_first(nullptr),
_curr_best(-1),
_prev_best(-1),
_first_failover_time(0),
_toggle_count(0)
{
DataValidator *next = _first;
for (unsigned i = 0; i < siblings; i++) {
next = new DataValidator(next);
}
_first = next;
}
DataValidatorGroup::~DataValidatorGroup()
{
}
void
DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
{
DataValidator *next = _first;
while (next != nullptr) {
next->set_timeout(timeout_interval_us);
next = next->sibling();
}
}
void
DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count)
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (i == index) {
next->put(timestamp, val, error_count);
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 max_confidence = -1.0f;
int max_index = -1;
uint64_t min_error_count = 30000;
DataValidator *best = nullptr;
unsigned i = 0;
while (next != nullptr) {
float confidence = next->confidence(timestamp);
if (confidence > max_confidence ||
(fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) {
max_index = i;
max_confidence = confidence;
min_error_count = next->error_count();
best = next;
}
next = next->sibling();
i++;
}
/* the current best sensor is not matching the previous best sensor */
if (max_index != _curr_best) {
/* 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;
_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;
}
float
DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
{
DataValidator *next = _first;
float vibe = 0.0f;
/* find the best RMS value of a non-timed out sensor */
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float* rms = next->rms();
for (unsigned j = 0; j < 3; j++) {
if (rms[j] > vibe) {
vibe = rms[j];
}
}
}
next = next->sibling();
}
return vibe;
}
void
DataValidatorGroup::print()
{
/* print the group's state */
warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
printf("sensor #%u:\n", i);
next->print();
next = next->sibling();
i++;
}
}
unsigned
DataValidatorGroup::failover_count()
{
return _toggle_count;
}