diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7e26f63fe9..5d33164397 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -340,10 +340,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) if (active_sensors == 0) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); return PX4_ERROR; - - } else if (active_sensors > MAX_ACCEL_SENS) { - calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", active_sensors, - MAX_ACCEL_SENS); } /* measure and calculate offsets & scales */ @@ -357,6 +353,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) const Dcmf board_rotation = calibration::GetBoardRotation(); const Dcmf board_rotation_t = board_rotation.transpose(); + bool param_save = false; + bool failed = true; + for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) { if (i < active_sensors) { // calculate offsets @@ -410,16 +409,25 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) } // save all calibrations including empty slots - calibrations[i].ParametersSave(); + if (calibrations[i].ParametersSave()) { + param_save = true; + failed = false; + + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } - param_notify_changes(); + if (param_save) { + param_notify_changes(); + } - /* if there is a any preflight-check system response, let the barrage of messages through */ - px4_usleep(200000); - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); - px4_usleep(600000); /* give this message enough time to propagate */ - return PX4_OK; + if (!failed) { + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); + return PX4_OK; + } } calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); @@ -431,7 +439,8 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) #if !defined(CONSTRAINED_FLASH) PX4_INFO("Accelerometer quick calibration"); - bool success = false; + bool param_save = false; + bool failed = true; // sensor thermal corrections (optional) uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; @@ -536,16 +545,25 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) } else { calibration.set_offset(offset); - success = true; - } + if (calibration.ParametersSave()) { + calibration.PrintStatus(); + param_save = true; + failed = false; - calibration.ParametersSave(); - calibration.PrintStatus(); + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "calibration save failed"); + break; + } + } } } - if (success) { + if (param_save) { param_notify_changes(); + } + + if (!failed) { return PX4_OK; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8df491efe5..dd52c1e19c 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -286,9 +286,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } if (res == PX4_OK) { - - /* set offset parameters to new values */ - bool failed = false; + // set offset parameters to new values + bool param_save = false; + bool failed = true; for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) { @@ -304,29 +304,28 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } calibration.set_calibration_index(uorb_index); - calibration.ParametersSave(); + + if (calibration.ParametersSave()) { + param_save = true; + failed = false; + + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } - if (failed) { - calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params"); - res = PX4_ERROR; + if (param_save) { + param_notify_changes(); + } + + if (!failed) { + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); + return PX4_OK; } } - param_notify_changes(); - - /* if there is a any preflight-check system response, let the barrage of messages through */ - px4_usleep(200000); - - if (res == PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); - - } else { - calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); - } - - /* give this message enough time to propagate */ - px4_usleep(600000); - - return res; + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + return PX4_ERROR; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index f8f38ad961..76619baf4b 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -857,6 +857,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } if (result == calibrate_return_ok) { + bool param_save = false; + bool failed = true; + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { auto ¤t_cal = worker_data.calibration[cur_mag]; @@ -882,20 +885,35 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } current_cal.set_calibration_index(cur_mag); - current_cal.ParametersSave(); + + if (current_cal.ParametersSave()) { + param_save = true; + failed = false; + + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } - // reset the learned EKF mag in-flight bias offsets which have been learned for the previous - // sensor calibration and will be invalidated by a new sensor calibration - for (int axis = 0; axis < 3; axis++) { - char axis_char = 'X' + axis; - char str[20] {}; - sprintf(str, "EKF2_MAGBIAS_%c", axis_char); - float offset = 0.f; - param_set_no_notification(param_find(str), &offset); + if (param_save) { + // reset the learned EKF mag in-flight bias offsets which have been learned for the previous + // sensor calibration and will be invalidated by a new sensor calibration + for (int axis = 0; axis < 3; axis++) { + char axis_char = 'X' + axis; + char str[20] {}; + sprintf(str, "EKF2_MAGBIAS_%c", axis_char); + float offset = 0.f; + param_set_no_notification(param_find(str), &offset); + } + + param_notify_changes(); } - param_notify_changes(); + if (failed) { + result = calibrate_return_error; + } } return result; @@ -925,7 +943,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian if (!mag_earth_available) { calibration_log_critical(mavlink_log_pub, "GPS required for mag quick cal"); - return calibrate_return_error; + return PX4_ERROR; } else { @@ -943,7 +961,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian if (hrt_elapsed_time(&attitude.timestamp) > 1_s) { calibration_log_critical(mavlink_log_pub, "attitude required for mag quick cal"); - return calibrate_return_error; + return PX4_ERROR; } calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees", @@ -954,6 +972,9 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian const Vector3f expected_field = Dcmf(euler).transpose() * mag_earth_pred; + bool param_save = false; + bool failed = true; + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { uORB::Subscription mag_sub{ORB_ID(sensor_mag), cur_mag}; sensor_mag_s mag{}; @@ -970,18 +991,30 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field); cal.set_offset(offset); - cal.PrintStatus(); - // save new calibration - cal.ParametersSave(); + if (cal.ParametersSave()) { + cal.PrintStatus(); + param_save = true; + failed = false; + + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } } - param_notify_changes(); + if (param_save) { + param_notify_changes(); + } - calibration_log_info(mavlink_log_pub, "Mag quick calibration finished"); - return calibrate_return_ok; + if (!failed) { + calibration_log_info(mavlink_log_pub, "Mag quick calibration finished"); + return PX4_OK; + } } - return calibrate_return_error; + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + return PX4_ERROR; }