commander: quick mag cal with fixed heading use MAV_CMD_FIXED_MAG_CAL_YAW message

- use proper Mavlink MAV_CMD_FIXED_MAG_CAL_YAW command for initiating magnetometer quick cal
 - MAV_CMD_FIXED_MAG_CAL_YAW allows specifying the yaw and optionally latitude and longitude if the vehicle doesn't have GPS

mavlink/include/mavlink/v2.0

@@ -1 +1 @@
-Subproject commit 7d6be862bca8f733a854bfd6c97f1bb25cdb477c
+Subproject commit efc2a1fa7e1c7814ca7b65f484f6e9968cb8f1c8

msg/vehicle_command.msg

@@ -62,6 +62,7 @@ uint16 VEHICLE_CMD_DO_LAST = 240			# NOP - This command is only used to mark the
 uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241		# Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION
 uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3		# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration
 uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242	# Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|
+uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
@@ -70,14 +71,14 @@ uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum|
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530            # Set camera capture mode (photo, video, etc.)
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531                # Set camera zoom
 uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-board camera triggering system
+uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
+uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
+uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600	# control starting/stopping transmitting data over the high latency link
 uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    # Command VTOL transition
 uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001    # Request arm authorization
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
-uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
-uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
-uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
-uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600	# control starting/stopping transmitting data over the high latency link
+uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
 
 uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
 uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |

src/modules/commander/Commander.cpp

@@ -258,8 +258,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 
 			} else if (!strcmp(argv[2], "mag")) {
 				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);
+					// magnetometer quick calibration: VEHICLE_CMD_FIXED_MAG_CAL_YAW
+					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW);
 
 				} else {
 					// magnetometer calibration: param2 = 1
@@ -276,7 +276,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f);
 				}
 
-
 			} else if (!strcmp(argv[2], "level")) {
 				// board level calibration: param5 = 2
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f, 0.f);
@@ -1115,6 +1114,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
+	case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -3521,11 +3521,6 @@ 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);
@@ -3611,6 +3606,47 @@ void *commander_low_prio_loop(void *arg)
 					break;
 				}
 
+			case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
+					// Magnetometer quick calibration using world magnetic model and known heading
+					if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+					    || (status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
+					    || status_flags.condition_calibration_enabled) {
+
+						// reject if armed or shutting down
+						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
+
+					} else {
+						// parameter 1: Yaw in degrees
+						// parameter 3: Latitude
+						// parameter 4: Longitude
+
+						// assume vehicle pointing north (0 degrees) if heading isn't specified
+						const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f;
+
+						float latitude = NAN;
+						float longitude = NAN;
+
+						if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) {
+							// invalid if both lat & lon are 0 (current mavlink spec)
+							if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) {
+								latitude = cmd.param3;
+								longitude = cmd.param4;
+							}
+						}
+
+						if (do_mag_calibration_quick(&mavlink_log_pub, heading_radians, latitude, longitude) == PX4_OK) {
+							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
+							tune_positive(true);
+
+						} else {
+							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
+							tune_negative(true);
+						}
+					}
+
+					break;
+				}
+
 			case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
 					if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)

src/modules/commander/mag_calibration.cpp

@@ -901,30 +901,25 @@ 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)
+int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, float latitude, float longitude)
 {
 	// 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);
+	if (PX4_ISFINITE(latitude) && PX4_ISFINITE(longitude)) {
+		mag_earth_available = true;
 
-			mag_earth_available = true;
+	} else {
+		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.timestamp != 0) && (gps.eph < 1000)) {
+				latitude = gps.lat / 1.e7f;
+				longitude = gps.lon / 1.e7f;
+				mag_earth_available = true;
+			}
 		}
 	}
 
@@ -934,6 +929,14 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 
 	} else {
 
+		// magnetic field data returned by the geo library using the current GPS position
+		const float mag_declination_gps = get_mag_declination_radians(latitude, longitude);
+		const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude);
+		const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude);
+
+		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps,
+						0, 0);
+
 		uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 		vehicle_attitude_s attitude{};
 		vehicle_attitude_sub.copy(&attitude);

src/modules/commander/mag_calibration.h

@@ -39,10 +39,12 @@
 #ifndef MAG_CALIBRATION_H_
 #define MAG_CALIBRATION_H_
 
+#include <math.h>
 #include <stdint.h>
 #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);
+int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians = 0, float latitude = NAN,
+			     float longitude = NAN);
 
 #endif /* MAG_CALIBRATION_H_ */