From d06c679252b7c63cf966198bda4f4353b269dcdb Mon Sep 17 00:00:00 2001
From: Claudio Micheli <claudio@auterion.com>
Date: Fri, 31 May 2019 13:15:20 +0200
Subject: [PATCH] uavcan esc: added timeout checks to escs.

Signed-off-by: Claudio Micheli <claudio@auterion.com>
---
 src/drivers/uavcan/actuators/esc.cpp | 20 +++++++++++++++++++-
 src/drivers/uavcan/actuators/esc.hpp |  7 +++++++
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp
index 2fbc670a63..fd015b468e 100644
--- a/src/drivers/uavcan/actuators/esc.cpp
+++ b/src/drivers/uavcan/actuators/esc.cpp
@@ -208,7 +208,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 		auto &ref = _esc_status.esc[msg.esc_index];
 
 		ref.esc_address = msg.getSrcNodeID().get();
-
+		ref.timestamp       = hrt_absolute_time();
 		ref.esc_voltage     = msg.voltage;
 		ref.esc_current     = msg.current;
 		ref.esc_temperature = msg.temperature;
@@ -222,6 +222,7 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 {
 	_esc_status.counter += 1;
 	_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
+	_esc_status.esc_online_flags = UavcanEscController::check_escs_status();
 
 	if (_esc_status_pub != nullptr) {
 		(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
@@ -230,3 +231,20 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 		_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
 	}
 }
+
+uint8_t UavcanEscController::check_escs_status()
+{
+	int esc_status_flags = 255;
+
+	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
+		if (_esc_status.esc[index].timestamp == 0) {
+			esc_status_flags &= ~(1 << index);
+
+		} else if (hrt_elapsed_time(&_esc_status.esc[index].timestamp) > 800000.0f) {
+			esc_status_flags &= ~(1 << index);
+		}
+
+	}
+
+	return esc_status_flags;
+}
diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp
index 071dbc190c..aee5a7aaba 100644
--- a/src/drivers/uavcan/actuators/esc.hpp
+++ b/src/drivers/uavcan/actuators/esc.hpp
@@ -50,6 +50,7 @@
 #include <perf/perf_counter.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <drivers/drv_hrt.h>
 
 
 class UavcanEscController
@@ -78,6 +79,11 @@ private:
 	 */
 	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 MAX_RATE_HZ = 200;			///< XXX make this configurable
 	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
@@ -95,6 +101,7 @@ private:
 	esc_status_s	_esc_status = {};
 	orb_advert_t	_esc_status_pub = nullptr;
 	orb_advert_t _actuator_outputs_pub = nullptr;
+	hrt_abstime _last_received_msg[esc_status_s::CONNECTED_ESC_MAX] {0};
 
 	/*
 	 * libuavcan related things