Allow UavcanEscController to operate using KDECAN protocol

msg/EscReport.msg

@@ -14,6 +14,9 @@ uint8 actuator_function				# actuator output function (one of Motor1...MotorN)
 uint16 failures					# Bitmask to indicate the internal ESC faults
 int8 esc_power					# Applied power 0-100 in % (negative values reserved)
 
+uint16 input_throttle				# input throttle that was given to the ESC
+float32 output_throttle				# output throttle that the ESC is trying to achieve as a percentage of the maximum throttle
+
 uint8 FAILURE_OVER_CURRENT = 0 			# (1 << 0)
 uint8 FAILURE_OVER_VOLTAGE = 1 			# (1 << 1)
 uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 	# (1 << 2)

src/drivers/uavcan/actuators/esc.cpp

@@ -45,28 +45,52 @@
 
 using namespace time_literals;
 
-UavcanEscController::UavcanEscController(uavcan::INode &node) :
+UavcanEscController::UavcanEscController(uavcan::INode &node, uavcan::Protocol can_protocol) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
-	_uavcan_sub_status(node)
+	_uavcan_sub_status(node),
+	_orb_timer(node),
+	_kde_status_pub(node),
+	_kde_status_sub(node),
+	_kde_ith_sub(node),
+	_kde_ith_pub(node),
+	_kde_oth_sub(node),
+	_kde_oth_pub(node),
+	_kde_pwm_pub(node),
+	_can_protocol(can_protocol),
+	_extended_id(true)
 {
 	_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority
 }
 
 int
 UavcanEscController::init()
 {
-	// ESC status subscription
-	int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
+	// ESC status subscription (only enabled if in UAVCAN mode)
+	if (_can_protocol == uavcan::Protocol::Standard) {
+		int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
 
-	if (res < 0) {
-		PX4_ERR("ESC status sub failed %i", res);
-		return res;
-	}
+		if (res < 0) {
+			PX4_ERR("ESC status sub failed %i", res);
+			return res;
+		}
+
+		_esc_status_pub.advertise();
 
-	_esc_status_pub.advertise();
+	} else if (_can_protocol == kdecan::protocolID) {
+		_kde_status_sub.setCallback(KdeStatusCbBinder(this, &UavcanEscController::kdeesc_status_sub_cb));
+		_kde_ith_sub.setCallback(KdeInputThrottleCbBinder(this, &UavcanEscController::kdeesc_input_throttle_sub_cb));
+		_kde_oth_sub.setCallback(KdeOutputThrottleCbBinder(this, &UavcanEscController::kdeesc_output_throttle_sub_cb));
+		_node.getDispatcher().registerCustomCanListener(_kde_status_sub.getKdeListener());
+		_node.getDispatcher().registerCustomCanListener(_kde_ith_sub.getKdeListener());
+		_node.getDispatcher().registerCustomCanListener(_kde_oth_sub.getKdeListener());
 
-	return res;
+		// Callback needed to trigger status queries
+		_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::kdeesc_status_timer_cb));
+		_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
+	}
+
+	return OK;
 }
 
 void
@@ -83,45 +107,62 @@ UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUA
 
 	_prev_cmd_pub = timestamp;
 
-	/*
-	 * Fill the command message
-	 * If unarmed, we publish an empty message anyway
-	 */
-	uavcan::equipment::esc::RawCommand msg;
+	if (_can_protocol == uavcan::Protocol::Standard) {\
+
+		/*
+		 * Fill the command message
+		 * If unarmed, we publish an empty message anyway
+		 */
+		uavcan::equipment::esc::RawCommand msg;
 
-	for (unsigned i = 0; i < num_outputs; i++) {
-		if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) {
-			msg.cmd.push_back(static_cast<unsigned>(0));
+		for (unsigned i = 0; i < num_outputs; i++) {
+			if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) {
+				msg.cmd.push_back(static_cast<unsigned>(0));
 
-		} else {
-			msg.cmd.push_back(static_cast<int>(outputs[i]));
+			} else {
+				msg.cmd.push_back(static_cast<int>(outputs[i]));
+			}
 		}
-	}
 
