From 88a812618c1259f3b662597f8bca1d4eb63821af Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Mar 2016 16:58:30 +0100 Subject: [PATCH] commander: fix the calibration for NuttX. This fixes some regressions on the calibration using NuttX, especially considering the PRIME param and the device ids. --- .../commander/accelerometer_calibration.cpp | 28 +++++++++++---- src/modules/commander/gyro_calibration.cpp | 31 +++++++++++----- src/modules/commander/mag_calibration.cpp | 35 ++++++++++++++----- 3 files changed, 70 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f6ce64f387..63ef33046c 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/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; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index a98e021e2e..2bc1bafa2b 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/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); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 39f9a01f03..5588f8c4c3 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/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