#679: Add support for setting Lighthouse deck LEDs

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);

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) {