PX4从放弃到精通（二十九）：传感器冗余机制

前言

PX4 1.13.2
一个人可以走的更快，一群人才能走的更远，可加文章底部微信名片

代码的位置如下
PX4冗余机制主要通过传感读数错误计数和传感器的优先级进行选优

一、parametersUpdate

这个函数主要是初始化每个imu传感器的优先级，PX4传感器在经过校准后，会给每个同类的传感器生成一个优先级，这个优先级在冗余机制中有着重要的作用

void VotedSensorsUpdate::parametersUpdate()
{_parameter_update = true;updateParams();// run through all IMUsfor (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 priorityint8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);if (accel_cal_index >= 0) {// found matching CAL_ACCx_PRIOint32_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 faultsint 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 priorityint8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id);if (gyro_cal_index >= 0) {// found matching CAL_GYROx_PRIOint32_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 faultsint 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 countsvehicle_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 accelerationconst 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 rateconst 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 sensorint 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 bestif (_sensor_selection_sub.update(&_selection)) {// reset inconsistency checks against primaryfor (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 publishedcheckFailover(_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 variablesif ((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 callbacksfor (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 changedif (_param_sens_imu_mode.get() || (_selection.timestamp == 0)) {if (_selection_changed) {// don't publish until selected IDs are validif (_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 votingint 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;
}