uavcan: esc: fix uavcan ESC control

This fixes a number of issues with uavcan ESC control.

- Motor Testing when using a CANnode as a PWM expander now works by allowing the FunctionMotors class to perform prearm control

- The esc.RawCommand message is now always published for configured uavcan ESC outputs. Previously prior to arming the message would be published empty, which causes certain ESCs to enter an error state.

- Useless and redundant code has been removed and small name changes have been applied.

src/drivers/uavcan/actuators/esc.cpp

@@ -74,15 +74,22 @@ UavcanEscController::init()
 		_uavcan_pub_raw_cmd.getTransferSender().setIfaceMask(iface_mask);
 	}
 
+	char param_name[17];
+
+	for (unsigned i = 0; i < MAX_ACTUATORS; ++i) {
+		snprintf(param_name, sizeof(param_name), "UAVCAN_EC_FUNC%d", i + 1);
+		_param_handles[i] = param_find(param_name);
+	}
+
 	return res;
 }
 
 void
-UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs)
 {
-	/*
-	 * Rate limiting - we don't want to congest the bus
-	 */
+	// TODO: remove stop_motors as it's unnecessary and unused almost everywhere
+
+	// TODO: configurable rate limit
 	const auto timestamp = _node.getMonotonicTime();
 
 	if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
@@ -91,44 +98,28 @@ 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;
 
-	for (unsigned i = 0; i < num_outputs; i++) {
-		if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) {
-			msg.cmd.push_back(static_cast<unsigned>(0));
+	// directly output values from mixer
+	for (unsigned i = 0; i < total_outputs; i++) {
+		msg.cmd.push_back(static_cast<int>(outputs[i]));
+	}
 
-		} else {
-			msg.cmd.push_back(static_cast<int>(outputs[i]));
+	// but only output as many channels as are configured
+	uint8_t min_size = 0;
+
+	for (int i = 0; i < MAX_ACTUATORS; i++) {
+		int32_t val = 0;
+
+		if (param_get(_param_handles[i], &val) == 0) {
+			if (val > 0) {
+				min_size = i + 1;
+			}
 		}
 	}
 
-	/*
-	 * 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;
-		}
-	}
+	msg.cmd.resize(min_size);
 
-	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);
 }
 

src/drivers/uavcan/actuators/esc.hpp

@@ -53,13 +53,13 @@
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
+#include <parameters/param.h>
 
 class UavcanEscController
 {
 public:
 	static constexpr int MAX_ACTUATORS = esc_status_s::CONNECTED_ESC_MAX;
 	static constexpr unsigned MAX_RATE_HZ = 400;
-	static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX;
 
 	static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
 
@@ -69,7 +69,7 @@ public:
 
 	int init();
 
-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
+	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs);
 
 	/**
 	 * Sets the number of rotors and enable timer
@@ -111,8 +111,6 @@ private:
 	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
 	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
 
-	/*
-	 * ESC states
-	 */
-	uint8_t				_max_number_of_nonzero_outputs{0};
+
+	param_t _param_handles[MAX_ACTUATORS] {PARAM_INVALID};
 };

src/drivers/uavcan/uavcan_main.cpp

@@ -602,18 +602,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		PX4_DEBUG("sensor bridge '%s' init ok", br->get_name());
 	}
 
-#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
-
-	// Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions
-	if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE
-	    || UavcanEscController::max_output_value() > (int)UINT16_MAX) {
-		PX4_ERR("ESC max output value assertion failed");
-		return -EINVAL;
-	}
-
-	_mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
-#endif
-
 	/* Set up shared service clients */
 	_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
 	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));

src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp

@@ -80,9 +80,7 @@ public:
 private:
 	void callback(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand> &msg)
 	{
-
-		actuator_motors_s actuator_motors;
-
+		actuator_motors_s actuator_motors = {};
 		actuator_motors.timestamp = hrt_absolute_time();
 		actuator_motors.timestamp_sample = actuator_motors.timestamp;
 
@@ -91,13 +89,18 @@ private:
 				break;
 			}
 
-			// Normalized to -8192, 8191 in uavcan. actuator_motors is -1 to 1
-			actuator_motors.control[i] = msg.cmd[i] / 8192.f;
+			// Treat zero as disarmed value
+			if (msg.cmd[i] == 0) {
+				// NAN maps to disarmed
+				actuator_motors.control[i] = NAN;
 
+			} else {
+				// Normalized to -8192, 8191 in uavcan. actuator_motors is -1 to 1
+				actuator_motors.control[i] = msg.cmd[i] / 8192.f;
+			}
 		}
 
 		_actuator_motors_pub.publish(actuator_motors);
-
 	}
 
 	uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};

src/lib/mixer_module/functions/FunctionMotors.hpp

@@ -68,7 +68,11 @@ public:
 
 	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; }
 
+#if defined(CONFIG_DRIVERS_UAVCANNODE)
+	bool allowPrearmControl() const override { return true; }
+#else
 	bool allowPrearmControl() const override { return false; }
+#endif
 
 	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
 

src/lib/mixer_module/mixer_module.cpp

@@ -541,8 +541,6 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const
 void
 MixingOutput::output_limit_calc(const bool armed, const int num_channels, const float output[MAX_ACTUATORS])
 {
-	const bool pre_armed = armNoThrottle();
-
 	// time to slowly ramp up the ESCs
 	static constexpr hrt_abstime RAMP_TIME_US = 500_ms;
 
@@ -589,7 +587,7 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const
 	 */
 	auto local_limit_state = _output_state;
 
-	if (pre_armed) {
+	if (isPrearmed()) {
 		local_limit_state = OutputLimitState::ON;
 	}
 

src/lib/mixer_module/mixer_module.hpp

@@ -209,7 +209,7 @@ protected:
 
 private:
 
-	bool armNoThrottle() const
+	bool isPrearmed() const
 	{
 		return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode;
 	}