PX4 Cannode Demo DO NOT MERGE

4 files changed, 87 insertions, 17 deletions

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 51589e43e..c5cf902f1 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -45,10 +45,12 @@
 
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
-	_uavcan_pub_raw_cmd(node),
+        _uavcan_pub_raw_cmd(node),
 	_uavcan_sub_status(node),
-	_orb_timer(node)
+	_orb_timer(node),
+        _uavcan_pub_rgb_cmd(node)
 {
+        _led_test = false;
 	if (_perfcnt_invalid_input == nullptr) {
 		errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
 	}
@@ -137,6 +139,23 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	 * Note that for a quadrotor it takes one CAN frame
 	 */
 	(void)_uavcan_pub_raw_cmd.broadcast(msg);
+
+
+	if (_led_test) {
+            uavcan::equipment::indication::LightsCommand rgb;
+            uavcan::equipment::indication::SingleLightCommand sc;
+            sc.color.red = _red;
+            sc.color.blue = _blue;
+            sc.color.green = _green;
+            sc.light_id = 0;
+            rgb.commands.push_back(sc);
+            sc.light_id = 1;
+            sc.color.red = _Hz;
+            sc.color.green = _Hz;
+            sc.color.blue = _Hz;
+            rgb.commands.push_back(sc);
+            (void)_uavcan_pub_rgb_cmd.broadcast(rgb);
+	}
 }
 
 void UavcanEscController::arm_all_escs(bool arm)
@@ -150,11 +169,22 @@ void UavcanEscController::arm_all_escs(bool arm)
 
 void UavcanEscController::arm_single_esc(int num, bool arm)
 {
-	if (arm) {
-		_armed_mask = MOTOR_BIT(num);
-	} else {
-		_armed_mask = 0;
-	}
+        if (arm) {
+                _armed_mask = MOTOR_BIT(num);
+        } else {
+                _armed_mask = 0;
+        }
+}
+
+void UavcanEscController::leds(int r, int g, int b, int hz)
+{
+      _led_test = false;
+      _red = r;
+      _green = g;
+      _blue = b;
+      _Hz = hz;
+      _led_test =  r || g || b || hz;
+
 }
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index 12c035542..af742e00f 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -46,6 +46,7 @@
 
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/indication/LightsCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
 #include <systemlib/perf_counter.h>
 #include <uORB/topics/esc_status.h>
@@ -63,6 +64,7 @@ public:
 
 	void arm_all_escs(bool arm);
 	void arm_single_esc(int num, bool arm);
+        void leds(int r, int g, int b, int hz);
 
 private:
 	/**
@@ -90,14 +92,21 @@ private:
 	esc_status_s	_esc_status = {};
 	orb_advert_t	_esc_status_pub = -1;
 
+        bool                    _led_test;
+        uint8_t                 _red;
+        uint8_t                 _green;
+        uint8_t                 _blue;
+        uint8_t                 _Hz;
+
 	/*
 	 * libuavcan related things
 	 */
 	uavcan::MonotonicTime							_prev_cmd_pub;   ///< rate limiting
 	uavcan::INode								&_node;
-	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
+        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;
+        uavcan::Publisher<uavcan::equipment::indication::LightsCommand>         _uavcan_pub_rgb_cmd;
 
 	/*
 	 * ESC states
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index b93a95f96..58d53ef2e 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -514,9 +514,17 @@ UavcanNode::teardown()
 int
 UavcanNode::arm_actuators(bool arm)
 {
-	_is_armed = arm;
-	_esc_controller.arm_all_escs(arm);
-	return OK;
+        _is_armed = arm;
+        _esc_controller.arm_all_escs(arm);
+        return OK;
+}
+
+int
+UavcanNode::leds(int r, int g, int b, int hz)
+{
+
+        _esc_controller.leds(r, g, b, hz);
+        return OK;
 }
 
 void
@@ -684,7 +692,7 @@ UavcanNode::print_info()
 static void print_usage()
 {
 	warnx("usage: \n"
-	      "\tuavcan {start|status|stop|arm|disarm}");
+	      "\tuavcan {start|status|stop|arm|disarm|led r g b hz}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -731,10 +739,32 @@ int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}
 
-	if (!std::strcmp(argv[1], "arm")) {
-		inst->arm_actuators(true);
-		::exit(0);
-	}
+        if (!std::strcmp(argv[1], "arm")) {
+                inst->arm_actuators(true);
+                ::exit(0);
+        }
+
+        if (!std::strcmp(argv[1], "led")) {
+
+            int r = 255;
+            int g = 255;
+            int b = 255;
+            int hz = 2;
+            if (argc > 2) {
+                r  = atol(argv[2]);
+            }
+            if (argc > 3) {
+                g  = atol(argv[3]);
+            }
+            if (argc > 4) {
+                b  = atol(argv[4]);
+            }
+            if (argc > 5) {
+                hz  = atol(argv[5]);
+            }
+            inst->leds(r, g, b, hz);
+            ::exit(0);
+        }
 
 	if (!std::strcmp(argv[1], "disarm")) {
 		inst->arm_actuators(false);
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 19b9b4b48..0b22fd31c 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -90,7 +90,8 @@ public:
 	void		subscribe();
 
 	int		teardown();
-	int		arm_actuators(bool arm);
+        int             arm_actuators(bool arm);
+        int             leds(int r, int g, int b, int hz = 2);
 
 	void		print_info();