-	/*
-	 * Remove channels that are always zero.
-	 * The objective of this optimization is to avoid broadcasting multi-frame transfers when a single frame
-	 * transfer would be enough. This is a valid optimization as the UAVCAN specification implies that all
-	 * non-specified ESC setpoints should be considered zero.
-	 * The positive outcome is a (marginally) lower bus traffic and lower CPU load.
-	 *
-	 * From the standpoint of the PX4 architecture, however, this is a hack. It should be investigated why
-	 * the mixer returns more outputs than are actually used.
-	 */
-	for (int index = int(msg.cmd.size()) - 1; index >= _max_number_of_nonzero_outputs; index--) {
-		if (msg.cmd[index] != 0) {
-			_max_number_of_nonzero_outputs = index + 1;
-			break;
+		/*
+		 * Remove channels that are always zero.
+		 * transfer would be enough. This is a valid optimization as the UAVCAN specification implies that all
+		 * non-specified ESC setpoints should be considered zero.
+		 * The positive outcome is a (marginally) lower bus traffic and lower CPU load.
+		 *
+		 * From the standpoint of the PX4 architecture, however, this is a hack. It should be investigated why
+		 * the mixer returns more outputs than are actually used.
+		 */
+		for (int index = int(msg.cmd.size()) - 1; index >= _max_number_of_nonzero_outputs; index--) {
+			if (msg.cmd[index] != 0) {
+				_max_number_of_nonzero_outputs = index + 1;
+				break;
+			}
 		}
-	}
 
-	msg.cmd.resize(_max_number_of_nonzero_outputs);
+		msg.cmd.resize(_max_number_of_nonzero_outputs);
 
-	/*
-	 * Publish the command message to the bus
-	 * Note that for a quadrotor it takes one CAN frame
-	 */
-	_uavcan_pub_raw_cmd.broadcast(msg);
+		/*
+		 * Publish the command message to the bus
+		 * Note that for a quadrotor it takes one CAN frame
+		 */
+		_uavcan_pub_raw_cmd.broadcast(msg);
+
+	} else if (_can_protocol == kdecan::protocolID) {
+		// Fill and publish the command message - one command per each ESC
+		for (unsigned i = 0; i < num_outputs; i++) {
+			uint16_t kdecan_output = 0;
+
+			if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) {
+				kdecan_output = kdecan::minPwmValue;
+
+			} else {
+				kdecan_output = kdecan::minPwmValue + (uint16_t)(((float)outputs[i]/max_output_value()) * (kdecan::maxPwmValue - kdecan::minPwmValue));
+			}
+
+			_kde_pwm_pub.publish(kdecan::PwmThrottle(kdecan::escNodeIdOffset + i, kdecan_output), _extended_id);
+		}
+	}
 }
 
 void
@@ -154,6 +195,70 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	}
 }
 
+void
+UavcanEscController::kdeesc_status_sub_cb(const kdecan::EscStatus& received_structure)
+{
+	if (received_structure.source_address_ - kdecan::escNodeIdOffset < esc_status_s::CONNECTED_ESC_MAX) {
+		auto &ref = _esc_status.esc[received_structure.source_address_  - kdecan::escNodeIdOffset];
+
+		/*PX4_INFO("status received: id=%d V=%.4f C=%.4F RPM=%.4f T=%d Warn=%d",
+			received_structure.source_address_,
+			(double)received_structure.voltage_,
+			(double)received_structure.current_,
+			(double)received_structure.erpm_,
+			received_structure.temperature_,
+			received_structure.warnings_);*/
+
+		ref.esc_address     = received_structure.source_address_;
+		ref.timestamp       = hrt_absolute_time();
+		ref.esc_voltage     = received_structure.voltage_;
+		ref.esc_current     = received_structure.current_;
+		ref.esc_temperature = received_structure.temperature_;
+		ref.esc_rpm         = received_structure.erpm_ / _kdecan_motor_poles;
+		ref.esc_errorcount  = received_structure.warnings_;
+	}
+}
+
+void
+UavcanEscController::kdeesc_input_throttle_sub_cb(const kdecan::InputThrottle& received_structure)
+{
+	if (received_structure.source_address_ - kdecan::escNodeIdOffset < esc_status_s::CONNECTED_ESC_MAX) {
+		auto &ref = _esc_status.esc[received_structure.source_address_  - kdecan::escNodeIdOffset];
+
+		/*PX4_INFO("in throttle: id = %d Th=%d",
+			received_structure.source_address_,
+			received_structure.input_throttle_);*/
+
+		ref.input_throttle = received_structure.input_throttle_;
+	}
+}
+
+void
+UavcanEscController::kdeesc_output_throttle_sub_cb(const kdecan::OutputThrottle& received_structure)
+{
+	if (received_structure.source_address_ - kdecan::escNodeIdOffset < esc_status_s::CONNECTED_ESC_MAX) {
+		auto &ref = _esc_status.esc[received_structure.source_address_  - kdecan::escNodeIdOffset];
+
+		/*PX4_INFO("out throttle: id = %d Th=%.4f",
+			received_structure.source_address_,
+			(double)received_structure.output_throttle_);*/
+
+		ref.output_throttle = received_structure.output_throttle_;
+	}
+}
+
+void
+UavcanEscController::kdeesc_status_timer_cb(const uavcan::TimerEvent &)
+{
+	// we need to actively send a request for the KDE protocol
+	if (_can_protocol == kdecan::protocolID) {
+		_kde_status_pub.publish(kdecan::EscStatus(kdecan::escNodeIdBroadcast), _extended_id);
+		_kde_ith_pub.publish(kdecan::InputThrottle(kdecan::escNodeIdBroadcast), _extended_id);
+		_kde_oth_pub.publish(kdecan::OutputThrottle(kdecan::escNodeIdBroadcast), _extended_id);
+	}
+}
+
+
 uint8_t
 UavcanEscController::check_escs_status()
 {

src/drivers/uavcan/actuators/esc.hpp

@@ -54,6 +54,8 @@
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
 
+#include "../third_party_protocols/kdecan.hpp"
+
 class UavcanEscController
 {
 public:
@@ -64,7 +66,7 @@ class UavcanEscController
 	static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
 
 
-	UavcanEscController(uavcan::INode &node);
+	UavcanEscController(uavcan::INode &node, uavcan::Protocol can_protocol);
 	~UavcanEscController() = default;
 
 	int init();
@@ -78,6 +80,22 @@ class UavcanEscController
 
 	static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); }
 
