From cfdbcb8dbfc0e500048c6d8fa94b4b80e802626d Mon Sep 17 00:00:00 2001
From: Arnaud Taffanel <arnaud@bitcraze.io>
Date: Wed, 3 Feb 2021 17:55:25 +0100
Subject: [PATCH] #679: Add support for setting Lighthouse deck LEDs

---
 .../interface/lighthouse/lighthouse_core.h      | 17 +++++++++++++++++
 src/modules/src/lighthouse/lighthouse_core.c    | 15 +++++++++++++++
 2 files changed, 32 insertions(+)

diff --git a/src/modules/interface/lighthouse/lighthouse_core.h b/src/modules/interface/lighthouse/lighthouse_core.h
index c3ffac6eb5..ba6d2ffe21 100644
--- a/src/modules/interface/lighthouse/lighthouse_core.h
+++ b/src/modules/interface/lighthouse/lighthouse_core.h
@@ -62,3 +62,20 @@ void lighthouseCoreSetCalibrationData(const uint8_t baseStation, const lighthous
  * @return true if data was stored
  */
 bool lighthouseCorePersistData(const uint8_t baseStation, const bool geoData, const bool calibData);
+
+
+typedef enum {
+  lh_led_off = 0,
+  lh_led_slow_blink = 1,
+  lh_led_fast_blink = 2,
+  lh_led_on = 3,
+} lighthouseCoreLedState_t;
+
+/**
+ * @brief Set LEDs on the lighthouse deck
+ *
+ * @param red  State of the red LED
+ * @param orange State of the orange LED
+ * @param green State of the green LED
+ */
+void lighthouseCoreSetLeds(lighthouseCoreLedState_t red, lighthouseCoreLedState_t orange, lighthouseCoreLedState_t green);
diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c
index d91ddea555..81107142d8 100644
--- a/src/modules/src/lighthouse/lighthouse_core.c
+++ b/src/modules/src/lighthouse/lighthouse_core.c
@@ -192,6 +192,16 @@ TESTABLE_STATIC void waitForUartSynchFrame() {
   }
 }
 
+void lighthouseCoreSetLeds(lighthouseCoreLedState_t red, lighthouseCoreLedState_t orange, lighthouseCoreLedState_t green)
+{
+  uint8_t commandBuffer[2];
+  
+  commandBuffer[0] = 0x01;
+  commandBuffer[1] = (green<<4) | (orange<<2) | red;
+
+  uart1SendData(2, commandBuffer);
+}
+
 
 // Method used to estimate position
 // 0 = Position calculated outside the estimator using intersection point of beams.
@@ -377,6 +387,11 @@ void lighthouseCoreTask(void *param) {
 
   lighthouseDeckFlasherCheckVersionAndBoot();
 
+  vTaskDelay(M2T(100));
+
+  // ToDo: LED should be set according to the current system state
+  lighthouseCoreSetLeds(lh_led_off, lh_led_slow_blink, lh_led_off);
+
   memset(&bsIdentificationData, 0, sizeof(bsIdentificationData));
 
   while(1) {