From a5979e16be61ba087060f46227ba5424c1f3da90 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 8 Mar 2021 18:11:49 -0500
Subject: [PATCH] sensors/vehicle_imu: move publish to separate method

---
 src/modules/sensors/vehicle_imu/VehicleIMU.cpp | 18 +++++++++++-------
 src/modules/sensors/vehicle_imu/VehicleIMU.hpp |  2 ++
 2 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
index 06357fc492..993de36e06 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
@@ -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();
 
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
index 5eeb12ddbe..a8c2a7753c 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
@@ -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;