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/vehicle_imu: move publish to separate method

Browse Source
This commit is contained in:
Daniel Agar 2021-03-08 18:11:49 -05:00
parent 0b9e4a5902
commit a5979e16be
2 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -211,7 +211,6 @@ void VehicleIMU::Run()
bool sensor_data_gap = false;
bool update_integrator_config = false;
bool publish_status = false;
// integrate queued gyro
sensor_gyro_s gyro;
@ -229,7 +228,7 @@ void VehicleIMU::Run()
// collect sample interval average for filters
if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample, gyro.samples)) {
update_integrator_config = true;
publish_status = true;
_publish_status = true;
_status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval;
_status.gyro_raw_rate_hz = 1e6f / _gyro_interval.update_interval_raw;
}
@ -240,7 +239,7 @@ void VehicleIMU::Run()
_gyro_calibration.set_device_id(gyro.device_id);
if (gyro.error_count != _status.gyro_error_count) {
publish_status = true;
_publish_status = true;
_status.gyro_error_count = gyro.error_count;
}
@ -278,7 +277,7 @@ void VehicleIMU::Run()
// collect sample interval average for filters
if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample, accel.samples)) {
update_integrator_config = true;
publish_status = true;
_publish_status = true;
_status.accel_rate_hz = 1e6f / _accel_interval.update_interval;
_status.accel_raw_rate_hz = 1e6f / _accel_interval.update_interval_raw;
}
@ -289,7 +288,7 @@ void VehicleIMU::Run()
_accel_calibration.set_device_id(accel.device_id);
if (accel.error_count != _status.accel_error_count) {
publish_status = true;
_publish_status = true;
_status.accel_error_count = accel.error_count;
}
@ -330,7 +329,7 @@ void VehicleIMU::Run()
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
}
publish_status = true;
_publish_status = true;
if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) {
// start notifying the user periodically if there's significant continuous clipping
@ -369,6 +368,11 @@ void VehicleIMU::Run()
UpdateIntegratorConfiguration();
}
Publish();
}
void VehicleIMU::Publish()
{
// publish if both accel & gyro integrators are ready
if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
@ -397,7 +401,7 @@ void VehicleIMU::Run()
// vehicle_imu_status
// publish before vehicle_imu so that error counts are available synchronously if needed
if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
if (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
_status.accel_device_id = _accel_calibration.device_id();
_status.gyro_device_id = _gyro_calibration.device_id();

2
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -73,6 +73,7 @@ public:
private:
void ParametersUpdate(bool force = false);
void Publish();
void Run() override;
struct IntervalAverage {
@ -134,6 +135,7 @@ private:
orb_advert_t _mavlink_log_pub{nullptr};
bool _intervals_configured{false};
bool _publish_status{false};
const uint8_t _instance;