PX4多冗余传感器代码解读
如有疏漏，请指正
先看
这个文件里实现了各种传感器的冗余处理，以气压计为例介绍，其他的都差不多。代码如下
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 Pafloat corrected_pressure = 100.0f * baro_report.pressure;// handle temperature compensationif (_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 dataif (_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 equationstatic 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 equationstatic 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节点向后移动，这样就能一直挂载节点，最后将尾节点赋值。
PX4多冗余传感器代码解读相关推荐
- softmax理论及代码解读——UFLDL
前言 看了各种softmax以后迷迷糊糊的,还是研究一下UFLDL的教程稳点.当然还是得参考挺多教程的:UFLDL-softmax .Softmax的理解与应用 .Logistic 分类器与 soft ...
- VGAE（Variational graph auto-encoders）论文及代码解读
一,论文来源 论文pdf Variational graph auto-encoders 论文代码 github代码 二,论文解读 理论部分参考: Variational Graph Auto-Enc ...
- 鱼眼图像自监督深度估计原理分析和Omnidet核心代码解读
作者丨苹果姐@知乎 来源丨https://zhuanlan.zhihu.com/p/508090405 编辑丨3D视觉工坊 在自动驾驶实际应用中,对相机传感器的要求之一是拥有尽可能大的视野范围,鱼眼相 ...
- STM32学习心得十八：通用定时器基本原理及相关实验代码解读
记录一下,方便以后翻阅~ 主要内容: 1) 三种定时器分类及区别: 2) 通用定时器特点: 3) 通用定时器工作过程: 4) 实验一:定时器中断实验补充知识及部代码解读: 6) 实验二:定时器PWM输 ...
- 一文详解单目VINS论文与代码解读目录
本文旨在对前一阶段学习vins-mono开源框架的总结.结合暑假秋招之前报名的深蓝学院的<从零开始手写VIO>课程,本文从VIO原理以及开源代码分析两部分进行详细介绍.PS:提升代码能力最 ...
- EMANet：Expectation-Maximization Attention Networks for Semantic Segmentation论文解读和代码解读
官方项目地址:含论文和代码 来自北大才子 立夏之光的 ICCV Oral ,理论很漂亮.属于Non local方式 Updated on 2020.1.6 最近一直有人私信我,这个attention ...
- 关于iic协议和对AT24C02进行读写数据的理解和代码解读
关于iic协议和对AT24C02进行读写数据的理解和代码解读 认识IIC协议 IIC协议软件模拟方法 管脚初始化 时序 AT24C02 简介 存储大小计算 工作方式 流程(代码) 认识IIC协议 本文 ...
- 2021-05-24匿名凌霄飞控代码解读Drv_AnoOf.c和LX_FC_EXT_Sensor.c
5.24:匿名凌霄飞控代码解读Drv_AnoOf.c和LX_FC_EXT_Sensor.c 注意:两部分的代码结合读,先看懂AnoOf.c然后再看对光流数据处理的文件Sensor.c 1.Drv_An ...
- 200行代码解读TDEngine背后的定时器
作者 | beyondma来源 | CSDN博客 导读:最近几周,本文作者几篇有关陶建辉老师最新的创业项目-TdEngine代码解读文章出人意料地引起了巨大的反响,原以为C语言已经是昨日黄花,不过从读 ...
最新文章
- DevOps是软件研发的洗碗机
- Compiled functions can't take variable number of arguments or use keyword-only arguments with defaul
- ES transport client使用
- 适合python的笔记本配置-jupyter之配置自己喜欢的python环境
- 怎么快速搜索服务器里的文件,Windows中快速搜索指定文件夹
- C++实现选择排序(附完整源码)
- 踩坑记(1)——使用slf4j+logback记录日志
- html的页面宽高变形问题,CSS＋div 设计的网页在不同浏览器中变形的问题
- 方差，协方差 、统计学的基本概念
- 使用iframe的一些经验
- 【爬虫】Python爬虫实战，Python实现中国地铁数据可视化arcmap
- 项目管理ppt_案例 | 华为内部项目管理PPT
- 有道云笔记怎么保存html,有道云笔记怎么保存网页？有道云笔记保存路径是什么...
- 浅析WLAN——无线局域网
- 华为首次自曝“天才少年”成果：入职不到一年算法研究用于千万台 Mate/P 系列手机，网友：值 200 万年薪
- DBG、DMB、DSB 和 ISB
- 无internet访问权限解决办法
- C++实现求小于n的所有素数
- python数字图像处理笔记10 图像旋转
- PST文件格式解读--PST文件的逻辑结构