NeoPixel Led support (#168)

* ws8212 led

* led strip

* log on button status led + set home

* flashfs fix unfinished log

Author: rtlopez

lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp

@@ -20,10 +20,10 @@ const FlashfsRuntime * flashfsGetRuntime()
     return &flashfs;
 }
 
-static uint32_t IRAM_ATTR flashfsJournalAddress(size_t index)
+static uint32_t IRAM_ATTR flashfsJournalAddress(size_t index, bool getEnd = false)
 {
-    uint32_t base = reinterpret_cast<const esp_partition_t*>(flashfs.partition)->size - FLASHFS_JOURNAL_SIZE;
-    return base + (index * sizeof(FlashfsJournalItem));
+    uint32_t journalBase = reinterpret_cast<const esp_partition_t*>(flashfs.partition)->size - FLASHFS_JOURNAL_SIZE;
+    return journalBase + (index * sizeof(FlashfsJournalItem)) + (getEnd * sizeof(uint32_t));
 }
 
 void flashfsJournalLoad(FlashfsJournalItem * data, size_t start, size_t num)
@@ -32,22 +32,6 @@ void flashfsJournalLoad(FlashfsJournalItem * data, size_t start, size_t num)
     flashfsReadAbs(flashfsJournalAddress(start), (uint8_t*)data, size);
 }
 
-static uint32_t flashfsLogLoad()
-{
-    if(!flashfs.partition) return 0;
-
-    uint32_t address = 0;
-    flashfsJournalLoad(flashfs.journal, 0, FLASHFS_JOURNAL_ITEMS);
-    for(size_t i = 0; i < FLASHFS_JOURNAL_ITEMS; i++)
-    {
-        const auto& it = flashfs.journal[i];
-        if(it.logEnd == FLASHFS_ERASED_VAL) break;
-        address = it.logEnd;
-        flashfs.journalIdx++;
-    }
-    return address;
-}
-
 static void IRAM_ATTR flashfsLogBegin()
 {
     uint32_t beginAddr = flashfs.address;
@@ -78,19 +62,55 @@ static void IRAM_ATTR flashfsLogEnd()
 
     if(!flashfs.partition) return;
 
-    size_t address = flashfsJournalAddress(idx) + sizeof(uint32_t);
+    size_t address = flashfsJournalAddress(idx, true);
 
     flashfsWriteAbs(address, (uint8_t*)&endAddr, sizeof(uint32_t));
 }
 
+void flashfsJournalFix(FlashfsJournalItem * data, size_t num)
+{
+    uint8_t buff[4];
+    for(size_t i = 0; i < num; i++)
+    {
+        if(data[i].logBegin != FLASHFS_ERASED_VAL && data[i].logEnd == FLASHFS_ERASED_VAL)
+        {
+            uint32_t addr = data[i].logBegin;
+            uint32_t end = flashfsGetSize();
+            while(addr < end)
+            {
+                flashfsReadAbs(addr, buff, 4);
+                if(*reinterpret_cast<uint32_t*>(&buff[0]) == FLASHFS_ERASED_VAL)
+                {
+                    flashfs.address = addr;
+                    flashfsLogEnd();
+                    break;
+                }
+                addr += 128;
+            }
+            break;
+        }
+    }
+}
+
 int flashfsInit(void)
 {
     flashfs.partition = Espfc::Device::FlashDevice::findPartition();
     if(!flashfs.partition) return 0;
 
     flashfs.buffer = (void*)&buff;
     flashfs.journalIdx = 0;
-    flashfs.address = flashfsLogLoad();
+    flashfs.address = 0;
+
+    flashfsJournalLoad(flashfs.journal, 0, FLASHFS_JOURNAL_ITEMS);
+    for(size_t i = 0; i < FLASHFS_JOURNAL_ITEMS; i++)
+    {
+        const auto& it = flashfs.journal[i];
+        if(it.logEnd == FLASHFS_ERASED_VAL) break;
+        flashfs.address = it.logEnd;
+        flashfs.journalIdx++;
+    }
+    flashfsJournalFix(flashfs.journal, FLASHFS_JOURNAL_ITEMS);
+
     return 1;
 }
 
