UAVCAN ESC controller - proof of concept state

5 files changed, 265 insertions, 45 deletions

diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp
new file mode 100644
index 000000000..bde4d2a7f
--- /dev/null
+++ b/src/modules/uavcan/esc_controller.cpp
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_controller.cpp
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "esc_controller.hpp"
+#include <systemlib/err.h>
+
+UavcanEscController::UavcanEscController(uavcan::INode &node) :
+	_node(node),
+	_uavcan_pub_raw_cmd(node),
+	_uavcan_sub_status(node),
+	_orb_timer(node)
+{
+}
+
+int UavcanEscController::init()
+{
+	int res = -1;
+
+	// ESC status subscription
+	res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
+	if (res < 0)
+	{
+		warnx("ESC status sub failed %i", res);
+		return res;
+	}
+
+	// ESC status will be relayed from UAVCAN bus into ORB at this rate
+	_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
+	_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
+
+	return res;
+}
+
+void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
+{
+	assert(outputs != nullptr);
+	assert(num_outputs <= MAX_ESCS);
+
+	/*
+	 * Rate limiting - we don't want to congest the bus
+	 */
+	const auto timestamp = _node.getMonotonicTime();
+	if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
+		return;
+	}
+	_prev_cmd_pub = timestamp;
+
+	/*
+	 * Fill the command message
+	 * If unarmed, we publish an empty message anyway
+	 */
+	uavcan::equipment::esc::RawCommand msg;
+
+	if (_armed) {
+		for (unsigned i = 0; i < num_outputs; i++) {
+
+			float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
+			if (scaled < 1.0F)
+				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
+
+			assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN);
+			assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX);
+
+			msg.cmd.push_back(scaled);
+		}
+	}
+
+	/*
+	 * Publish the command message to the bus
+	 * Note that for a quadrotor it takes one CAN frame
+	 */
+	(void)_uavcan_pub_raw_cmd.broadcast(msg);
+}
+
+void UavcanEscController::arm_esc(bool arm)
+{
+	_armed = 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()
+}
+
+void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
+{
+	// TODO publish to ORB
+}
diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp
new file mode 100644
index 000000000..0ed0c59b5
--- /dev/null
+++ b/src/modules/uavcan/esc_controller.hpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_controller.hpp
+ *
+ * UAVCAN <--> ORB bridge for ESC messages:
+ *     uavcan.equipment.esc.RawCommand
+ *     uavcan.equipment.esc.RPMCommand
+ *     uavcan.equipment.esc.Status
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/esc/Status.hpp>
+
+class UavcanEscController
+{
+public:
+	UavcanEscController(uavcan::INode& node);
+
+	int init();
+
+	void update_outputs(float *outputs, unsigned num_outputs);
+
+	void arm_esc(bool arm);
+
+private:
+	/**
+	 * ESC status message reception will be reported via this callback.
+	 */
+	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
+
+	/**
+	 * ESC status will be published to ORB from this callback (fixed rate).
+	 */
+	void orb_pub_timer_cb(const uavcan::TimerEvent &event);
+
+
+	static constexpr unsigned MAX_RATE_HZ = 100;			///< 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;
+
+	typedef uavcan::MethodBinder<UavcanEscController*,
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
+		StatusCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
+		TimerCbBinder;
+
+	/*
+	 * 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;
+
+	/*
+	 * ESC states
+	 */
+	bool 				_armed = false;
+	uavcan::equipment::esc::Status	_states[MAX_ESCS];
+};
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 098543879..ce9f981a8 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -40,8 +40,9 @@ MODULE_COMMAND = uavcan
 
 MAXOPTIMIZATION = -Os
 
-SRCS += uavcan_main.cpp \
-        uavcan_clock.cpp
+SRCS += uavcan_main.cpp     \
+        uavcan_clock.cpp    \
+        esc_controller.cpp
 
 #
 # libuavcan
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 859db93c7..e7829cbba 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -61,18 +61,8 @@ UavcanNode *UavcanNode::_instance;
 
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
-	_task(0),
-	_task_should_exit(false),
-	_armed_sub(-1),
-	_is_armed(false),
-	_output_count(0),
 	_node(can_driver, system_clock),
