Jelajahi Sumber

generalised the LED support

and support WS2812 LEDs
Andrew Tridgell 3 tahun lalu
induk
melakukan
8b9a2b7a53

+ 2 - 1
RemoteIDModule/Makefile

@@ -36,6 +36,7 @@ setup:
 	@echo "Installing ESP32 support"
 	$(ARDUINO_CLI) core update-index --config-file arduino-cli.yaml
 	$(ARDUINO_CLI) core install esp32:esp32@$(ESP32_VER)
+	$(ARDUINO_CLI) lib install "Adafruit NeoPixel"
 
 headers:
 	@../scripts/git-version.sh
@@ -48,7 +49,7 @@ ArduRemoteID-%.bin: *.cpp *.ino *.h romfs_files.h
 	@echo "Building $* on $(CHIP)"
 	@BUILD_FLAGS="-DBOARD_$*"
 	@rm -rf build build-$*
-	@$(ARDUINO_CLI) compile -b esp32:esp32:$(CHIP) --export-binaries --build-property build.extra_flags=-DBOARD_$* --build-property upload.maximum_size=$(APP_PARTITION_SIZE)
+	@$(ARDUINO_CLI) compile -b esp32:esp32:$(CHIP) --export-binaries --build-property build.extra_flags="-DBOARD_$* -DESP32" --build-property upload.maximum_size=$(APP_PARTITION_SIZE)
 	@cp build/esp32.esp32.$(CHIP)/RemoteIDModule.ino.bin ArduRemoteID_$*_OTA.bin
 	@echo "Merging $*"
 	@python3 $(ESPTOOL) --chip $(CHIP) merge_bin -o ArduRemoteID-$*.bin --flash_size 4MB 0x0 build/esp32.esp32.$(CHIP)/RemoteIDModule.ino.bootloader.bin 0x8000 build/esp32.esp32.$(CHIP)/RemoteIDModule.ino.partitions.bin 0xe000 $(ESP32_TOOLS)/partitions/boot_app0.bin 0x10000 build/esp32.esp32.$(CHIP)/RemoteIDModule.ino.bin

+ 10 - 33
RemoteIDModule/RemoteIDModule.ino

@@ -21,6 +21,7 @@
 #include "check_firmware.h"
 #include <esp_ota_ops.h>
 #include "efuse.h"
+#include "led.h"
 
 #if AP_DRONECAN_ENABLED
 static DroneCAN dronecan;
@@ -44,7 +45,6 @@ static WebInterface webif;
 #include "soc/soc.h"
 #include "soc/rtc_cntl_reg.h"
 
-static uint32_t last_led_trig_ms;
 static bool arm_check_ok = false; // goes true for LED arm check status
 static bool pfst_check_ok = false; 
 
@@ -58,6 +58,9 @@ void setup()
 
     g.init();
 
+    led.set_state(Led::LedState::INIT);
+    led.update();
+
     // Serial for debug printf
     Serial.begin(g.baudrate);
 
@@ -97,11 +100,10 @@ void setup()
     digitalWrite(PIN_CAN_TERM, HIGH);
 #endif
 
-#ifdef PIN_STATUS_LED
-    // LED off if good to arm
     pfst_check_ok = true;   // note - this will need to be expanded to better capture PFST test status
-    pinMode(PIN_STATUS_LED, OUTPUT);
-#endif
+
+    // initially set LED for fail
+    led.set_state(Led::LedState::ARM_FAIL);
 
     esp_log_level_set("*", ESP_LOG_DEBUG);
 
@@ -230,6 +232,7 @@ static void set_data(Transport &t)
     t.set_parse_fail(reason);
 
     arm_check_ok = (reason==nullptr);
+    led.set_state(pfst_check_ok && arm_check_ok? Led::LedState::ARM_OK : Led::LedState::ARM_FAIL);
 
     uint32_t now_ms = millis();
     uint32_t location_age_ms = now_ms - t.get_last_location_ms();
