commander: sensor calibration check parameter save success

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 */
+		if (!failed) {
-		px4_usleep(200000);
+			calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
-		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
+			return PX4_OK;
-		px4_usleep(600000); /* give this message enough time to propagate */
+		}
-		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();
+				} else {
-			calibration.PrintStatus();
+					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;
 	}
 

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
-		/* set offset parameters to new values */
+		bool param_save = false;
-		bool failed = 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) {
+		if (param_save) {
-			calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params");
+			param_notify_changes();
-			res = PX4_ERROR;
+		}
+
+		if (!failed) {
+			calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
+			return PX4_OK;
 		}
 	}
 
-	param_notify_changes();
+	calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
-
+	return PX4_ERROR;
-	/* 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;
 }

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 &current_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
+		if (param_save) {
-		//  sensor calibration and will be invalidated by a new sensor calibration
+			// reset the learned EKF mag in-flight bias offsets which have been learned for the previous
-		for (int axis = 0; axis < 3; axis++) {
+			//  sensor calibration and will be invalidated by a new sensor calibration
-			char axis_char = 'X' + axis;
+			for (int axis = 0; axis < 3; axis++) {
-			char str[20] {};
+				char axis_char = 'X' + axis;
-			sprintf(str, "EKF2_MAGBIAS_%c", axis_char);
+				char str[20] {};
-			float offset = 0.f;
+				sprintf(str, "EKF2_MAGBIAS_%c", axis_char);
-			param_set_no_notification(param_find(str), &offset);
+				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");
+		if (!failed) {
-		return calibrate_return_ok;
+			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;
 }