UAVCAN: improve printing, ready for full closed loop test

2 files changed, 47 insertions, 8 deletions

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 332c3a5e8..a86314852 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -68,7 +68,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_output_count(0),
 	_node(can_driver, system_clock),
 	_controls({}),
-	_poll_fds({})
+	_poll_fds({}),
+	_mixers(nullptr),
+	_groups_required(0),
+	_groups_subscribed(0),
+	_poll_fds_num(0)
 {
 	_control_topics[0] = ORB_ID(actuator_controls_0);
 	_control_topics[1] = ORB_ID(actuator_controls_1);
@@ -225,7 +229,7 @@ int UavcanNode::run()
 			_groups_subscribed = _groups_required;
 		}
 
-		int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */);
+		int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */);
 
 		// this would be bad...
 		if (ret < 0) {
@@ -234,6 +238,7 @@ int UavcanNode::run()
 
 		} else if (ret == 0) {
 			// timeout: no control data, switch to failsafe values
+			// XXX trigger failsafe
 
 		} else {
 
@@ -278,11 +283,12 @@ int UavcanNode::run()
 				/* output to the bus */
 				for (unsigned i = 0; i < outputs.noutputs; i++) {
 					printf("%u: %8.4f ", i, outputs.output[i]);
-					// can_send_xxx
+					// XXX send out via CAN here
 				}
 				printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED");
 
 			}
+
 		}
 
 		// Check arming state
@@ -300,7 +306,7 @@ int UavcanNode::run()
 
 		// Output commands and fetch data
 
-		const int res = _node.spin(uavcan::MonotonicDuration::getInfinite());
+		const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000));
 
 		if (res < 0) {
 			warnx("Spin error %i", res);
@@ -309,6 +315,7 @@ int UavcanNode::run()
 	}
 
 	teardown();
+	warnx("exiting.");
 
 	exit(0);
 }
@@ -378,7 +385,7 @@ UavcanNode::subscribe()
 }
 
 int
-UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
@@ -428,7 +435,7 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				ret = _mixers->load_from_buf(buf, buflen);
 
 				if (ret != 0) {
-					debug("mixer load failed with %d", ret);
+					warnx("mixer load failed with %d", ret);
 					delete _mixers;
 					_mixers = nullptr;
 					_groups_required = 0;
@@ -449,9 +456,24 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	unlock();
 
+	if (ret == -ENOTTY) {
+		ret = CDev::ioctl(filp, cmd, arg);
+	}
+
 	return ret;
 }
 
+void
+UavcanNode::print_info()
+{
+	if (!_instance) {
+		warnx("not running, start first");
+	}
+
+	warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
+}
+
 /*
  * App entry point
  */
@@ -511,5 +533,17 @@ int uavcan_main(int argc, char *argv[])
 		::exit(1);
 	}
 
+	/* commands below require the app to be started */
+	UavcanNode *inst = UavcanNode::instance();
+
+	if (!inst) {
+		errx(1, "application not running");
+	}
+
+	if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
+		
+			inst->print_info();
+	}
+
 	return 0;
 }
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 857c1dc66..97598ddf3 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -66,10 +66,12 @@ 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();
 
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+
 	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
 
 	Node&		getNode() { return _node; }
@@ -84,10 +86,13 @@ public:
 	int		teardown();
 	int		arm_actuators(bool arm);
 
+	void		print_info();
+
+	static UavcanNode* instance() { return _instance; }
+
 private:
 	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