@@ -239,31 +242,6 @@ static void set_data(Transport &t)
     }
 }
 
-void set_output_led(uint32_t _now) {
-#ifdef PIN_STATUS_LED
-#ifdef LED_MODE_FLASH
-    // LED off if good to arm
-    //pinMode(PIN_STATUS_LED, OUTPUT);
-    
-    if (arm_check_ok && pfst_check_ok) {
-        digitalWrite(PIN_STATUS_LED, STATUS_LED_ON);
-        last_led_trig_ms = 0; 
-    } else if (!arm_check_ok && pfst_check_ok) {
-        if (_now - last_led_trig_ms > 100) {
-            digitalWrite(PIN_STATUS_LED,!digitalRead(PIN_STATUS_LED));
-            last_led_trig_ms = _now;
-        }
-    } else {
-        digitalWrite(PIN_STATUS_LED, STATUS_LED_ON);
-        last_led_trig_ms = 0;
-    }
-#else
-    // Default LED Behavior
-    digitalWrite(PIN_STATUS_LED,arm_check_ok?!STATUS_LED_ON:STATUS_LED_ON);
-#endif // LED_MODE_FLASH
-#endif // PIN_STATUS_LED
-}
-
 static uint8_t loop_counter = 0;
 
 void loop()
@@ -296,6 +274,8 @@ void loop()
     const uint32_t last_location_ms = transport.get_last_location_ms();
     const uint32_t last_system_ms = transport.get_last_system_ms();
 
+    led.update();
+
     if (g.bcast_powerup) {
         // if we are broadcasting on powerup we always mark location valid
         // so the location with default data is sent
@@ -349,9 +329,6 @@ void loop()
         ble.transmit_legacy_name(UAS_data);
     }
 
-#ifdef PIN_STATUS_LED
-    set_output_led(now_ms);
-#endif 
     // sleep for a bit for power saving
     delay(1);
 }

+ 10 - 3
RemoteIDModule/board_config.h

@@ -12,6 +12,8 @@
 #define PIN_UART_TX 18
 #define PIN_UART_RX 17
 
+#define WS2812_LED_PIN GPIO_NUM_48
+
 #elif defined(BOARD_ESP32C3_DEV)
 #define BOARD_ID 2
 #define PIN_CAN_TX GPIO_NUM_5
@@ -20,6 +22,8 @@
 #define PIN_UART_TX 3
 #define PIN_UART_RX 2
 
+#define WS2812_LED_PIN GPIO_NUM_8
+
 #elif defined(BOARD_BLUEMARK_DB200)
 #define BOARD_ID 3
 
@@ -37,7 +41,8 @@
 #define PIN_UART_RX 2
 
 #define PIN_STATUS_LED GPIO_NUM_8 // LED to signal the status
-#define STATUS_LED_ON 1
+// LED off when ready to arm
+#define STATUS_LED_OK 0
 
 #elif defined(BOARD_BLUEMARK_DB110)
 #define BOARD_ID 4
@@ -45,7 +50,8 @@
 #define PIN_UART_RX 4
 
 #define PIN_STATUS_LED GPIO_NUM_8 // LED to signal the status
-#define STATUS_LED_ON 1
+// LED off when ready to arm
+#define STATUS_LED_OK 0
 
 #elif defined(BOARD_JW_TBD)
 #define BOARD_ID 5
@@ -57,7 +63,8 @@
 
 #define PIN_STATUS_LED GPIO_NUM_5 // LED to signal the status
 #define LED_MODE_FLASH 1 // flashing LED configuration
-#define STATUS_LED_ON 1
+// LED on when ready to arm
+#define STATUS_LED_OK 1
 
 #define CAN_APP_NODE_NAME "JW TBD"
 #else

+ 61 - 0
RemoteIDModule/led.cpp

