前言
PX4 1.13.2
一个人可以走的更快，一群人才能走的更远，可加文章底部微信名片
代码的位置如下
PX4冗余机制主要通过传感读数错误计数和传感器的优先级进行选优
一、parametersUpdate
这个函数主要是初始化每个imu传感器的优先级，PX4传感器在经过校准后，会给每个同类的传感器生成一个优先级，这个优先级在冗余机制中有着重要的作用
void VotedSensorsUpdate::parametersUpdate()
{
_parameter_update = true;
updateParams();
// run through all IMUs
for (uint8_t uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
imu.update();
if (imu.advertised() && (imu.get().timestamp != 0)
&& (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) {
// find corresponding configured accel priority
int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);
if (accel_cal_index >= 0) {
// found matching CAL_ACCx_PRIO
int32_t accel_priority_old = _accel.priority_configured[uorb_index];
_accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index);
if (accel_priority_old != _accel.priority_configured[uorb_index]) {
if (_accel.priority_configured[uorb_index] == 0) {
// disabled
_accel.priority[uorb_index] = 0;
} else {
// change relative priority to incorporate any sensor faults
int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old;
_accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, static_cast<int32_t>(1),
static_cast<int32_t>(100));
}
}
}
// find corresponding configured gyro priority
int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id);
if (gyro_cal_index >= 0) {
// found matching CAL_GYROx_PRIO
int32_t gyro_priority_old = _gyro.priority_configured[uorb_index];
_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index);
if (gyro_priority_old != _gyro.priority_configured[uorb_index]) {
if (_gyro.priority_configured[uorb_index] == 0) {
// disabled
_gyro.priority[uorb_index] = 0;
} else {
// change relative priority to incorporate any sensor faults
int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old;
_gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, static_cast<int32_t>(1),
static_cast<int32_t>(100));
}
}
}
}
}
}
二、imuPoll
这个函数里首先是对传感器的数据进行循环订阅，然后赋值到_last_sensor_data中，通过put方法将数据放入链表中进行处理。PX4通过单向链表DataValidator对传感器的数据进行存储和处理，put函数调用了DataValidator的put函数，在里面计算了数据的均方根误差还有错误密度，然后通过错误密度计算出每个传感器的confidence，根据confidence和优先级，通过get_best得出目前的最优传感器，然后把最优传感器的数据通过形参raw返回，这个raw最后会通过sensor_combine话题发布。
void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
{
const hrt_abstime time_now_us = hrt_absolute_time();
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
vehicle_imu_s imu_report;
if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0)
&& _vehicle_imu_sub[uorb_index].update(&imu_report)) {
// copy corresponding vehicle_imu_status for accel & gyro error counts
vehicle_imu_status_s imu_status{};
_vehicle_imu_status_subs[uorb_index].copy(&imu_status);
_accel_device_id[uorb_index] = imu_report.accel_device_id;
_gyro_device_id[uorb_index] = imu_report.gyro_device_id;
// convert the delta velocities to an equivalent acceleration
const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt;
Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv;
// convert the delta angles to an equivalent angular rate
const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt;
Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv;
_last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample;
_last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0);
_last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1);
_last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2);
_last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt;
_last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping;
_last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0);
_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
_last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count;
_last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count;
_last_accel_timestamp[uorb_index] = imu_report.timestamp_sample;
_accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
imu_status.accel_error_count, _accel.priority[uorb_index]);
_gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad,
imu_status.gyro_error_count, _gyro.priority[uorb_index]);
}
}
// find the best sensor
int accel_best_index = _accel.last_best_vote;
int gyro_best_index = _gyro.last_best_vote;
if (!_parameter_update) {
// update current accel/gyro selection, skipped on cycles where parameters update
_accel.voter.get_best(time_now_us, &accel_best_index);
_gyro.voter.get_best(time_now_us, &gyro_best_index);
if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
// use sensor_selection to find best
if (_sensor_selection_sub.update(&_selection)) {
// reset inconsistency checks against primary
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
_gyro_diff[sensor_index].zero();
}
}
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
accel_best_index = i;
}
if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
gyro_best_index = i;
}
}
} else {
// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
}
}
// write data for the best sensor to output variables
if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0)
&& (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) {
raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
sizeof(raw.accelerometer_m_s2));
memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad));
raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt;
raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping;
raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count;
raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count;
if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
_accel.last_best_vote = (uint8_t)accel_best_index;
_selection.accel_device_id = _accel_device_id[accel_best_index];
_selection_changed = true;
}
if ((_gyro.last_best_vote != gyro_best_index) || (_selection.gyro_device_id != _gyro_device_id[gyro_best_index])) {
_gyro.last_best_vote = (uint8_t)gyro_best_index;
_selection.gyro_device_id = _gyro_device_id[gyro_best_index];
_selection_changed = true;
// clear all registered callbacks
for (auto &sub : _vehicle_imu_sub) {
sub.unregisterCallback();
}
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
vehicle_imu_s report{};
if (_vehicle_imu_sub[i].copy(&report)) {
if ((report.gyro_device_id != 0) && (report.gyro_device_id == _gyro_device_id[gyro_best_index])) {
_vehicle_imu_sub[i].registerCallback();
}
}
}
}
}
// publish sensor selection if changed
if (_param_sens_imu_mode.get() || (_selection.timestamp == 0)) {
if (_selection_changed) {
// don't publish until selected IDs are valid
if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) {
_selection.timestamp = hrt_absolute_time();
_sensor_selection_pub.publish(_selection);
_selection_changed = false;
}
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
_gyro_diff[sensor_index].zero();
}
}
}
}
三、 put
这个函数计算了错误密度_error_density，这个将在计算confidence时用到，这个_error_density取决于_error_count，而_error_count实在传感器驱动部分赋值的，也就是说这里的错误计数是根据数据的读取错误来确定的，而数据本身的对错是不关注的，个人觉得这个地方还需要改进，例如气压计被堵住导致数据不准，应该加一些这方面的判断。
除了_error_density的计算，还计算了均方根误差_rms
void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_t error_count_in, uint8_t 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 (PX4_ISFINITE(val[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.99f + 0.01f * val[i];
_value[i] = val[i];
}
}
_time_last = timestamp;
}
四、 confidence
前面是一些错误判断以及错误密度抗饱和，没问题的话就根据错误密度_error_density计算confidence。
公式如下。
ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);、
_error_density是在上面put函数里根据传感器的_error_count计算的，ERROR_DENSITY_WINDOW是常数100.
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;
}
五、 get_best
这个函数就是根据confidence和传感器优先级来确定最优的传感器，判断如下，max_confidence是目前最优传感器的confidence，max_priority是目前最优的传感器的优先级，confidence是当前的传感器的confidence，根据这两个confidence 还有优先级确定当前传感器是否要取代最优传感器。
可以看到，((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >=
MIN_REGULAR_CONFIDENCE)) ，这个判断一般是在目前最优传感器失效的情况下才会触发，所以这个判断是没有考虑优先级的，这很好理解，级别你优先级再高，如果你失效了，我只能往低优先级的传感器切换。实际上这个条件一般不会触发，一个稳定的硬件很少会出现传感器损坏的情况。
大多数时候会在后面两个判断里面进行判断，只有在优先级比目前最优传感器高或者相等的情况下，才有可能取代目前的最优传感器，否则即使confidence高也没用，因此，我们可以手动的给一些质量好的传感器设置高的优先级。否则的话，飞控是有可能一直在使用低质量的传感器的（即便精度较差）
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)) {
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 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;
}