PX4多冗余传感器代码解读
如有疏漏，请指正先看这个文件里实现了各种传感器的冗余处理，以气压计为例介绍，其他的都差不多。代码如下void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata){bool got_update = false;float *offsets[] = {&_corrections.baro_offset_0, &_
如有疏漏，请指正
先看
这个文件里实现了各种传感器的冗余处理，以气压计为例介绍，其他的都差不多。代码如下
void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
{
bool got_update = false;
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 };
float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 };
for (int uorb_index = 0; uorb_index < _baro.subscription_count; uorb_index++) {
bool baro_updated;
orb_check(_baro.subscription[uorb_index], &baro_updated);
if (baro_updated) {
sensor_baro_s baro_report;
int ret = orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
if (ret != PX4_OK || baro_report.timestamp == 0) {
continue; //ignore invalid data
}
// Convert from millibar to Pa
float corrected_pressure = 100.0f * baro_report.pressure;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// First publication with data
if (_baro.priority[uorb_index] == 0) {
int32_t priority = 0;
orb_priority(_baro.subscription[uorb_index], &priority);
_baro.priority[uorb_index] = (uint8_t)priority;
}
_baro_device_id[uorb_index] = baro_report.device_id;
got_update = true;
float vect[3] = {baro_report.pressure, baro_report.temperature, 0.f};
_last_airdata[uorb_index].timestamp = baro_report.timestamp;
_last_airdata[uorb_index].baro_temp_celcius = baro_report.temperature;
_last_airdata[uorb_index].baro_pressure_pa = corrected_pressure;
_baro.voter.put(uorb_index, baro_report.timestamp, vect, baro_report.error_count, _baro.priority[uorb_index]);
}
}
if (got_update) {
int best_index;
_baro.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
airdata = _last_airdata[best_index];
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
_corrections.selected_baro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.baro_device_id != _baro_device_id[best_index]) {
_selection_changed = true;
_selection.baro_device_id = _baro_device_id[best_index];
}
// calculate altitude using the hypsometric equation
static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; /* temperature at base height in Kelvin */
static constexpr float a = -6.5f / 1000.0f; /* temperature gradient in degrees per metre */
/* current pressure at MSL in kPa (QNH in hPa)*/
const float p1 = _parameters.baro_qnh * 0.1f;
/* measured pressure in kPa */
const float p = airdata.baro_pressure_pa * 0.001f;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
airdata.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
// calculate air density
// estimate air density assuming typical 20degC ambient temperature
// TODO: use air temperature if available (differential pressure sensors)
static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
airdata.rho = pressure_to_density * airdata.baro_pressure_pa;
}
}
}
首先，第一个循环 for (int uorb_index = 0; uorb_index < _baro.subscription_count; uorb_index++)
把每个可用的气压计的数据进行订阅
int ret = orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
然后将数据简单处理后赋值到相应的GROUP中
_baro.voter.put(uorb_index, baro_report.timestamp, vect, baro_report.error_count, _baro.priority[uorb_index]);
PX4里管理多冗余传感器用的是一种类似链表的数据结构，这里称它为GROUP。先跳到put这个函数里看一下
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++;
}
}
首先定义了一个类指针next指向GROUP中的第一个节点_first，然后定义一个下标i,然后while循环遍历GROUP中的节点，遍历的方法与链表类似
next = next->sibling();
这句就是把next指针指向GROUP中下一个节点的地址
每个节点其实就是一个气压计。当i和需要赋值的节点下标一样时，用put将相应的节点赋值,跳到put里看一下
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;
}
}
_vibe[i] = _vibe[i] * 0.99f + 0.01f * fabsf(val[i] - _lp[i]);
// 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;
}
这里主要是计算了实时的错误密度_error_density，这个在后面计算自信值的时候会用到，以及做了一些低通滤波和振动值的计算。
再回到
在对各个节点进行赋值后，就是从这些节点里选择最优的数据，代码如下
if (got_update) {
int best_index;
_baro.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
airdata = _last_airdata[best_index];
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
_corrections.selected_baro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.baro_device_id != _baro_device_id[best_index]) {
_selection_changed = true;
_selection.baro_device_id = _baro_device_id[best_index];
}
// calculate altitude using the hypsometric equation
static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; /* temperature at base height in Kelvin */
static constexpr float a = -6.5f / 1000.0f; /* temperature gradient in degrees per metre */
/* current pressure at MSL in kPa (QNH in hPa)*/
const float p1 = _parameters.baro_qnh * 0.1f;
/* measured pressure in kPa */
const float p = airdata.baro_pressure_pa * 0.001f;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
airdata.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
// calculate air density
// estimate air density assuming typical 20degC ambient temperature
// TODO: use air temperature if available (differential pressure sensors)
static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
airdata.rho = pressure_to_density * airdata.baro_pressure_pa;
}
通过
_baro.voter.get_best(hrt_absolute_time(), &best_index);这个函数来获取最优的传感器的uorb下标。
跳到这个函数
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;
}
首先指向第一个节点DataValidator *next = _first;
在while (next != nullptr) {这个循环里遍历所以节点，并计算每个节点的自信值float confidence = next->confidence(timestamp);并据此判断最优的传感器
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;
}
然后进行失效判断，最后通过参数返回最优传感器的uorb下标max_index
*index = max_index;
这里调到计算自信值的函数里看一下
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;
}
前面是一些错误判断以及错误密度抗饱和，没问题的话就根据错误密度_error_density计算confidence。
公式如下。
ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);、
_error_density是在上面put函数里根据传感器的_error_count计算的，ERROR_DENSITY_WINDOW是常数100.
再回到
_baro.voter.get_best(hrt_absolute_time(), &best_index);
获取到best_index后
airdata = _last_airdata[best_index];
const float p = airdata.baro_pressure_pa * 0.001f;
根据best_index来计算气压值。
到这里大概的流程就结束了。
再跟大家讲一下GROUP是怎么建立的，其实跟链表是一样的，只不过链表里的节点是类，而不是结构体。如下
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();
}
}
先声明下一个节点next和上一个节点prev
for循环里先实例化next,如果i等于0，把next赋给头结点，否则把next节点的地址赋给prev节点的Sibling成员，然后将prev节点向后移动，这样就能一直挂载节点，最后将尾节点赋值。
技术共进，成长同行——讯飞AI开发者社区
更多推荐
0
0- 0
已为社区贡献3条内容
所有评论(0)