commander: mag calibration add simple offset only quick cal using GPS & attitude

.ci/Jenkinsfile-hardware

@@ -842,6 +842,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate accel quick; sleep 2; param show CAL_ACC*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate mag quick; sleep 2; param show CAL_MAG*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "df"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"'
@@ -909,6 +910,7 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate accel quick; sleep 2; param show CAL_ACC*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate mag quick; sleep 2; param show CAL_MAG*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "df -h"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"'

src/modules/commander/Commander.cpp

@@ -257,8 +257,14 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f);
 
 			} else if (!strcmp(argv[2], "mag")) {
-				// magnetometer calibration: param2 = 1
-				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f);
+				if (argc > 3 && (strcmp(argv[3], "quick") == 0)) {
+					// magnetometer quick calibration: param2 = 2
+					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 2.f, 0.f, 0.f, 0.f, 0.f, 0.f);
+
+				} else {
+					// magnetometer calibration: param2 = 1
+					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f);
+				}
 
 			} else if (!strcmp(argv[2], "accel")) {
 				if (argc > 3 && (strcmp(argv[3], "quick") == 0)) {
@@ -3507,6 +3513,11 @@ void *commander_low_prio_loop(void *arg)
 							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
 							calib_ret = do_mag_calibration(&mavlink_log_pub);
 
+						} else if ((int)(cmd.param2) == 2) {
+							/* magnetometer calibration quick (requires GPS & attitude) */
+							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
+							calib_ret = do_mag_calibration_quick(&mavlink_log_pub);
+
 						} else if ((int)(cmd.param3) == 1) {
 							/* zero-altitude pressure calibration */
 							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);

src/modules/commander/mag_calibration.cpp

@@ -59,6 +59,7 @@
 #include <uORB/SubscriptionBlocking.hpp>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 using namespace matrix;
@@ -887,3 +888,85 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 	return result;
 }
+
+int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians)
+{
+	// magnetometer quick calibration
+	//  if GPS available use world magnetic model to zero mag offsets
+
+	Vector3f mag_earth_pred{};
+	bool mag_earth_available = false;
+
+	uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	vehicle_gps_position_s gps{};
+
+	if (vehicle_gps_position_sub.copy(&gps)) {
+		if (gps.eph < 1000) {
+			const double lat = gps.lat / 1.e7;
+			const double lon = gps.lon / 1.e7;
+
+			// magnetic field data returned by the geo library using the current GPS position
+			const float mag_declination_gps = get_mag_declination_radians(lat, lon);
+			const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
+			const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
+
+			mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);
+
+			mag_earth_available = true;
+		}
+	}
+
+	if (!mag_earth_available) {
+		calibration_log_critical(mavlink_log_pub, "GPS required for mag quick cal");
+		return calibrate_return_error;
+
+	} else {
+
+		uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+		vehicle_attitude_s attitude{};
+		vehicle_attitude_sub.copy(&attitude);
+
+		if (hrt_elapsed_time(&attitude.timestamp) > 1_s) {
+			calibration_log_critical(mavlink_log_pub, "attitude required for mag quick cal");
+			return calibrate_return_error;
+		}
+
+		calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees",
+					 (double)math::radians(heading_radians));
+
+		matrix::Eulerf euler{matrix::Quatf{attitude.q}};
+		euler(2) = heading_radians;
+
+		const Vector3f expected_field = Dcmf(euler).transpose() * mag_earth_pred;
+
+		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{};
+			mag_sub.copy(&mag);
+
+			if (mag_sub.advertised() && (hrt_elapsed_time(&mag.timestamp) < 1_s)) {
+
+				calibration::Magnetometer cal{mag.device_id, mag.is_external};
+
+				// force calibration index to uORB index
+				cal.set_calibration_index(cur_mag);
+
+				// use any existing scale and store the offset to the expected earth field
+				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();
+			}
+		}
+
+		param_notify_changes();
+
+		calibration_log_info(mavlink_log_pub, "Mag quick calibration finished");
+		return calibrate_return_ok;
+	}
+
+	return calibrate_return_error;
+}

src/modules/commander/mag_calibration.h

@@ -43,5 +43,6 @@
 #include <uORB/uORB.h>
 
 int do_mag_calibration(orb_advert_t *mavlink_log_pub);
+int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians = 0);
 
 #endif /* MAG_CALIBRATION_H_ */