fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

airspeed calibration simplify logic

Browse Source
This commit is contained in:
Daniel Agar
2017-07-06 10:05:07 -04:00
parent eb067291bf
commit 1c7833e3b1
1 changed files with 24 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/airspeed_calibration.cpp
+24 -25
View File
@@ -213,38 +213,37 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
if (diff_pres.differential_pressure_filtered_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
break;
} else {
/* do not allow negative values */
calibration_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
calibration_log_info(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
if (fabsf(diff_pres.differential_pressure_filtered_pa) < 50.0f) {
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true);
}
} else if (diff_pres.differential_pressure_filtered_pa < 0.0f) {
/* do not allow negative values */
calibration_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
calibration_log_info(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* save */
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
param_save_default();
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
/* save */
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
param_save_default();
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
} else {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
break;
}
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true);
}
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);