@@ -161,7 +181,7 @@ void IRAM_ATTR flashfsWriteAbs(uint32_t address, const uint8_t *data, unsigned i
     esp_partition_write_raw(p, address, data, len);
 }
 
-int flashfsReadAbs(uint32_t address, uint8_t *data, unsigned int len)
+int IRAM_ATTR flashfsReadAbs(uint32_t address, uint8_t *data, unsigned int len)
 {
     if(!flashfs.partition) return 0;
 

lib/Espfc/src/Connect/Cli.cpp

@@ -356,6 +356,7 @@ const Cli::Param * Cli::initialize(ModelConfig& c)
   static const char* currentSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL };
   static const char* blackboxDevChoices[] = { PSTR("NONE"), PSTR("FLASH"), PSTR("SD_CARD"), PSTR("SERIAL"), NULL };
   static const char* blackboxModeChoices[] = { PSTR("NORMAL"), PSTR("TEST"), PSTR("ALWAYS"), NULL };
+  static const char* ledTypeChoices[] = { PSTR("SIMPLE"), PSTR("STRIP"), NULL };
 
   size_t i = 0;
   static const Param params[] = {
@@ -677,6 +678,7 @@ const Cli::Param * Cli::initialize(ModelConfig& c)
 #endif
     Param(PSTR("pin_buzzer_invert"), &c.buzzer.inverted),
     Param(PSTR("pin_led_invert"), &c.led.invert),
+    Param(PSTR("pin_led_type"), &c.led.type, ledTypeChoices),
 
 #ifdef ESPFC_I2C_0
     Param(PSTR("i2c_speed"), &c.i2cSpeed),

lib/Espfc/src/Connect/StatusLed.cpp

@@ -1,6 +1,97 @@
 #include "StatusLed.hpp"
+#include "Target/Target.h"
 #include <Arduino.h>
 
+#ifdef ESPFC_LED_WS2812
+#include "driver/i2s.h"
+
+// https://docs.espressif.com/projects/esp-idf/en/v4.4.4/esp32/api-reference/peripherals/i2s.html
+// https://github.com/vunam/esp32-i2s-ws2812/blob/master/ws2812.c
+
+static constexpr size_t LED_NUMBER = 1;
+static constexpr size_t PIXEL_SIZE = 12; // each colour takes 4 bytes in buffer
+static constexpr size_t ZERO_BUFFER = 32;
+static constexpr size_t SIZE_BUFFER = LED_NUMBER * PIXEL_SIZE + ZERO_BUFFER;
+static constexpr uint32_t SAMPLE_RATE = 93750;
+static constexpr i2s_port_t I2S_NUM = I2S_NUM_0;
+
+typedef struct {
+  uint8_t g;
+  uint8_t r;
+  uint8_t b;
+} ws2812_pixel_t;
+
+static i2s_config_t i2s_config = {
+  .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),
+  .sample_rate = SAMPLE_RATE,
+  .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT,
+  .channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,
+  .communication_format = I2S_COMM_FORMAT_STAND_MSB,
+  .intr_alloc_flags = 0,
+  .dma_buf_count = 2,
+  .dma_buf_len = SIZE_BUFFER / 2,
+  .use_apll = false,
+  .tx_desc_auto_clear = false,
+  .fixed_mclk = 0,
+  .mclk_multiple = I2S_MCLK_MULTIPLE_DEFAULT,
+  .bits_per_chan = I2S_BITS_PER_CHAN_DEFAULT,
+};
+
+static i2s_pin_config_t pin_config = {
+  .bck_io_num = -1,
+  .ws_io_num = -1,
+  .data_out_num = -1,
+  .data_in_num = -1
+};
+
+static uint8_t out_buffer[SIZE_BUFFER] = {0};
+
+static const uint16_t bitpatterns[4] = {0x88, 0x8e, 0xe8, 0xee};
+
+static void ws2812_init(int8_t pin)
+{
+  pin_config.data_out_num = pin;
+  i2s_driver_install(I2S_NUM, &i2s_config, 0, NULL);
+  i2s_set_pin(I2S_NUM, &pin_config);
+  i2s_zero_dma_buffer(I2S_NUM);
+  std::fill_n(out_buffer, SIZE_BUFFER, 0);
+}
+
+static void ws2812_write_pixel(uint8_t * buffer, const ws2812_pixel_t& pixel)
+{
+  *buffer++ = bitpatterns[pixel.g >> 6 & 0x03];
+  *buffer++ = bitpatterns[pixel.g >> 4 & 0x03];
+  *buffer++ = bitpatterns[pixel.g >> 2 & 0x03];
+  *buffer++ = bitpatterns[pixel.g >> 0 & 0x03];
+
+  *buffer++ = bitpatterns[pixel.r >> 6 & 0x03];
+  *buffer++ = bitpatterns[pixel.r >> 4 & 0x03];
+  *buffer++ = bitpatterns[pixel.r >> 2 & 0x03];
+  *buffer++ = bitpatterns[pixel.r >> 0 & 0x03];
+
+  *buffer++ = bitpatterns[pixel.b >> 6 & 0x03];
+  *buffer++ = bitpatterns[pixel.b >> 4 & 0x03];
+  *buffer++ = bitpatterns[pixel.b >> 2 & 0x03];
+  *buffer++ = bitpatterns[pixel.b >> 0 & 0x03];
+}
+
+static void ws2812_update(const ws2812_pixel_t * pixels)
+{
+  size_t bytes_written = 0;
+  for (size_t i = 0; i < LED_NUMBER; i++)
+  {
+    size_t loc = i * PIXEL_SIZE;
+    ws2812_write_pixel(out_buffer + loc, pixels[i]);
+  }
+  i2s_zero_dma_buffer(I2S_NUM);
+  i2s_write(I2S_NUM, out_buffer, SIZE_BUFFER, &bytes_written, portMAX_DELAY);
+}
+
+static const ws2812_pixel_t PIXEL_ON[] = {{0x40, 0x40, 0x80}};
+static const ws2812_pixel_t PIXEL_OFF[] = {{0, 0, 0}};
+
+#endif
+
 namespace Espfc::Connect
 {
 
@@ -11,12 +102,20 @@ static int LED_ON_PATTERN[] = {100, 0};
 
 StatusLed::StatusLed() : _pin(-1), _invert(0), _status(LED_OFF), _next(0), _state(LOW), _step(0), _pattern(LED_OFF_PATTERN) {}
 
-void StatusLed::begin(int8_t pin, uint8_t invert)
+void StatusLed::begin(int8_t pin, uint8_t type, uint8_t invert)
 {
   if(pin == -1) return;
+
   _pin = pin;
+  _type = type;
   _invert = invert;
+
+#ifdef ESPFC_LED_WS2812
+  if(_type == LED_STRIP) ws2812_init(_pin);
+  if(_type == LED_SIMPLE) pinMode(_pin, OUTPUT);
+#else
   pinMode(_pin, OUTPUT);
+#endif
   setStatus(LED_ON, true);
 }
 
@@ -74,7 +173,12 @@ void StatusLed::update()
 
 void StatusLed::_write(uint8_t val)
 {
+#ifdef ESPFC_LED_WS2812
+  if(_type == LED_STRIP) ws2812_update(val ? PIXEL_ON : PIXEL_OFF);
+  if(_type == LED_SIMPLE) digitalWrite(_pin, val ^ _invert);
+#else
   digitalWrite(_pin, val ^ _invert);
+#endif
 }
 
 }

