fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 09:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4 style fixes (excluding EKF generated code)

Browse Source
This commit is contained in:
Daniel Agar
2018-05-03 14:14:12 -04:00
parent e5952fadaf
commit c5b2f59f57
22 changed files with 384 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
validation/data_validator_group.cpp
+33 -18
View File
@@ -56,17 +56,20 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
for (unsigned i = 0; i < siblings; i++) {
next = new DataValidator();
if(i == 0) {
if (i == 0) {
_first = next;
} else {
prev->setSibling(next);
}
prev = next;
}
_last = next;
if(_first) {
if (_first) {
_timeout_interval_us = _first->get_timeout();
}
}
@@ -74,7 +77,7 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
DataValidatorGroup::~DataValidatorGroup()
{
while (_first) {
DataValidator* next = _first->sibling();
DataValidator *next = _first->sibling();
delete (_first);
_first = next;
}
@@ -83,9 +86,11 @@ DataValidatorGroup::~DataValidatorGroup()
DataValidator *DataValidatorGroup::add_new_validator()
{
DataValidator *validator = new DataValidator();
if (!validator) {
return nullptr;
}
_last->setSibling(validator);
_last = validator;
_last->set_timeout(_timeout_interval_us);
@@ -101,6 +106,7 @@ DataValidatorGroup::set_timeout(uint32_t timeout_interval_us)
next->set_timeout(timeout_interval_us);
next = next->sibling();
}
_timeout_interval_us = timeout_interval_us;
}
@@ -127,12 +133,13 @@ DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64
next->put(timestamp, val, error_count, priority);
break;
}
next = next->sibling();
i++;
}
}
float*
float *
DataValidatorGroup::get_best(uint64_t timestamp, int *index)
{
DataValidator *next = _first;
@@ -162,9 +169,10 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
* 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)) {
(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();
@@ -183,7 +191,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* 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) {
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 */
@@ -193,6 +201,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* 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;
@@ -210,6 +219,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* 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;
}
@@ -225,7 +235,7 @@ DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float* rms = next->rms();
float *rms = next->rms();
for (unsigned j = 0; j < 3; j++) {
if (rms[j] > vibe) {
@@ -251,7 +261,7 @@ DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis)
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float* vibration_offset = next->vibration_offset();
float *vibration_offset = next->vibration_offset();
if (vibe < 0.0f || vibration_offset[axis] < vibe) {
vibe = vibration_offset[axis];
@@ -269,8 +279,8 @@ DataValidatorGroup::print()
{
/* print the group's state */
ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
DataValidator *next = _first;
unsigned i = 0;
@@ -280,15 +290,16 @@ DataValidatorGroup::print()
uint32_t flags = next->state();
ECL_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" : ""));
((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++;
}
@@ -310,9 +321,11 @@ DataValidatorGroup::failover_index()
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
return i;
}
next = next->sibling();
i++;
}
return -1;
}
@@ -326,8 +339,10 @@ DataValidatorGroup::failover_state()
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;
}