commander: fix the calibration for NuttX.

This fixes some regressions on the calibration using NuttX, especially
considering the PRIME param and the device ids.

src/modules/commander/accelerometer_calibration.cpp

@@ -410,25 +410,39 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 
 	uint64_t timestamps[max_accel_sens];
 
-	for (unsigned i = 0; i < max_accel_sens; i++) {
+	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
+	const unsigned accel_count = orb_group_count(ORB_ID(sensor_accel));
+	for (unsigned i = 0; i < accel_count; i++) {
 		worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
 		if (worker_data.subs[i] < 0) {
 			result = calibrate_return_error;
 			break;
 		}
 
+#ifdef __PX4_QURT
+		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
+		struct accel_report	accel_report;
+		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
+		device_id[i] = accel_report.device_id;
+#endif
 		/* store initial timestamp - used to infer which sensors are active */
 		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(worker_data.subs[i], &prio);
+		if (device_id[i] != 0) {
+			// Get priority
+			int32_t prio;
+			orb_priority(worker_data.subs[i], &prio);
 
-		if (prio > device_prio_max) {
-			device_prio_max = prio;
-			device_id_primary = device_id[i];
+			if (prio > device_prio_max) {
+				device_prio_max = prio;
+				device_id_primary = device_id[i];
+			}
+		} else {
+			mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", i);
+			result = calibrate_return_error;
+			break;
 		}
 	}
 

src/modules/commander/gyro_calibration.cpp

@@ -173,8 +173,10 @@ int do_gyro_calibration(int mavlink_fd)
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
 
-		// Reset gyro ids to unavailable
+		// Reset gyro ids to unavailable.
 		worker_data.device_id[s] = 0;
+		// And set default subscriber values.
+		worker_data.gyro_sensor_sub[s] = -1;
 		(void)sprintf(str, "CAL_GYRO%u_ID", s);
 		res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
 		if (res != OK) {
@@ -232,16 +234,29 @@ int do_gyro_calibration(int mavlink_fd)
 
 	}
 
-	for (unsigned s = 0; s < max_gyros; s++) {
+	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
+	const unsigned gyro_count = orb_group_count(ORB_ID(sensor_gyro));
+	for (unsigned s = 0; s < gyro_count; s++) {
+
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+#ifdef __PX4_QURT
+		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
+		struct gyro_report	gyro_report;
+		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
+		worker_data.device_id[s] = gyro_report.device_id;
+#endif
 
-		// Get priority
-		int32_t prio;
-		orb_priority(worker_data.gyro_sensor_sub[s], &prio);
+		if (worker_data.device_id[s] != 0) {
+			// Get priority
+			int32_t prio;
+			orb_priority(worker_data.gyro_sensor_sub[s], &prio);
 
-		if (prio > device_prio_max) {
-			device_prio_max = prio;
-			device_id_primary = worker_data.device_id[s];
+			if (prio > device_prio_max) {
+				device_prio_max = prio;
+				device_id_primary = worker_data.device_id[s];
+			}
+		} else {
+			mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Gyro #%u no device id, abort", s);
 		}
 	}
 

src/modules/commander/mag_calibration.cpp

@@ -468,16 +468,27 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 
 	// Setup subscriptions to mag sensors
 	if (result == calibrate_return_ok) {
-		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
-			if (device_ids[cur_mag] != 0) {
-				// Mag in this slot is available
-				worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
-				if (worker_data.sub_mag[cur_mag] < 0) {
-					mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
-					result = calibrate_return_error;
-					break;
-				}
 
+		// We should not try to subscribe if the topic doesn't actually exist and can be counted.
+		const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
+
+		for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
+			// Mag in this slot is available
+			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
+
+#ifdef __PX4_QURT
+			// For QURT respectively the driver framework, we need to get the device ID by copying one report.
+			struct mag_report	mag_report;
+			orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
+			device_ids[cur_mag] = mag_report.device_id;
+#endif
+			if (worker_data.sub_mag[cur_mag] < 0) {
+				mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
+				result = calibrate_return_error;
+				break;
+			}
+
+			if (device_ids[cur_mag] != 0) {
 				// Get priority
 				int32_t prio;
 				orb_priority(worker_data.sub_mag[cur_mag], &prio);
@@ -486,6 +497,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					device_prio_max = prio;
 					device_id_primary = device_ids[cur_mag];
 				}
+			} else {
+				mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
+				result = calibrate_return_error;
+				break;
 			}
 		}
 	}
@@ -696,9 +711,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
 									     cur_mag,
 									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
+#ifndef __PX4_QURT
 						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
 									     cur_mag,
 									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
+#endif
 					}
 				}
 			}