zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

data validator: Move implementations to CPP files

sbg
Lorenz Meier 10 years ago
parent
ddb3cde1d7
commit
8da3f1f8f9
5 changed files with 330 additions and 249 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      src/lib/ecl/module.mk
  2. 135
      src/lib/ecl/validation/data_validator.cpp
  3. 96
      src/lib/ecl/validation/data_validator.h
  4. 192
      src/lib/ecl/validation/data_validator_group.cpp
  5. 152
      src/lib/ecl/validation/data_validator_group.h

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

@ -39,6 +39,8 @@ SRCS = attitude_fw/ecl_controller.cpp \ @@ -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
Unescape View File

@ -0,0 +1,135 @@ @@ -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
Unescape View File

@ -119,99 +119,3 @@ private: @@ -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
Unescape View File

@ -0,0 +1,192 @@ @@ -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
Unescape View File

@ -103,155 +103,3 @@ private: @@ -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;
}

Write Preview
Loading…
Cancel
Save