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

voted_sensors_update: set priority properly in case of late mag subscriptions or sensors enabled param changes

Browse Source
This commit is contained in:
Dusan Zivkovic 2019-09-21 18:48:46 +02:00 committed by Beat Küng
parent 7f63ed8202
commit d3e79c4726
1 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/sensors/voted_sensors_update.cpp
View File

@ -260,6 +260,8 @@ void VotedSensorsUpdate::parametersUpdate()
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == driver_device_id) {
if (!_gyro.enabled[driver_index]) { _gyro.priority[driver_index] = 0; }
struct gyro_calibration_s gscale = {};
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@ -348,6 +350,8 @@ void VotedSensorsUpdate::parametersUpdate()
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == driver_device_id) {
if (!_accel.enabled[driver_index]) { _accel.priority[driver_index] = 0; }
struct accel_calibration_s ascale = {};
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &ascale.x_offset));
@ -460,6 +464,10 @@ void VotedSensorsUpdate::parametersUpdate()
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == _mag_device_id[topic_instance]) {
// the mags that were published after the initial parameterUpdate
// would be given the priority even if disabled. Reset it to 0 in this case
if (!_mag.enabled[topic_instance]) { _mag.priority[topic_instance] = 0; }
struct mag_calibration_s mscale = {};
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &mscale.x_offset));
@ -768,6 +776,15 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
/* force a scale and offset update the first time we get data */
parametersUpdate();
if (!_mag.enabled[uorb_index]) {
/* in case the data on the mag topic comes after the initial parameterUpdate(), we would get here since the sensor
* is enabled by default. The latest parameterUpdate() call would set enabled to false and reset priority to zero
* for disabled sensors, and we shouldn't cal voter.put() in that case
*/
continue;
}
}
Vector3f vect(mag_report.x, mag_report.y, mag_report.z);