-	_controls({}),
-	_poll_fds({}),
-	_mixers(nullptr),
-	_groups_required(0),
-	_groups_subscribed(0),
-	_poll_fds_num(0)
+	_esc_controller(_node)
 {
 	_control_topics[0] = ORB_ID(actuator_controls_0);
 	_control_topics[1] = ORB_ID(actuator_controls_1);
@@ -183,8 +173,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 
 int UavcanNode::init(uavcan::NodeID node_id)
 {
-
-	int ret;
+	int ret = -1;
 
 	/* do regular cdev init */
 	ret = CDev::init();
@@ -192,6 +181,10 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	if (ret != OK)
 		return ret;
 
+	ret = _esc_controller.init();
+	if (ret < 0)
+		return ret;
+
 	uavcan::protocol::SoftwareVersion swver;
 	swver.major = 12;                        // TODO fill version info
 	swver.minor = 34;
@@ -222,6 +215,11 @@ int UavcanNode::run()
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
 
+	/*
+	 * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
+	 *     IO multiplexing shall be done here.
+	 */
+
 	while (!_task_should_exit) {
 
 		if (_groups_subscribed != _groups_required) {
@@ -279,14 +277,16 @@ int UavcanNode::run()
 					}
 				}
 
+				/*
+				 * Output to the bus
+				 */
 				printf("CAN out: ");
-				/* output to the bus */
 				for (unsigned i = 0; i < outputs.noutputs; i++) {
 					printf("%u: %8.4f ", i, outputs.output[i]);
-					// XXX send out via CAN here
 				}
 				printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED");
 
+				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
 			}
 
 		}
@@ -304,13 +304,12 @@ int UavcanNode::run()
 			arm_actuators(set_armed);
 		}
 
-		// Output commands and fetch data
+		// Output commands and fetch data TODO ORB multiplexing
 
-		const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000));
+		const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1));
 
-		if (res < 0) {
-			warnx("Spin error %i", res);
-			::sleep(1);
+		if (spin_res < 0) {
+			warnx("node spin error %i", spin_res);
 		}
 	}
 
@@ -347,14 +346,8 @@ UavcanNode::teardown()
 int
 UavcanNode::arm_actuators(bool arm)
 {
-	bool changed = (_is_armed != arm);
-
 	_is_armed = arm;
-
-	if (changed) {
-		// Propagate immediately to CAN bus
-	}
-
+	_esc_controller.arm_esc(arm);
 	return OK;
 }
 
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index beaa5e1d4..f4a709c79 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -42,6 +42,8 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
+#include "esc_controller.hpp"
+
 /**
  * @file uavcan_main.hpp
  *
@@ -94,24 +96,25 @@ private:
 	int		init(uavcan::NodeID node_id);
 	int		run();
 
-	int			_task;			///< handle to the OS task
-	bool			_task_should_exit;	///< flag to indicate to tear down the CAN driver
-	int			_armed_sub;		///< uORB subscription of the arming status
-	actuator_armed_s	_armed;			///< the arming request of the system
-	bool			_is_armed;		///< the arming status of the actuators on the bus
+	int			_task = -1;			///< handle to the OS task
+	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
+	int			_armed_sub = -1;		///< uORB subscription of the arming status
+	actuator_armed_s	_armed;				///< the arming request of the system
+	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
-	unsigned		_output_count;		///< number of actuators currently available
+	unsigned		_output_count = 0;		///< number of actuators currently available
 
-	static UavcanNode	*_instance;		///< pointer to the library instance
-	Node			_node;
+	static UavcanNode	*_instance;			///< singleton pointer
+	Node			_node;				///< library instance
+	UavcanEscController	_esc_controller;
 
-	MixerGroup		*_mixers;
+	MixerGroup		*_mixers = nullptr;
 
-	uint32_t		_groups_required;
-	uint32_t		_groups_subscribed;
-	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
-	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
-	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
-	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
-	unsigned		_poll_fds_num;
+	uint32_t		_groups_required = 0;
+	uint32_t		_groups_subscribed = 0;
+	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	unsigned		_poll_fds_num = 0;
 };