Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

voted_sensors_update: fix invalid/stale mag data on posix

Browse Source 
Problem: _mag_device_id is used to get the correct rotation matrix for each
mag. But on POSIX, _mag_device_id was always 0, leading to invalid rotation
matrices.
This resulted in stale mag error messages (rot matrix=0 ==> mag data=0).
_mag_device_id was 0 because there are no /dev/magX devices (eg. on RPi),
thus the mag driver could not be opened.

This patch does: get the device id from the uorb topic instead. We still
need the device handle on certain platforms to apply the calibration params
and to check if the mag is internal or external.

Problem left: on POSIX, the check for external mag does not work.
This commit is contained in:
Beat Küng
2017-06-14 08:35:20 +02:00
committed by Lorenz Meier
parent 76f593afb0
commit ba3d66abba
1 changed files with 61 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+61 -34
View File
@@ -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
}