中国室内设计联盟官方网站入口,实时seo排名点击软件,三门峡网站设计,网站建设专业工资文章目录 前言一、parametersUpdate二、imuPoll三、 put四、 confidence五、 get_best 前言
PX4 1.13.2 一个人可以走的更快#xff0c;一群人才能走的更远#xff0c;可加文章底部微信名片
代码的位置如下 PX4冗余机制主要通过传感读数错误计数和传感器的优先级进行选优 … 文章目录 前言一、parametersUpdate二、imuPoll三、 put四、 confidence五、 get_best 前言
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::SubscriptionDatavehicle_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_castint32_t(1),static_castint32_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_castint32_t(1),static_castint32_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) {// dont 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 - thats 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是目前最优传感器的confidencemax_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 were 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;
}
