UAVCAN: Fixed all compile errors

2 files changed, 123 insertions, 47 deletions

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 94fb15544..332c3a5e8 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -31,14 +31,18 @@
  *
  ****************************************************************************/
 
+#include <nuttx/config.h>
+
 #include <cstdlib>
 #include <cstring>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/mixer/mixer.h>
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
-#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 
 #include "uavcan_main.hpp"
 
@@ -55,7 +59,13 @@
  */
 UavcanNode *UavcanNode::_instance;
 
-UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
+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({})
@@ -65,8 +75,37 @@ UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	_control_topics[2] = ORB_ID(actuator_controls_2);
 	_control_topics[3] = ORB_ID(actuator_controls_3);
 
-	memset(_controls, 0, sizeof(_controls));
-	memset(_poll_fds, 0, sizeof(_poll_fds));
+	// memset(_controls, 0, sizeof(_controls));
+	// memset(_poll_fds, 0, sizeof(_poll_fds));
+}
+
+UavcanNode::~UavcanNode()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		unsigned i = 10;
+
+		do {
+			/* wait 50ms - it should wake every 100ms or so worst-case */
+			usleep(50000);
+
+			/* if we have given up, kill it */
+			if (--i == 0) {
+				task_delete(_task);
+				break;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+		// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	::close(_armed_sub);
+
+	_instance = nullptr;
 }
 
 int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
@@ -123,24 +162,32 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 		return node_init_res;
 	}
 
-	int ret;
-
-	/* do regular cdev init */
-	ret = CDev::init();
-
-	if (ret != OK)
-		return ret;
-
 	/*
 	 * Start the task. Normally it should never exit.
 	 */
 	static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
-	return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
+	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
 			      static_cast<main_t>(run_trampoline), nullptr);
+
+	if (_instance->_task < 0) {
+		warnx("start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
 }
 
 int UavcanNode::init(uavcan::NodeID node_id)
 {
+
+	int ret;
+
+	/* do regular cdev init */
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
 	uavcan::protocol::SoftwareVersion swver;
 	swver.major = 12;                        // TODO fill version info
 	swver.minor = 34;
@@ -162,32 +209,35 @@ int UavcanNode::run()
 {
 	_node.setStatusOk();
 
+	// XXX figure out the output count
+	_output_count = 2;
+
+
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-	while (true) {
-		
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+
+	while (!_task_should_exit) {
+
 		if (_groups_subscribed != _groups_required) {
 			subscribe();
 			_groups_subscribed = _groups_required;
-			/* force setting update rate */
-			_current_update_rate = 0;
 		}
 
 		int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */);
 
-				/* this would be bad... */
+		// this would be bad...
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 
 		} else if (ret == 0) {
-			/* timeout: no control data, switch to failsafe values */
-//			warnx("no PWM: failsafe");
+			// timeout: no control data, switch to failsafe values
 
 		} else {
 
-			/* get controls for required topics */
+			// get controls for required topics
 			unsigned poll_id = 0;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
@@ -198,7 +248,7 @@ int UavcanNode::run()
 				}
 			}
 
-			/* can we mix? */
+			//can we mix?
 			if (_mixers != nullptr) {
 
 				// XXX one output group has 8 outputs max,
@@ -209,9 +259,9 @@ int UavcanNode::run()
 				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
 				outputs.timestamp = hrt_absolute_time();
 
-				/* iterate actuators */
+				// iterate actuators
 				for (unsigned i = 0; i < outputs.noutputs; i++) {
-					/* last resort: catch NaN, INF and out-of-band errors */
+					// last resort: catch NaN, INF and out-of-band errors
 					if (!isfinite(outputs.output[i]) ||
 						outputs.output[i] < -1.0f ||
 						outputs.output[i] > 1.0f) {
@@ -224,25 +274,25 @@ int UavcanNode::run()
 					}
 				}
 
-				printf("CAN out: ")
+				printf("CAN out: ");
 				/* output to the bus */
 				for (unsigned i = 0; i < outputs.noutputs; i++) {
-					printf("%u: %8.4f ", i, outputs.output[i])
+					printf("%u: %8.4f ", i, outputs.output[i]);
 					// can_send_xxx
 				}
-				printf("\n");
+				printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED");
 
 			}
 		}
 
-		/* check arming state */
+		// Check arming state
 		bool updated = false;
 		orb_check(_armed_sub, &updated);
 
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-			/* update the armed status and check that we're not locked down */
+			// Update the armed status and check that we're not locked down
 			bool set_armed = _armed.armed && !_armed.lockdown;
 
 			arm_actuators(set_armed);
@@ -271,7 +321,7 @@ UavcanNode::control_callback(uintptr_t handle,
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
-	input = _controls[control_group].control[control_index];
+	input = controls[control_group].control[control_index];
 	return 0;
 }
 
@@ -287,22 +337,24 @@ UavcanNode::teardown()
 	::close(_armed_sub);
 }
 
-void
+int
 UavcanNode::arm_actuators(bool arm)
 {
-	bool changed = (_armed != arm);
+	bool changed = (_is_armed != arm);
 
-	_armed = arm;
+	_is_armed = arm;
 
 	if (changed) {
 		// Propagate immediately to CAN bus
 	}
+
+	return OK;
 }
 
 void
 UavcanNode::subscribe()
 {
-	/* subscribe/unsubscribe to required actuator control groups */
+	// Subscribe/unsubscribe to required actuator control groups
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	_poll_fds_num = 0;
@@ -326,7 +378,7 @@ UavcanNode::subscribe()
 }
 
 int
-PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 213cb4206..857c1dc66 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -33,8 +33,14 @@
 
 #pragma once
 
+#include <nuttx/config.h>
+
 #include <uavcan_stm32/uavcan_stm32.hpp>
-#include <drivers/drv_pwm_output.h>
+#include <drivers/device/device.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 
 /**
  * @file uavcan_main.hpp
@@ -44,6 +50,9 @@
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
+#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
+#define UAVCAN_DEVICE_PATH	"/dev/uavcan"
+
 /**
  * A UAVCAN node.
  */
@@ -57,21 +66,36 @@ public:
 	typedef uavcan::Node<MemPoolSize> Node;
 	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
 
-	UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
+			UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
+
+	virtual		~UavcanNode();
 
-	static int start(uavcan::NodeID node_id, uint32_t bitrate);
+	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
 
-	Node &getNode() { return _node; }
+	Node&		getNode() { return _node; }
 
 	static int	control_callback(uintptr_t handle,
 					 uint8_t control_group,
 					 uint8_t control_index,
 					 float &input);
-	void	subscribe();
+
+	void		subscribe();
+
+	int		teardown();
+	int		arm_actuators(bool arm);
 
 private:
-	int init(uavcan::NodeID node_id);
-	int run();
+	int		init(uavcan::NodeID node_id);
+	int		run();
+	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
+
+	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
+
+	unsigned		_output_count;		///< number of actuators currently available
 
 	static UavcanNode	*_instance;		///< pointer to the library instance
 	Node			_node;
@@ -80,9 +104,9 @@ private:
 
 	uint32_t		_groups_required;
 	uint32_t		_groups_subscribed;
-	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
-	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS];
-	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
-	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+	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;
 };