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,18 +410,27 @@ 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;
 
+		if (device_id[i] != 0) {
 			// Get priority
 			int32_t prio;
 			orb_priority(worker_data.subs[i], &prio);
@@ -430,6 +439,11 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 				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;
+		}
 	}
 
 	if (result == calibrate_return_ok) {

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,9 +234,19 @@ 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
 
+		if (worker_data.device_id[s] != 0) {
 			// Get priority
 			int32_t prio;
 			orb_priority(worker_data.gyro_sensor_sub[s], &prio);
@@ -243,6 +255,9 @@ int do_gyro_calibration(int mavlink_fd)
 				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);
+		}
 	}
 
 	int cancel_sub  = calibrate_cancel_subscribe();

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) {
+		// 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
 					}
 				}
 			}