From 8a689b2cd19d9151add29c0d8b1f49a0d40a0cbc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 10 Sep 2019 07:37:42 +1000
Subject: [PATCH] AP_Periph: support neopixel LEDs on CAN GPS

---
 Tools/AP_Periph/AP_Periph.cpp |  5 +++++
 Tools/AP_Periph/can.cpp       | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 38 insertions(+)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index c9b2d04f56..d358133254 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -67,6 +67,11 @@ void AP_Periph_FW::init()
     baro.init();
     baro.calibrate(false);
 #endif
+
+#ifdef HAL_PERIPH_NEOPIXEL_COUNT
+    hal.rcout->init();
+    hal.rcout->set_neopixel_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, HAL_PERIPH_NEOPIXEL_COUNT);
+#endif
 }
 
 void AP_Periph_FW::update()
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index bb53ef9968..e86836ea75 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -33,6 +33,7 @@
 #include <uavcan/equipment/air_data/StaticPressure.h>
 #include <uavcan/equipment/air_data/StaticTemperature.h>
 #include <uavcan/equipment/indication/BeepCommand.h>
+#include <uavcan/equipment/indication/LightsCommand.h>
 #include <ardupilot/indication/SafetyState.h>
 #include <ardupilot/indication/Button.h>
 #include <uavcan/protocol/debug/LogMessage.h>
@@ -405,6 +406,27 @@ static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer)
     safety_state = req.status;
 }
 
+#ifdef HAL_PERIPH_NEOPIXEL_COUNT
+/*
+  handle lightscommand
+ */
+static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    uavcan_equipment_indication_LightsCommand req;
+    uint8_t arraybuf[UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_MAX_SIZE];
+    uint8_t *arraybuf_ptr = arraybuf;
+    if (uavcan_equipment_indication_LightsCommand_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) {
+        return;
+    }
+    for (uint8_t i=0; i<req.commands.len; i++) {
+        uavcan_equipment_indication_SingleLightCommand &cmd = req.commands.data[i];
+        hal.rcout->set_neopixel_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN, (1U<<HAL_PERIPH_NEOPIXEL_COUNT)-1,
+                                        cmd.color.red, cmd.color.green, cmd.color.blue);
+    }
+    hal.rcout->neopixel_send();
+}
+#endif
+
 /*
   update safety LED
  */
@@ -525,6 +547,12 @@ static void onTransferReceived(CanardInstance* ins,
         handle_safety_state(ins, transfer);
         break;
 #endif
+
+#ifdef HAL_PERIPH_NEOPIXEL_COUNT
+    case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID:
+        handle_lightscommand(ins, transfer);
+        break;
+#endif
     }
 }
 
@@ -583,6 +611,11 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
     case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
         *out_data_type_signature = ARDUPILOT_INDICATION_SAFETYSTATE_SIGNATURE;
         return true;
+#endif
+#ifdef HAL_PERIPH_NEOPIXEL_COUNT
+    case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID:
+        *out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_SIGNATURE;
+        return true;
 #endif
     default:
         break;