diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index c0a0503a12..1e372d7a2e 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -223,7 +223,6 @@ void VotedSensorsUpdate::parameters_update() /* set offset parameters to new values */ bool failed; char str[30]; - unsigned mag_count = 0; unsigned gyro_count = 0; unsigned accel_count = 0; unsigned gyro_cal_found_count = 0; @@ -393,20 +392,46 @@ void VotedSensorsUpdate::parameters_update() } } - /* run through all mag sensors */ - for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; driver_index++) { + /* run through all mag sensors + * Because we store the device id in _mag_device_id, we need to get the id via uorb topic since + * the DevHandle method does not work on POSIX. + */ + for (unsigned topic_instance = 0; topic_instance < MAG_COUNT_MAX && topic_instance < _mag.subscription_count; + ++topic_instance) { - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index); + struct mag_report report; - DevHandle h; - DevMgr::getHandle(str, h); - - if (!h.isValid()) { - /* the driver is not running, abort */ + if (orb_copy(ORB_ID(sensor_mag), _mag.subscription[topic_instance], &report) != 0) { continue; } - _mag_device_id[driver_index] = h.ioctl(DEVIOCGDEVICEID, 0); + int topic_device_id = report.device_id; + _mag_device_id[topic_instance] = topic_device_id; + + // find the driver handle that matches the topic_device_id + DevHandle h; + + for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) { + + (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index); + + DevMgr::getHandle(str, h); + + if (!h.isValid()) { + /* the driver is not running, continue with the next */ + continue; + } + + int driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); + + if (driver_device_id == topic_device_id) { + break; // we found the matching driver + + } else { + DevMgr::releaseHandle(h); + } + } + bool config_ok = false; /* run through all stored calibrations */ @@ -423,7 +448,7 @@ void VotedSensorsUpdate::parameters_update() } /* if the calibration is for this device, apply it */ - if (device_id == _mag_device_id[driver_index]) { + if (device_id == _mag_device_id[topic_instance]) { struct mag_calibration_s mscale = {}; (void)sprintf(str, "CAL_MAG%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &mscale.x_offset)); @@ -440,26 +465,28 @@ void VotedSensorsUpdate::parameters_update() (void)sprintf(str, "CAL_MAG%u_ROT", i); - if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) { - /* mag is internal - reset param to -1 to indicate internal mag */ - int32_t minus_one; - param_get(param_find(str), &minus_one); + if (h.isValid()) { //FIXME: get the 'is external' information via uorb topic + if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) { + /* mag is internal - reset param to -1 to indicate internal mag */ + int32_t minus_one; + param_get(param_find(str), &minus_one); - if (minus_one != MAG_ROT_VAL_INTERNAL) { - minus_one = MAG_ROT_VAL_INTERNAL; - param_set_no_notification(param_find(str), &minus_one); - } + if (minus_one != MAG_ROT_VAL_INTERNAL) { + minus_one = MAG_ROT_VAL_INTERNAL; + param_set_no_notification(param_find(str), &minus_one); + } - } else { - /* mag is external */ - int32_t mag_rot; - param_get(param_find(str), &mag_rot); + } else { + /* mag is external */ + int32_t mag_rot; + param_get(param_find(str), &mag_rot); - /* check if this mag is still set as internal, otherwise leave untouched */ - if (mag_rot < 0) { - /* it was marked as internal, change to external with no rotation */ - mag_rot = 0; - param_set_no_notification(param_find(str), &mag_rot); + /* check if this mag is still set as internal, otherwise leave untouched */ + if (mag_rot < 0) { + /* it was marked as internal, change to external with no rotation */ + mag_rot = 0; + param_set_no_notification(param_find(str), &mag_rot); + } } } @@ -479,10 +506,6 @@ void VotedSensorsUpdate::parameters_update() break; } } - - if (config_ok) { - mag_count++; - } } } @@ -1017,13 +1040,17 @@ VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const struct accel_cal bool VotedSensorsUpdate::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id) { -#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX) + + if (!h.isValid()) { + return false; + } /* On most systems, we can just use the IOCTL call to set the calibration params. */ return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal); #else - /* On QURT, the params are read directly in the respective wrappers. */ + /* On QURT & POSIX, the params are read directly in the respective wrappers. */ return true; #endif }