diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index d013eb9e73..f68e9c807b 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -178,10 +178,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } - /* save */ calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); - param_save_default(true); - feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; } @@ -201,6 +198,14 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) } if (hrt_elapsed_time(&calibration_started) > 90_s) { + 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); + return PX4_ERROR; + } + + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; } @@ -209,6 +214,14 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) } if (calibration_counter == maxcount) { + 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); + return PX4_ERROR; + } + + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; }