1 files changed, 32 insertions, 12 deletions

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 223d94731..1990651ef 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;
 	}
@@ -93,25 +93,24 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	 */
 	uavcan::equipment::esc::RawCommand msg;
 
+	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
+
 	if (_armed) {
 		for (unsigned i = 0; i < num_outputs; i++) {
 
-			float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
+			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
 			if (scaled < 1.0F) {
 				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
 			}
 
-			if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
-				scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
-				perf_count(_perfcnt_scaling_error);
-			} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
-				scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
+			if (scaled > cmd_max) {
+				scaled = cmd_max;
 				perf_count(_perfcnt_scaling_error);
-			} else {
-				; // Correct value
 			}
 
-			msg.cmd.push_back(static_cast<unsigned>(scaled));
+			msg.cmd.push_back(static_cast<int>(scaled));
+
+			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
 		}
 	}
 
@@ -129,10 +128,31 @@ 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();
+
+		ref.esc_voltage     = msg.voltage;
+		ref.esc_current     = msg.current;
+		ref.esc_temperature = msg.temperature;
+		ref.esc_setpoint    = msg.power_rating_pct;
+		ref.esc_rpm         = 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);
+	}
 }