fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 07:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mpu9250 : fill correct device ID into uORB topic

Browse Source
This commit is contained in:
Mohammed Kabir
2017-05-27 00:31:05 +02:00
committed by Lorenz Meier
parent d7611cac89
commit 302e2372cf
2 changed files with 3 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+2 -9
View File
@@ -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]);
}