From 302e2372cf05b9f2a9216884c0d7ad4d174033c1 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 24 May 2017 10:10:32 +0200 Subject: [PATCH] mpu9250 : fill correct device ID into uORB topic --- src/drivers/mpu9250/mag.cpp | 2 +- src/modules/commander/mag_calibration.cpp | 11 ++--------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp index ca22481252..a27c23aa66 100644 --- a/src/drivers/mpu9250/mag.cpp +++ b/src/drivers/mpu9250/mag.cpp @@ -227,7 +227,7 @@ MPU9250_mag::_measure(struct ak8963_regs data) mrb.range_ga = 48.0f; mrb.scaling = _mag_range_scale; mrb.temperature = _parent->_last_temperature; - mrb.device_id = _parent->get_device_id().devid; + mrb.device_id = _parent->_mag->_device_id.devid; mrb.error_count = perf_event_count(_mag_errors); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 00cbced00a..a3873e51bd 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -592,14 +592,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) // 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; - } + for(unsigned i = 0; i < orb_mag_count && !found_cur_mag; i++) { + worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), i); struct mag_report report; orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report); @@ -613,7 +607,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) 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]); }