mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-07 01:04:19 +08:00
sensor calibration: remove param_save_default() calls
This commit is contained in:
@@ -406,13 +406,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
||||
}
|
||||
|
||||
if (res == PX4_OK) {
|
||||
/* auto-save to EEPROM */
|
||||
res = param_save_default();
|
||||
|
||||
if (res != PX4_OK) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
|
||||
}
|
||||
|
||||
/* if there is a any preflight-check system response, let the barrage of messages through */
|
||||
usleep(200000);
|
||||
|
||||
|
||||
@@ -182,15 +182,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
||||
goto error_return;
|
||||
}
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
|
||||
goto error_return;
|
||||
}
|
||||
|
||||
} else {
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
goto error_return;
|
||||
|
||||
@@ -62,6 +62,5 @@
|
||||
#define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u"
|
||||
#define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration, sensor %u"
|
||||
#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters, sensor %u"
|
||||
#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] calibration failed: failed to save parameters"
|
||||
|
||||
#endif /* CALIBRATION_MESSAGES_H_ */
|
||||
|
||||
@@ -4244,17 +4244,10 @@ void *commander_low_prio_loop(void *arg)
|
||||
|
||||
/* reset parameters and save empty file */
|
||||
param_reset_all();
|
||||
int ret = param_save_default();
|
||||
|
||||
if (ret == OK) {
|
||||
/* do not spam MAVLink, but provide the answer / green led mechanism */
|
||||
mavlink_log_critical(&mavlink_log_pub, "onboard parameters reset");
|
||||
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(&mavlink_log_pub, "param reset error");
|
||||
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack);
|
||||
}
|
||||
/* do not spam MAVLink, but provide the answer / green led mechanism */
|
||||
mavlink_log_critical(&mavlink_log_pub, "onboard parameters reset");
|
||||
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
@@ -456,15 +456,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
||||
/* store last 32bit number - not unique, but unique in a given set */
|
||||
(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[PX4_CPU_UUID_WORD32_UNIQUE_H]);
|
||||
|
||||
if (res == PX4_OK) {
|
||||
/* auto-save to EEPROM */
|
||||
res = param_save_default();
|
||||
|
||||
if (res != PX4_OK) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
/* if there is a any preflight-check system response, let the barrage of messages through */
|
||||
usleep(200000);
|
||||
|
||||
|
||||
@@ -220,25 +220,14 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
|
||||
break;
|
||||
|
||||
case calibrate_return_ok:
|
||||
/* auto-save to EEPROM */
|
||||
result = param_save_default();
|
||||
|
||||
/* if there is a any preflight-check system response, let the barrage of messages through */
|
||||
usleep(200000);
|
||||
|
||||
if (result == PX4_OK) {
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
|
||||
usleep(20000);
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
|
||||
usleep(20000);
|
||||
break;
|
||||
|
||||
} else {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
|
||||
usleep(20000);
|
||||
}
|
||||
|
||||
// Fall through
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
|
||||
usleep(20000);
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
|
||||
usleep(20000);
|
||||
break;
|
||||
|
||||
default:
|
||||
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
|
||||
|
||||
@@ -96,11 +96,7 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
|
||||
p = sp.r * yaw_scale + yaw_trim_active;
|
||||
int p3r = param_set(param_find("TRIM_YAW"), &p);
|
||||
|
||||
/* store to permanent storage */
|
||||
/* auto-save */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
|
||||
if (p1r != 0 || p2r != 0 || p3r != 0) {
|
||||
mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
|
||||
px4_close(sub_man);
|
||||
return PX4_ERROR;
|
||||
|
||||
Reference in New Issue
Block a user