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

sensors: refresh parameters on sensor init

Browse Source
This commit is contained in:
Daniel Agar 2020-05-04 14:32:27 -04:00
parent cb180427cd
commit 692aaada31
1 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -633,17 +633,6 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
// First publication with data
if (_mag.priority[uorb_index] == 0) {
_mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority();
/* 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);
@ -750,6 +739,7 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max)
{
bool added = false;
int max_sensor_index = -1;
for (unsigned i = 0; i < sensor_count_max; i++) {
@ -761,7 +751,10 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor
if (i > 0) {
/* the first always exists, but for each further sensor, add a new validator */
if (!sensor_data.voter.add_new_validator()) {
if (sensor_data.voter.add_new_validator()) {
added = true;
} else {
PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i);
}
}
@ -772,6 +765,11 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor
if (max_sensor_index + 1 > sensor_data.subscription_count) {
sensor_data.subscription_count = max_sensor_index + 1;
}
if (added) {
// force parameter refresh if anything was added
parametersUpdate();
}
}
void VotedSensorsUpdate::printStatus()