dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors: skip selection and failover checks during parameter update cycles

This commit is contained in:
Daniel Agar 2022-02-07 10:44:51 -05:00
parent b6607a7b78
commit 97a75fc388
6 changed files with 68 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -127,7 +127,7 @@ void VehicleAirData::AirTemperatureUpdate()
}
}
void VehicleAirData::ParametersUpdate()
bool VehicleAirData::ParametersUpdate()
{
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
@ -136,7 +136,10 @@ void VehicleAirData::ParametersUpdate()
_parameter_update_sub.copy(&param_update);
updateParams();
return true;
}
return false;
}
void VehicleAirData::Run()
@ -145,7 +148,7 @@ void VehicleAirData::Run()
const hrt_abstime time_now_us = hrt_absolute_time();
ParametersUpdate();
const bool parameter_update = ParametersUpdate();
SensorCorrectionsUpdate();
@ -203,7 +206,8 @@ void VehicleAirData::Run()
_voter.get_best(time_now_us, &best_index);
if (best_index >= 0) {
if (_selected_sensor_sub_index != best_index) {
// handle selection change (don't process on same iteration as parameter update)
if ((_selected_sensor_sub_index != best_index) && !parameter_update) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
@ -281,8 +285,8 @@ void VehicleAirData::Run()
}
}
// check failover and report
if (_last_failover_count != _voter.failover_count()) {
// check failover and report (save failover report for a cycle where parameters didn't update)
if (_last_failover_count != _voter.failover_count() && !parameter_update) {
uint32_t flags = _voter.failover_state();
int failover_index = _voter.failover_index();

4
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -71,7 +71,7 @@ public:
private:
void Run() override;
void ParametersUpdate();
bool ParametersUpdate();
void SensorCorrectionsUpdate(bool force = false);
void AirTemperatureUpdate();

17
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -79,7 +79,7 @@ void VehicleMagnetometer::Stop()
}
}
void VehicleMagnetometer::ParametersUpdate(bool force)
bool VehicleMagnetometer::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_parameter_update_sub.updated() || force) {
@ -161,7 +161,11 @@ void VehicleMagnetometer::ParametersUpdate(bool force)
}
}
return true;
}
return false;
}
void VehicleMagnetometer::UpdateMagBiasEstimate()
@ -360,7 +364,7 @@ void VehicleMagnetometer::Run()
const hrt_abstime time_now_us = hrt_absolute_time();
ParametersUpdate();
const bool parameter_update = ParametersUpdate();
// check vehicle status for changes to armed state
if (_vehicle_control_mode_sub.updated()) {
@ -450,7 +454,8 @@ void VehicleMagnetometer::Run()
_voter.get_best(time_now_us, &best_index);
if (best_index >= 0) {
if (_selected_sensor_sub_index != best_index) {
// handle selection change (don't process on same iteration as parameter update)
if ((_selected_sensor_sub_index != best_index) && !parameter_update) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
@ -488,9 +493,9 @@ void VehicleMagnetometer::Run()
}
// check failover and report
if (_param_sens_mag_mode.get()) {
if (_last_failover_count != _voter.failover_count()) {
// check failover and report (save failover report for a cycle where parameters didn't update)
if (_last_failover_count != _voter.failover_count() && !parameter_update) {
uint32_t flags = _voter.failover_state();
int failover_index = _voter.failover_index();

4
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -79,7 +79,7 @@ public:
private:
void Run() override;
void ParametersUpdate(bool force = false);
bool ParametersUpdate(bool force = false);
void Publish(uint8_t instance, bool multi = false);

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -80,6 +80,8 @@ void VotedSensorsUpdate::initializeSensors()
void VotedSensorsUpdate::parametersUpdate()
{
_parameter_update = true;
updateParams();
// run through all IMUs
@ -188,40 +190,44 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
}
// find the best sensor
int accel_best_index = -1;
int gyro_best_index = -1;
_accel.voter.get_best(time_now_us, &accel_best_index);
_gyro.voter.get_best(time_now_us, &gyro_best_index);
int accel_best_index = _accel.last_best_vote;
int gyro_best_index = _gyro.last_best_vote;
if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
// use sensor_selection to find best
if (_sensor_selection_sub.update(&_selection)) {
// reset inconsistency checks against primary
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
_gyro_diff[sensor_index].zero();
if (!_parameter_update) {
// update current accel/gyro selection, skipped on cycles where parameters update
_accel.voter.get_best(time_now_us, &accel_best_index);
_gyro.voter.get_best(time_now_us, &gyro_best_index);
if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
// use sensor_selection to find best
if (_sensor_selection_sub.update(&_selection)) {
// reset inconsistency checks against primary
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
_gyro_diff[sensor_index].zero();
}
}
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
accel_best_index = i;
}
if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
gyro_best_index = i;
}
}
} else {
// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
}
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
accel_best_index = i;
}
if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
gyro_best_index = i;
}
}
} else {
// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
}
// write data for the best sensor to output variables
if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT)
&& (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT)) {
if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0)
&& (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) {
raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
@ -423,6 +429,11 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
status.timestamp = hrt_absolute_time();
_sensors_status_imu_pub.publish(status);
if (_parameter_update) {
// reset
_parameter_update = false;
}
}
void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)

4
src/modules/sensors/voted_sensors_update.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -182,6 +182,8 @@ private:
sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */
bool _parameter_update{false};
DEFINE_PARAMETERS(
(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
)