fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 04:13:33 +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-06 09:31:09 -04:00
parent cb180427cd
commit 692aaada31
1 changed files with 10 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+10 -12
View File
@@ -633,17 +633,6 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
// First publication with data // First publication with data
if (_mag.priority[uorb_index] == 0) { if (_mag.priority[uorb_index] == 0) {
_mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority(); _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); 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) void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max)
{ {
bool added = false;
int max_sensor_index = -1; int max_sensor_index = -1;
for (unsigned i = 0; i < sensor_count_max; i++) { 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) { if (i > 0) {
/* the first always exists, but for each further sensor, add a new validator */ /* 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); 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) { if (max_sensor_index + 1 > sensor_data.subscription_count) {
sensor_data.subscription_count = max_sensor_index + 1; sensor_data.subscription_count = max_sensor_index + 1;
} }
if (added) {
// force parameter refresh if anything was added
parametersUpdate();
}
} }
void VotedSensorsUpdate::printStatus() void VotedSensorsUpdate::printStatus()