From d7611cac898c854950347f2ccd700b3822ac7109 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 24 May 2017 00:15:24 +0200 Subject: [PATCH] commander : make mag calibration correctly lock-in to corresponding uORB topic --- src/modules/commander/mag_calibration.cpp | 54 +++++++++++++++++------ 1 file changed, 41 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 3742e8fd27..00cbced00a 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -581,26 +581,54 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) if (result == calibrate_return_ok) { // 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)); + const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag)); // Warn that we will not calibrate more than max_mags magnetometers - if (mag_count > max_mags) { - calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", mag_count, max_mags); + if (orb_mag_count > max_mags) { + calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", orb_mag_count, max_mags); } - for (unsigned cur_mag = 0; cur_mag < mag_count && cur_mag < max_mags; cur_mag++) { - // Mag in this slot is available - worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); + for (unsigned cur_mag = 0; cur_mag < orb_mag_count && cur_mag < max_mags; cur_mag++) { + + // Lock in to correct ORB instance + bool found_cur_mag = false; + for(unsigned i = 0; i < orb_mag_count; i++) { + worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); + + if (worker_data.sub_mag[cur_mag] < 0) { + calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag); + result = calibrate_return_error; + break; + } + + struct mag_report report; + orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report); + +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) + + // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL + // and match it up with the one from the uORB subscription, because the + // instance ordering of uORB and the order of the FDs may not be the same. + + if(report.device_id == device_ids[cur_mag]) { + // Device IDs match, correct ORB instance for this mag + found_cur_mag = true; + break; + } else { + orb_unsubscribe(worker_data.sub_mag[cur_mag]); + } + +#elif defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) + + // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. + device_ids[cur_mag] = report.device_id; + found_cur_mag = true; -#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) - // 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) { - calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag); + if(!found_cur_mag) { + calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u (ID %u) no matching uORB devid", cur_mag, device_ids[cur_mag]); result = calibrate_return_error; break; }