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

voted_sensors_update: parameter CAL_*_EN enables sensor with id CAL_*_ID

Browse Source
This commit is contained in:
Dusan Zivkovic
2019-10-10 10:31:56 +02:00
committed by Beat Küng
parent d3e79c4726
commit 316a188e8e
1 changed files with 16 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+16 -6
View File
@@ -222,6 +222,7 @@ void VotedSensorsUpdate::parametersUpdate()
/* run through all gyro sensors */
for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) {
_gyro.enabled[driver_index] = true;
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index);
@@ -248,8 +249,6 @@ void VotedSensorsUpdate::parametersUpdate()
int32_t device_enabled = 1;
failed = failed || (OK != param_get(param_find(str), &device_enabled));
_gyro.enabled[i] = (device_enabled == 1);
if (failed) {
continue;
}
@@ -260,6 +259,8 @@ void VotedSensorsUpdate::parametersUpdate()
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == driver_device_id) {
_gyro.enabled[driver_index] = (device_enabled == 1);
if (!_gyro.enabled[driver_index]) { _gyro.priority[driver_index] = 0; }
struct gyro_calibration_s gscale = {};
@@ -312,6 +313,7 @@ void VotedSensorsUpdate::parametersUpdate()
/* run through all accel sensors */
for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) {
_accel.enabled[driver_index] = true;
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index);
@@ -338,8 +340,6 @@ void VotedSensorsUpdate::parametersUpdate()
int32_t device_enabled = 1;
failed = failed || (OK != param_get(param_find(str), &device_enabled));
_accel.enabled[i] = (device_enabled == 1);
if (failed) {
continue;
}
@@ -350,6 +350,8 @@ void VotedSensorsUpdate::parametersUpdate()
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == driver_device_id) {
_accel.enabled[driver_index] = (device_enabled == 1);
if (!_accel.enabled[driver_index]) { _accel.priority[driver_index] = 0; }
struct accel_calibration_s ascale = {};
@@ -404,6 +406,14 @@ void VotedSensorsUpdate::parametersUpdate()
* 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.
*/
/* first we have to reset all possible mags, since we are looping through the uORB instances instead of the drivers,
* and not all uORB instances have to be published yet at the initial call of parametersUpdate()
*/
for (int i = 0; i < MAG_COUNT_MAX; i++) {
_mag.enabled[i] = true;
}
for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX
&& topic_instance < _mag.subscription_count; ++topic_instance) {
@@ -456,14 +466,14 @@ void VotedSensorsUpdate::parametersUpdate()
int32_t device_enabled = 1;
failed = failed || (OK != param_get(param_find(str), &device_enabled));
_mag.enabled[i] = (device_enabled == 1);
if (failed) {
continue;
}
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == _mag_device_id[topic_instance]) {
_mag.enabled[topic_instance] = (device_enabled == 1);
// 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; }