@@ -0,0 +1,61 @@
+#include <Arduino.h>
+#include "led.h"
+#include "board_config.h"
+
+Led led;
+
+void Led::init(void)
+{
+    if (done_init) {
+        return;
+    }
+    done_init = true;
+#ifdef PIN_STATUS_LED
+    pinMode(PIN_STATUS_LED, OUTPUT);
+#endif
+#ifdef WS2812_LED_PIN
+    pinMode(WS2812_LED_PIN, OUTPUT);
+    ledStrip.begin();
+#endif
+}
+
+void Led::update(void)
+{
+    const uint32_t now_ms = millis();
+
+#ifdef PIN_STATUS_LED
+    switch (state) {
+    case LedState::ARM_OK: {
+        digitalWrite(PIN_STATUS_LED, STATUS_LED_OK);
+        last_led_trig_ms = now_ms;
+        break;
+    }
+
+    default:
+        if (now_ms - last_led_trig_ms > 100) {
+            digitalWrite(PIN_STATUS_LED, !digitalRead(PIN_STATUS_LED));
+            last_led_trig_ms = now_ms;
+        }
+        break;
+    }
+#endif
+
+#ifdef WS2812_LED_PIN
+    ledStrip.clear();
+
+    switch (state) {
+    case LedState::ARM_OK:
+        ledStrip.setPixelColor(0, ledStrip.Color(0, 255, 0));
+        break;
+
+    default:
+        ledStrip.setPixelColor(0, ledStrip.Color(255, 0, 0));
+        break;
+    }
+    if (now_ms - last_led_strip_ms >= 200) {
+        last_led_strip_ms = now_ms;
+        ledStrip.show();
+    }
+#endif
+}
+

+ 36 - 0
RemoteIDModule/led.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <stdint.h>
+#include "board_config.h"
+
+#ifdef WS2812_LED_PIN
+#include <Adafruit_NeoPixel.h>
+#endif
+
+class Led {
+public:
+    enum class LedState {
+        INIT=0,
+        PFST_FAIL,
+        ARM_FAIL,
+        ARM_OK
+    };
+
+    void set_state(LedState _state) {
+        state = _state;
+    }
+    void update(void);
+
+private:
+    void init(void);
+    bool done_init;
+    uint32_t last_led_trig_ms;
+    LedState state;
+
+#ifdef WS2812_LED_PIN
+    uint32_t last_led_strip_ms;
+    Adafruit_NeoPixel ledStrip{1, WS2812_LED_PIN, NEO_GRB + NEO_KHZ800};
+#endif
+};
+
+extern Led led;

+ 4 - 4
RemoteIDModule/parameters.cpp

@@ -64,10 +64,10 @@ int16_t Parameters::param_index_float(const Parameters::Param *f)
 {
     uint16_t count = 0;
     for (const auto &p : params) {
-    switch (p.ptype) {
         if (p.flags & PARAM_FLAG_HIDDEN) {
             continue;
         }
+        switch (p.ptype) {
         case ParamType::UINT8:
         case ParamType::UINT32:
         case ParamType::FLOAT:
@@ -76,7 +76,7 @@ int16_t Parameters::param_index_float(const Parameters::Param *f)
             }
             count++;
             break;
-    }
+        }
     }
     return -1;
 }
@@ -112,10 +112,10 @@ const Parameters::Param *Parameters::find_by_index_float(uint16_t index)
 {
     uint16_t count = 0;
     for (const auto &p : params) {
-    switch (p.ptype) {
         if (p.flags & PARAM_FLAG_HIDDEN) {
             continue;
         }
+        switch (p.ptype) {
         case ParamType::UINT8:
         case ParamType::UINT32:
         case ParamType::FLOAT:
@@ -124,7 +124,7 @@ const Parameters::Param *Parameters::find_by_index_float(uint16_t index)
             }
             count++;
             break;
-    }
+        }
     }
     return nullptr;
 }