Store primary sensor ID to allow cross-check of calibration on next boot

src/modules/commander/PreflightCheck.cpp

@@ -110,7 +110,7 @@ int check_calibration(int fd, const char* param_template)
 	return !calibration_found;
 }
 
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -338,50 +338,107 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 	/* ---- MAG ---- */
 	if (checkMag) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_mag_count; i++) {
 			bool required = (i < max_mandatory_mag_count);
+			int device_id;
 
-			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
+			if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found) {
+			//failed = true;
 		}
 	}
 
 	/* ---- ACCEL ---- */
 	if (checkAcc) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_accel_count; i++) {
 			bool required = (i < max_mandatory_accel_count);
+			int device_id
 
-			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found) {
+			//failed = true;
 		}
 	}
 
 	/* ---- GYRO ---- */
 	if (checkGyro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
 			bool required = (i < max_mandatory_gyro_count);
+			int device_id;
 
-			if (!gyroCheck(mavlink_fd, i, !required) && required) {
+			if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found) {
+			//failed = true;
 		}
 	}
 
 	/* ---- BARO ---- */
 	if (checkBaro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_baro_count; i++) {
 			bool required = (i < max_mandatory_baro_count);
+			int device_id;
 
-			if (!baroCheck(mavlink_fd, i, !required) && required) {
+			if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
 		}
+
+		// TODO there is no logic in place to calibrate the primary baro yet
+		// // check if the primary device is present 
+		// if (!prime_found) {
+		// 	failed = true;
+		// }
 	}
 
 	/* ---- AIRSPEED ---- */

src/modules/commander/accelerometer_calibration.cpp

@@ -173,6 +173,8 @@ int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
 	int32_t device_id[max_accel_sens];
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -259,6 +261,8 @@ int do_accel_calibration(int mavlink_fd)
 		
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification("CAL_ACC_PRIME", &(device_id_primary)));
+
 		/* set parameters */
 		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@@ -370,6 +374,14 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 		struct accel_report arp = {};
 		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
 		timestamps[i] = arp.timestamp;
+
+		// Get priority
+		int32_t prio = orb_priority(work_data.subs[i]);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = device_id[i];
+		}
 	}
 
 	if (result == calibrate_return_ok) {

src/modules/commander/gyro_calibration.cpp

@@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,	// z scale
 	};
 	
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
+
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
 		
@@ -199,6 +202,14 @@ int do_gyro_calibration(int mavlink_fd)
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+
+		// Get priority
+		int32_t prio = orb_priority(work_data.gyro_sensor_subs[s]);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = worker_data.device_id[s];
+		}
 	}
 
 	int cancel_sub  = calibrate_cancel_subscribe();
@@ -258,9 +269,12 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
+
 		/* set offset parameters to new values */
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification("CAL_GYRO_PRIME", &(device_id_primary)));
+
 		for (unsigned s = 0; s < max_gyros; s++) {
 			if (worker_data.device_id[s] != 0) {
 				char str[30];

src/modules/commander/mag_calibration.cpp

@@ -109,6 +109,8 @@ int do_mag_calibration(int mavlink_fd)
 	// Determine which mags are available and reset each
 
 	int32_t	device_ids[max_mags];
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
 	char str[30];
 
 	for (size_t i=0; i<max_mags; i++) {
@@ -434,6 +436,14 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					result = calibrate_return_error;
 					break;
 				}
+
+				// Get priority
+				int32_t prio = orb_priority(work_data.sub_mag[cur_mag]);
+
+				if (prio > device_prio_max) {
+					device_prio_max = prio;
+					device_id_primary = device_ids[cur_mag];
+				}
 			}
 		}
 	}
@@ -550,6 +560,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	}
 	
 	if (result == calibrate_return_ok) {
+
+		(void)param_set_no_notification("CAL_MAG_PRIME", &(device_id_primary));
+
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				int fd_mag = -1;