From f19a18798b72f8bc21b2adb412ccec22d782c052 Mon Sep 17 00:00:00 2001
From: Jacob Dahl <dahl.jakejacob@gmail.com>
Date: Thu, 13 Aug 2020 11:26:17 -0800
Subject: [PATCH] refactored UavcanNode publication blocks into functions

---
 boards/px4/fmu-v4/cannode.cmake       |  2 +-
 src/drivers/uavcannode/UavcanNode.cpp | 42 +++++++++++++++++++++------
 src/drivers/uavcannode/UavcanNode.hpp |  7 +++++
 3 files changed, 41 insertions(+), 10 deletions(-)

diff --git a/boards/px4/fmu-v4/cannode.cmake b/boards/px4/fmu-v4/cannode.cmake
index 238491e6d6..934dea5e9c 100644
--- a/boards/px4/fmu-v4/cannode.cmake
+++ b/boards/px4/fmu-v4/cannode.cmake
@@ -81,7 +81,7 @@ px4_add_board(
 		#sd_bench
 		#shutdown
 		top
-		#topic_listener
+		topic_listener
 		#tune_control
 		ver
 		work_queue
diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp
index d0a1480a9d..c309e10664 100644
--- a/src/drivers/uavcannode/UavcanNode.cpp
+++ b/src/drivers/uavcannode/UavcanNode.cpp
@@ -330,6 +330,24 @@ void UavcanNode::Run()
 		PX4_ERR("node spin error %i", spin_res);
 	}
 
+	send_battery_info();
+	send_raw_air_data();
+	send_static_pressure();
+	send_magnetic_field_strength2();
+	send_gnss_fix2();
+
+	perf_end(_cycle_perf);
+
+	pthread_mutex_unlock(&_node_mutex);
+
+	if (_task_should_exit.load()) {
+		ScheduleClear();
+		_instance = nullptr;
+	}
+}
+
+void UavcanNode::send_battery_info()
+{
 	// battery_status -> uavcan::equipment::power::BatteryInfo
 	if (_battery_status_sub.updated()) {
 		battery_status_s battery;
@@ -359,7 +377,10 @@ void UavcanNode::Run()
 			_power_battery_info_publisher.broadcast(battery_info);
 		}
 	}
+}
 
+void UavcanNode::send_raw_air_data()
+{
 	// differential_pressure -> uavcan::equipment::air_data::RawAirData
 	if (_diff_pressure_sub.updated()) {
 		differential_pressure_s diff_press;
@@ -378,7 +399,10 @@ void UavcanNode::Run()
 			_raw_air_data_publisher.broadcast(raw_air_data);
 		}
 	}
+}
 
+void UavcanNode::send_range_sensor_measurement()
+{
 	// distance_sensor[] -> uavcan::equipment::range_sensor::Measurement
 	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 		distance_sensor_s dist;
@@ -427,7 +451,10 @@ void UavcanNode::Run()
 			_range_sensor_measurement.broadcast(range_sensor);
 		}
 	}
+}
 
+void UavcanNode::send_static_pressure()
+{
 	// sensor_baro -> uavcan::equipment::air_data::StaticTemperature
 	if (_sensor_baro_sub.updated()) {
 		sensor_baro_s baro;
@@ -445,7 +472,10 @@ void UavcanNode::Run()
 			}
 		}
 	}
+}
 
+void UavcanNode::send_magnetic_field_strength2()
+{
 	// sensor_mag -> uavcan::equipment::ahrs::MagneticFieldStrength2
 	if (_sensor_mag_sub.updated()) {
 		sensor_mag_s mag;
@@ -459,7 +489,10 @@ void UavcanNode::Run()
 			_ahrs_magnetic_field_strength2_publisher.broadcast(magnetic_field);
 		}
 	}
+}
 
+void UavcanNode::send_gnss_fix2()
+{
 	// vehicle_gps_position -> uavcan::equipment::gnss::Fix2
 	if (_vehicle_gps_position_sub.updated()) {
 		vehicle_gps_position_s gps;
@@ -494,15 +527,6 @@ void UavcanNode::Run()
 			_gnss_fix2_publisher.broadcast(fix2);
 		}
 	}
-
-	perf_end(_cycle_perf);
-
-	pthread_mutex_unlock(&_node_mutex);
-
-	if (_task_should_exit.load()) {
-		ScheduleClear();
-		_instance = nullptr;
-	}
 }
 
 void UavcanNode::print_info()
diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp
index 1ebeef2715..e77dd44485 100644
--- a/src/drivers/uavcannode/UavcanNode.hpp
+++ b/src/drivers/uavcannode/UavcanNode.hpp
@@ -146,6 +146,13 @@ private:
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
 
+	void send_battery_info();
+	void send_raw_air_data();
+	void send_static_pressure();
+	void send_magnetic_field_strength2();
+	void send_gnss_fix2();
+	void send_range_sensor_measurement();
+
 	px4::atomic_bool	_task_should_exit{false};	///< flag to indicate to tear down the CAN driver
 
 	bool		_initialized{false};		///< number of actuators currently available