lib/Espfc/src/Connect/StatusLed.hpp

@@ -5,27 +5,34 @@
 namespace Espfc::Connect
 {
 
+enum LedType
+{
+  LED_SIMPLE,
+  LED_STRIP,
+};
+
 enum LedStatus
 {
   LED_OFF,
   LED_OK,
   LED_ERROR,
-  LED_ON
+  LED_ON,
 };
 
 class StatusLed
 {
 
 public:
   StatusLed();
-  void begin(int8_t pin, uint8_t invert);
+  void begin(int8_t pin, uint8_t type, uint8_t invert);
   void update();
   void setStatus(LedStatus newStatus, bool force = false);
 
 private:
   void _write(uint8_t val);
   int8_t _pin;
-  int8_t _invert;
+  uint8_t _type;
+  uint8_t _invert;
   LedStatus _status;
   uint32_t _next;
   bool _state;

lib/Espfc/src/Control/Actuator.cpp

@@ -260,13 +260,14 @@ void Actuator::updateRescueConfig()
 
 void Actuator::updateLed()
 {
-  if(_model.armingDisabled())
+  if(_model.isModeActive(MODE_ARMED) || _model.state.mode.isLongClickActive())
   {
-    _model.state.led.setStatus(Connect::LED_ERROR);
+    if(_model.state.mode.isLongClickActive()) _model.setGpsHome();
+    _model.state.led.setStatus(Connect::LED_ON);
   }
-  else if(_model.isModeActive(MODE_ARMED) || _model.state.mode.isLongClickActive())
+  else if(_model.armingDisabled())
   {
-    _model.state.led.setStatus(Connect::LED_ON);
+    _model.state.led.setStatus(Connect::LED_ERROR);
   }
   else
   {

lib/Espfc/src/Espfc.cpp

@@ -19,7 +19,7 @@ int Espfc::load()
 
 int Espfc::begin()
 {
-  _model.state.led.begin(_model.config.pin[PIN_LED_BLINK], _model.config.led.invert);
+  _model.state.led.begin(_model.config.pin[PIN_LED_BLINK], _model.config.led.type, _model.config.led.invert);
 
   _serial.begin();      // requires _model.load()
   //_model.logStorageResult();

lib/Espfc/src/ModelConfig.h

@@ -668,6 +668,7 @@ struct GpsConfig
 struct LedConfig
 {
   uint8_t invert = 0;
+  int8_t type = 0;
 };
 
 // persistent data

lib/Espfc/src/Target/TargetESP32s2.h

@@ -24,8 +24,8 @@
 #define ESPFC_SERIAL_1
 #define ESPFC_SERIAL_1_DEV Serial1
 #define ESPFC_SERIAL_1_DEV_T HardwareSerial
-#define ESPFC_SERIAL_1_TX 16
-#define ESPFC_SERIAL_1_RX 15
+#define ESPFC_SERIAL_1_TX 17
+#define ESPFC_SERIAL_1_RX 16
 #define ESPFC_SERIAL_1_FN (SERIAL_FUNCTION_RX_SERIAL)
 #define ESPFC_SERIAL_1_BAUD (SERIAL_SPEED_115200)
 #define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE)
@@ -48,17 +48,17 @@
 #define ESPFC_SPI_0_MOSI 11
 #define ESPFC_SPI_0_MISO 13
 
-#define ESPFC_SPI_CS_GYRO 8
+#define ESPFC_SPI_CS_GYRO 10
 #define ESPFC_SPI_CS_BARO 7
 
 #define ESPFC_I2C_0
-#define ESPFC_I2C_0_SCL 10
-#define ESPFC_I2C_0_SDA 9
+#define ESPFC_I2C_0_SCL 9
+#define ESPFC_I2C_0_SDA 8
 #define ESPFC_I2C_0_SOFT
 
 #define ESPFC_BUZZER_PIN 5
 #define ESPFC_BUTTON_PIN -1
-#define ESPFC_LED_PIN -1
+#define ESPFC_LED_PIN 15
 
 #define ESPFC_ADC_0
 #define ESPFC_ADC_0_PIN 1

lib/Espfc/src/Target/TargetEsp32Common.h

@@ -20,6 +20,7 @@
 
 #define ESPFC_WIFI
 #define ESPFC_ESPNOW
+#define ESPFC_LED_WS2812
 
 namespace Espfc {
 

lib/betaflight/src/platform.c

@@ -62,7 +62,7 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS];
 uint32_t targetPidLooptime;
 float rcCommand[4];
 
-int32_t GPS_home[2];
+int32_t GPS_home[2] = { 0, 0 };
 gpsSolutionData_t gpsSol;
 
 const char* const lookupTableMixerType[] = {