From 81ecd130fcd539c867e690139937ae129c98b026 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 11 Jan 2022 14:34:52 -0500
Subject: [PATCH] uavcan: ESC publish esc_status in callback instead of timer

 - this ensures every ESC status gets published with minimal latency
 - also prevents publishing ORB_ID(esc_status) when there's no actual
data (bug)
---
 src/drivers/uavcan/actuators/esc.cpp | 39 +++++++---------------------
 src/drivers/uavcan/actuators/esc.hpp |  9 +------
 2 files changed, 11 insertions(+), 37 deletions(-)

diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp
index cd9ff51b70..e808c5d01e 100644
--- a/src/drivers/uavcan/actuators/esc.cpp
+++ b/src/drivers/uavcan/actuators/esc.cpp
@@ -48,16 +48,11 @@ using namespace time_literals;
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
-	_uavcan_sub_status(node),
-	_orb_timer(node)
+	_uavcan_sub_status(node)
 {
 	_uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
 }
 
-UavcanEscController::~UavcanEscController()
-{
-}
-
 int
 UavcanEscController::init()
 {
@@ -69,11 +64,6 @@ UavcanEscController::init()
 		return res;
 	}
 
-	// ESC status will be relayed from UAVCAN bus into ORB at this rate
-	_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
-	_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
-	_esc_status_pub.advertise();
-
 	return res;
 }
 
@@ -136,10 +126,6 @@ void
 UavcanEscController::set_rotor_count(uint8_t count)
 {
 	_rotor_count = count;
-
-	if (_rotor_count != 0u) {
-		_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
-	}
 }
 
 void
@@ -148,29 +134,24 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
 		auto &ref = _esc_status.esc[msg.esc_index];
 
-		ref.esc_address = msg.getSrcNodeID().get();
 		ref.timestamp       = hrt_absolute_time();
+		ref.esc_address = msg.getSrcNodeID().get();
 		ref.esc_voltage     = msg.voltage;
 		ref.esc_current     = msg.current;
 		ref.esc_temperature = msg.temperature;
 		ref.esc_rpm         = msg.rpm;
 		ref.esc_errorcount  = msg.error_count;
+
+		_esc_status.esc_count = _rotor_count;
+		_esc_status.counter += 1;
+		_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
+		_esc_status.esc_online_flags = check_escs_status();
+		_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
+		_esc_status.timestamp = hrt_absolute_time();
+		_esc_status_pub.publish(_esc_status);
 	}
 }
 
-void
-UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
-{
-	_esc_status.timestamp = hrt_absolute_time();
-	_esc_status.esc_count = _rotor_count;
-	_esc_status.counter += 1;
-	_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
-	_esc_status.esc_online_flags = check_escs_status();
-	_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
-
-	_esc_status_pub.publish(_esc_status);
-}
-
 uint8_t
 UavcanEscController::check_escs_status()
 {
diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp
index 1783f7fa4b..99f3ac4ef2 100644
--- a/src/drivers/uavcan/actuators/esc.hpp
+++ b/src/drivers/uavcan/actuators/esc.hpp
@@ -65,7 +65,7 @@ public:
 
 
 	UavcanEscController(uavcan::INode &node);
-	~UavcanEscController();
+	~UavcanEscController() = default;
 
 	int init();
 
@@ -84,17 +84,11 @@ private:
 	 */
 	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
 
-	/**
-	 * ESC status will be published to ORB from this callback (fixed rate).
-	 */
-	void orb_pub_timer_cb(const uavcan::TimerEvent &event);
-
 	/**
 	 * Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
 	 */
 	uint8_t check_escs_status();
 
-	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
 	static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5;	///< 0..31, inclusive, 0 - highest, 31 - lowest
 
 	typedef uavcan::MethodBinder<UavcanEscController *,
@@ -116,7 +110,6 @@ private:
 	uavcan::INode								&_node;
 	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
 	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
-	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
 
 	/*
 	 * ESC states