Cannode Demo is ready

4 files changed, 28 insertions, 21 deletions

diff --git a/src/modules/uavcannode/indication_controller.cpp b/src/modules/uavcannode/indication_controller.cpp
index 100f43472..3ac31a42b 100644
--- a/src/modules/uavcannode/indication_controller.cpp
+++ b/src/modules/uavcannode/indication_controller.cpp
@@ -43,31 +43,41 @@ unsigned self_light_index = 0;
 
 void cb_light_command(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::LightsCommand>& msg)
 {
+        uavcan::uint32_t red = 0;
+        uavcan::uint32_t green = 0;
+        uavcan::uint32_t blue = 0;
 	for (auto& cmd : msg.commands)
 	{
 		if (cmd.light_id == self_light_index)
 		{
 			using uavcan::equipment::indication::RGB565;
 
-			uavcan::uint32_t red = uavcan::uint32_t(float(cmd.color.red) *
+			red = uavcan::uint32_t(float(cmd.color.red) *
 			                       (255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F);
 
-			uavcan::uint32_t green = uavcan::uint32_t(float(cmd.color.green) *
+			green = uavcan::uint32_t(float(cmd.color.green) *
 			                         (255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F);
 
-			uavcan::uint32_t blue = uavcan::uint32_t(float(cmd.color.blue) *
+			blue = uavcan::uint32_t(float(cmd.color.blue) *
 			                        (255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F);
 
 			red   = uavcan::min<uavcan::uint32_t>(red, 0xFFU);
 			green = uavcan::min<uavcan::uint32_t>(green, 0xFFU);
 			blue  = uavcan::min<uavcan::uint32_t>(blue, 0xFFU);
+		}
 
-			rgb_led(red, green, blue, 0);
+		if (cmd.light_id == self_light_index+1) {
+		        static int c = 0;
+		        if (c++ % 100 == 0) {
+		            ::syslog(LOG_INFO,"rgb:%d %d %d hz %d\n",red, green, blue,  int(cmd.color.red));
+		        }
+			rgb_led(red, green, blue, int(cmd.color.red));
 			break;
 		}
 	}
 }
 }
+
 int init_indication_controller(uavcan::INode& node)
 {
 	static uavcan::Subscriber<uavcan::equipment::indication::LightsCommand> sub_light(node);
diff --git a/src/modules/uavcannode/led.cpp b/src/modules/uavcannode/led.cpp
index af57edd15..0c069471a 100644
--- a/src/modules/uavcannode/led.cpp
+++ b/src/modules/uavcannode/led.cpp
@@ -69,8 +69,8 @@ void rgb_led(int r, int g ,int b, int freqs)
 
   p  = freqs==0 ? p1s+1 : p0p5s/freqs;
 
-  STM32_TIM_SETCOMPARE(tim, 2, (r * p)/255 );
-  STM32_TIM_SETCOMPARE(tim, 1, (b * p)/255);
-  STM32_TIM_SETCOMPARE(tim, 3, (g * p)/255);
+  STM32_TIM_SETCOMPARE(tim, 1, (r * p)/255 );
+  STM32_TIM_SETCOMPARE(tim, 2, (g * p)/255);
+  STM32_TIM_SETCOMPARE(tim, 3, (b * p)/255);
 }
 
diff --git a/src/modules/uavcannode/sim_controller.cpp b/src/modules/uavcannode/sim_controller.cpp
index 816ce2e8d..bd084d3f7 100644
--- a/src/modules/uavcannode/sim_controller.cpp
+++ b/src/modules/uavcannode/sim_controller.cpp
@@ -48,43 +48,40 @@ uavcan::Publisher<uavcan::equipment::esc::Status>* pub_status;
 namespace {
 unsigned self_index = 0;
 int rpm = 0;
-bool on = false;
-
 
 static void cb_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& msg)
 {
 	if (msg.cmd.size() <= self_index) {
-	        on = false;
 		rgb_led(0,0,0,0);
 		return;
 	}
 
 	const float scaled = msg.cmd[self_index] / float(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max());
 
-        ::syslog(LOG_INFO,"scaled:%09.6f\n",(double)scaled);
+        static int c = 0;
+        if (c++ % 100 == 0) {
+            ::syslog(LOG_INFO,"scaled:%d\n",(int)scaled);
+        }
 	if (scaled > 0) {
-            on = true;
-	    rgb_led(255,0,0,0);
 	} else {
-            on = false;
-	    rgb_led(0,0,0,0);
 	}
 }
 
 static void cb_rpm_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RPMCommand>& msg)
 {
 	if (msg.rpm.size() <= self_index) {
-	        rgb_led(0,0,0,0);
 		return;
 	}
 
 	rpm = msg.rpm[self_index];
-	::syslog(LOG_INFO,"rpm:%d\n",rpm);
+        static int c = 0;
+        if (c++ % 100 == 0) {
+            ::syslog(LOG_INFO,"rpm:%d\n",rpm);
+        }
+
 	if (rpm > 0) {
-            on = true;
             rgb_led(255,0,0,rpm);
 	} else {
-            on = false;
 	     rgb_led(0,0,0,0);
 	}
 }
@@ -101,7 +98,7 @@ void cb_10Hz(const uavcan::TimerEvent& event)
 	msg.power_rating_pct = static_cast<unsigned>(.5F * 100 + 0.5F);
 	msg.error_count = 0;
 
-	if (!on) {
+	if (rpm != 0) {
 		// Lower the publish rate to 1Hz if the motor is not running
 		static uavcan::MonotonicTime prev_pub_ts;
 		if ((event.scheduled_time - prev_pub_ts).toMSec() >= 990) {
diff --git a/src/modules/uavcannode/uavcannode_main.hpp b/src/modules/uavcannode/uavcannode_main.hpp
index 7b3d9e96b..d35c48c90 100644
--- a/src/modules/uavcannode/uavcannode_main.hpp
+++ b/src/modules/uavcannode/uavcannode_main.hpp
@@ -58,7 +58,7 @@ class UavcanNode : public device::CDev
 {
 	static constexpr unsigned MemPoolSize        = 2800; ///< Refer to the libuavcan manual to learn why
 	static constexpr unsigned RxQueueLenPerIface = 64;
-	static constexpr unsigned StackSize          = 1024;
+	static constexpr unsigned StackSize          = 1064;
 
 public:
 	typedef uavcan::Node<MemPoolSize> Node;