ExpressLRS(ELRS) ESP32 受信テストコード

ELRS_RX_SimpleCode.cpp

// ELRS受信テスト

#include <Arduino.h>

#define ELRS_RX_PIN 21
#define ELRS_TX_PIN 22
#define ELRS_BAUD 420000

#define CRSF_DEVICEADDR_FC 0xC8
#define CRSF_FRAME_LENGTH 24
#define CRSF_FRAMETYPE_RC_CHANNELS_PACKED 0x16

typedef struct
{
  unsigned ch0 : 11;
  unsigned ch1 : 11;
  unsigned ch2 : 11;
  unsigned ch3 : 11;
  unsigned ch4 : 11;
  unsigned ch5 : 11;
  unsigned ch6 : 11;
  unsigned ch7 : 11;
} __attribute__((packed)) ELRS_CHANNEL_DATA;

#define CRSF_CRC_POLY 0xd5

// CRC8 計算
static byte getCRC8(byte *buf, size_t size)
{
  byte crc8 = 0x00;

  for (int i = 0; i < size; i++)
  {
    crc8 ^= buf[i];

    for (int j = 0; j < 8; j++)
    {
      if (crc8 & 0x80)
      {
        crc8 <<= 1;
        crc8 ^= CRSF_CRC_POLY;
      }
      else
      {
        crc8 <<= 1;
      }
    }
  }
  return crc8;
}

static ELRS_CHANNEL_DATA elrs;
volatile int packet_count = 0;
static int total_count = 0;
static int error_count = 0;

volatile size_t pps = 0;
int max_len = 0;
hw_timer_t *timer = NULL;
volatile SemaphoreHandle_t timerSemaphore;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
volatile uint32_t isrCounter = 0;
volatile uint32_t lastIsrAt = 0;

hw_timer_t *timer2 = NULL;
volatile SemaphoreHandle_t timerSemaphore2;
portMUX_TYPE timerMux2 = portMUX_INITIALIZER_UNLOCKED;

void IRAM_ATTR onTimer()
{
  // Increment the counter and set the time of ISR
  portENTER_CRITICAL_ISR(&timerMux);
  pps = packet_count;
  packet_count = 0;
  max_len = 0;
  isrCounter = 0;
  portEXIT_CRITICAL_ISR(&timerMux);
  // Give a semaphore that we can check in the loop
  xSemaphoreGiveFromISR(timerSemaphore, NULL);
  // It is safe to use digitalRead/Write here if you want to toggle an output
}
void IRAM_ATTR onTimer2()
{
  // Increment the counter and set the time of ISR
  // Give a semaphore that we can check in the loop
  portENTER_CRITICAL_ISR(&timerMux);
  isrCounter++;
  portEXIT_CRITICAL_ISR(&timerMux);
  xSemaphoreGiveFromISR(timerSemaphore2, NULL);
  // It is safe to use digitalRead/Write here if you want to toggle an output
}

void ELRSInit()
{
  Serial2.begin(ELRS_BAUD, SERIAL_8N1, ELRS_RX_PIN, ELRS_TX_PIN);
}

void ELRSUpdate()
{
  size_t len = Serial2.available();
  if (len > 0)
  {
    if (max_len < len)
    {
      max_len = len;
    }
    byte buf[len];

    Serial2.readBytes(buf, 2);
    total_count++;
    if (
        buf[0] == CRSF_DEVICEADDR_FC)
    {
      size_t frame_len = buf[1];
      Serial2.readBytes(buf, frame_len);

           byte crc = getCRC8(&buf[0], frame_len - 1);
      if (buf[frame_len - 1] == crc)
      {
        //        buf[1] == CRSF_FRAME_LENGTH  )
        if (buf[0] == CRSF_FRAMETYPE_RC_CHANNELS_PACKED)
        {
          portENTER_CRITICAL_ISR(&timerMux);
          packet_count++;
          portEXIT_CRITICAL_ISR(&timerMux);
          memcpy(&elrs, &buf[1], sizeof(ELRS_CHANNEL_DATA));
        }
      }
      else
      {
        // CRC Error!
        error_count++;
      }
    } else {
      error_count++;
    }
  }
}

void ELRSTask(void *param)
{
  static time_t old_time = 0;

  while (1)
  {
      ELRSUpdate();
  }
}

TaskHandle_t elrsTaskHandler;
TaskHandle_t mainTaskHandler;

void setup()
{
  Serial.begin(115200);

  ELRSInit();
  // Create semaphore to inform us when the timer has fired
  timerSemaphore = xSemaphoreCreateBinary();
  timerSemaphore2 = xSemaphoreCreateBinary();
  // Use 1st timer of 4 (counted from zero).
  // Set 80 divider for prescaler (see ESP32 Technical Reference Manual for more
  // info).
  timer = timerBegin(1, 80, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, 1000000, true);

  timer2 = timerBegin(0, 80, true);
  timerAttachInterrupt(timer2, &onTimer2, true);
  timerAlarmWrite(timer2, 1000000 / 4000, true);

  // ウォッチドッグ停止
  disableCore0WDT();
  //  disableCore1WDT();

  xTaskCreatePinnedToCore(
      ELRSTask,         // Task function.
      "ELRSTask",       // name of task.
      10000,            // Stack size of task
      NULL,             // parameter of the task
      -1,               // priority of the task
      &elrsTaskHandler, // Task handle to keep track of created task
      0);               // pin task to core

  timerAlarmEnable(timer);
  timerAlarmEnable(timer2);
}

void loop()
{
  Serial.printf("CH0:%4d CH1:%4d CH2:%4d CH3:%4d %6d/%6d (CRC Err:%d) %dHz %4d %d\n",
                  elrs.ch0, elrs.ch1, elrs.ch2, elrs.ch3, packet_count, total_count, error_count, pps, isrCounter, max_len);

  delay(100);
}