ESC status feedback

2 files changed, 33 insertions, 11 deletions

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index b516536c7..0601d9fa2 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -73,7 +73,7 @@ int UavcanEscController::init()
 
 void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
-	if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
+	if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
 		perf_count(_perfcnt_invalid_input);
 		return;
 	}
@@ -126,10 +126,33 @@ void UavcanEscController::arm_esc(bool arm)
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
 {
-	// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
+	if (msg.esc_index < CONNECTED_ESC_MAX) {
+		_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
+		_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
+
+		auto &ref = _esc_status.esc[msg.esc_index];
+
+		ref.esc_address = msg.getSrcNodeID().get();
+
+		// >0 checks allow to weed out NaNs and negative values that aren't supported.
+		ref.esc_voltage     = (msg.voltage > 0)     ? msg.voltage * 10.0F     : 0;
+		ref.esc_current     = (msg.current > 0)     ? msg.current * 10.0F     : 0;
+		ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0;
+
+		ref.esc_setpoint   = msg.power_rating_pct;
+		ref.esc_rpm        = abs(msg.rpm);
+		ref.esc_errorcount = msg.error_count;
+	}
 }
 
 void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
 {
-	// TODO publish to ORB
+	_esc_status.counter += 1;
+	_esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN;
+
+	if (_esc_status_pub > 0) {
+		(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
+	} else {
+		_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
+	}
 }
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index 0add7e0ad..f4a3877e6 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -48,6 +48,8 @@
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
 #include <systemlib/perf_counter.h>
+#include <uORB/topics/esc_status.h>
+
 
 class UavcanEscController
 {
@@ -74,8 +76,7 @@ private:
 
 
 	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
-	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
-	static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
+	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
 
 	typedef uavcan::MethodBinder<UavcanEscController*,
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
@@ -84,6 +85,10 @@ private:
 	typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
 		TimerCbBinder;
 
+	bool		_armed = false;
+	esc_status_s	_esc_status = {};
+	orb_advert_t	_esc_status_pub = -1;
+
 	/*
 	 * libuavcan related things
 	 */
@@ -94,12 +99,6 @@ private:
 	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
 
 	/*
-	 * ESC states
-	 */
-	bool 				_armed = false;
-	uavcan::equipment::esc::Status	_states[MAX_ESCS];
-
-	/*
 	 * Perf counters
 	 */
 	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");