+	bool setCANProtocol(uavcan::Protocol can_protocol) {
+		if (can_protocol != uavcan::Protocol::Invalid) {
+			_can_protocol = can_protocol;
+
+			return true;
+		} else {
+			return false;
+		}
+	}
+
+	uavcan::Protocol getCANProtocol() { return _can_protocol; }
+
+	void kdeCanSetExtendedId(bool extended_id) { _extended_id = extended_id; }
+
+	void kdeCanSetMotorPoles(uint8_t motor_poles) { _kdecan_motor_poles = motor_poles; }
+
 	esc_status_s &esc_status() { return _esc_status; }
 
 private:
@@ -86,14 +104,37 @@ class UavcanEscController
 	 */
 	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
 
+	/**
+	 * KDECAN ESC status message reception will be reported via these callbacks.
+	 */
+	void kdeesc_status_sub_cb(const kdecan::EscStatus&);
+	void kdeesc_input_throttle_sub_cb(const kdecan::InputThrottle&);
+	void kdeesc_output_throttle_sub_cb(const kdecan::OutputThrottle&);
+
+	/**
+	 * ESC status requests will be triggered from this callback (fixed rate).
+	 */
+	void kdeesc_status_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;
+
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
 
+	typedef uavcan::MethodBinder<UavcanEscController *,
+		void (UavcanEscController::*)(const kdecan::EscStatus&)>  KdeStatusCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanEscController *,
+		void (UavcanEscController::*)(const kdecan::InputThrottle&)>  KdeInputThrottleCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanEscController *,
+		void (UavcanEscController::*)(const kdecan::OutputThrottle&)>  KdeOutputThrottleCbBinder;
+
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
 
@@ -103,13 +144,27 @@ class UavcanEscController
 
 	uint8_t		_rotor_count{0};
 
+	uint8_t 	_kdecan_motor_poles{1};
+
 	/*
 	 * libuavcan related things
 	 */
 	uavcan::MonotonicTime							_prev_cmd_pub;   ///< rate limiting
 	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;
+
+	kdecan::Publisher<kdecan::EscStatus>					_kde_status_pub;
+	kdecan::Subscriber<kdecan::EscStatus, KdeStatusCbBinder>		_kde_status_sub;
+	kdecan::Subscriber<kdecan::InputThrottle, KdeInputThrottleCbBinder>	_kde_ith_sub;
+	kdecan::Publisher<kdecan::InputThrottle>				_kde_ith_pub;
+	kdecan::Subscriber<kdecan::OutputThrottle, KdeOutputThrottleCbBinder>	_kde_oth_sub;
+	kdecan::Publisher<kdecan::OutputThrottle>				_kde_oth_pub;
+	kdecan::Publisher<kdecan::PwmThrottle>					_kde_pwm_pub;
+
+	uavcan::Protocol							_can_protocol;
+	bool 									_extended_id;
 
 	/*
 	 * ESC states