---
# System prepended metadata

title: ISP32-RTOS — components 與 main

---

# 「[Avionic 101](https://hackmd.io/@Jackiempty/avionic-101-list)」：ISP32-RTOS — components 與 main

> 目的：把專案 `components/` 和 `main/` 目錄下的程式依檔案與功能分層列出，請逐一貼上原始碼、補註解、做 code review。`.c` / `.h` 原始碼貼到各節的程式區塊中。

---

## 目錄

* [components/](#components)

  * [components/LSM6DSM/](#componentslsm6dsm)
  * [components/ahrs/](#componentsahrs)
  * [components/bsp/](#componentsbsp)
  * [components/lora/](#componentslora)
  * [components/sensors/](#componentssensors)
    * [IMU](#IMU)
    * [BMP280](#BMP280)
    * [GPS](#GPS)
  * [components/storage/](#componentsstorage)
  * [components/slave/](#componentsslave)

* [main/](#main)

  * [components/comm/](#maincomm)
  * [components/fsm/](#mainfsm)
  * [components/recv/](#mainrecv)
  * [components/sensors/](#mainsensors)

---

# components

---
所有在程式中所提及和設定的腳位皆儲存在 main/Kconfig.projbuild 中
## components/lsm6dsm
LSM6DSM.h -> 在github上的開源軟體，提供IMU不同的函數去使用，以下是在程式中使用的函式：
```cpp
lsm6dsm_init() //初始化函式及IMU等內容
readData() //讀取來自IMU的資料
```

## components/ahrs
MadgwickAHRS.h -> 在github上的開源軟體，提供IMU不同的函數去使用，以下是在程式中使用的函式：
```cpp
MadgwickAHRSinit() //初始化函式
MadgwickAHRSupdate() //主更新函式，融合 加速度計、陀螺儀、磁力計 三者數據，更新內部四元數
MadgwickGetEulerAnglesDegrees() //將收到的數值轉換成歐拉角(Roll、Pitch、Yaw)
```

## components/bsp
bsp.h -> 主要對gpio、i2c、uart、spi、sd這些通訊管道做初始化及初始設定，用以簡略其他程式在設定上的內容
詢問chatgpt後得到的答案：https://chatgpt.com/share/68e235ee-8a30-8002-87db-03334af3fbad
```cpp
#include "bsp.h"

#include "driver/gpio.h"

#define TAG "BSP"

static QueueHandle_t uart_queue;
static uint8_t ignitor[] = {GPIO_FIRE_1, GPIO_FIRE_2};//儲存點火腳位

void gpio_init() {
  for (ssize_t i = 0; i < sizeof(ignitor); i++) {
    gpio_set_level(ignitor[i], 1);
    const gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = (1ULL << ignitor[i]),
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_ENABLE,//設定為上拉開關
    };
    gpio_set_level(ignitor[i], 1);
    gpio_config(&io_conf);
  }//逐一設定 ignitor 腳位為 輸出模式

  gpio_set_level(CONFIG_INDI_LED, 0);
  gpio_set_direction(CONFIG_INDI_LED, GPIO_MODE_OUTPUT);
  //設定一顆 指示燈 LED 腳位為輸出並預設熄滅。
    
  gpio_set_level(CONFIG_INT_IO, 1);
  gpio_set_direction(CONFIG_INT_IO, GPIO_MODE_INPUT);
  gpio_pullup_en(CONFIG_INT_IO);
  //設定某個外部中斷輸入腳（CONFIG_INT_IO）為輸入並啟用上拉電阻
}

void i2c_init() {
  static const i2c_config_t conf = {
      .mode = I2C_MODE_MASTER,
      .sda_io_num = I2C_MASTER_SDA_IO,
      .scl_io_num = I2C_MASTER_SCL_IO,
      .sda_pullup_en = GPIO_PULLUP_DISABLE,
      .scl_pullup_en = GPIO_PULLUP_DISABLE,
      .master.clk_speed = I2C_MASTER_FREQ_HZ,
  };//設定i2c
  i2c_param_config(I2C_MASTER_NUM, &conf);//對 I2C 介面參數化設定
  i2c_driver_install(I2C_MASTER_NUM, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}//安裝 I2C 驅動程式

void uart_init() {
  const static uart_config_t uart_config = {
      .baud_rate = 9600,
      .data_bits = UART_DATA_8_BITS,
      .parity = UART_PARITY_DISABLE,
      .stop_bits = UART_STOP_BITS_1,
      .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
      .source_clk = UART_SCLK_DEFAULT,
  };//設定uart(8N1, 9600 baud) -> 用於gps
  if (uart_driver_install(GPS_UART_NUM, 256, 0, 20, &uart_queue, 0) != ESP_OK) printf("UART install failed\n");
  if (uart_param_config(GPS_UART_NUM, &uart_config) != ESP_OK) printf("UART param failed\n");
  if (uart_set_pin(GPS_UART_NUM, 18, 17, -1, -1) != ESP_OK) printf("UART set pin failed\n");
  if (uart_enable_pattern_det_baud_intr(GPS_UART_NUM, '\n', 1, 9, 0, 0) != ESP_OK) printf("UART enable interrupt failed\n");
  if (uart_pattern_queue_reset(GPS_UART_NUM, 20) != ESP_OK) printf("UART pattern failed\n");
  uart_flush(GPS_UART_NUM);
  printf("GPS init...\n");
}

void spi_init(spi_host_device_t device, uint32_t mosi, uint32_t miso, uint32_t sck) {
  esp_err_t ret;
  const spi_bus_config_t spi_bus_config = {
      .sclk_io_num = sck,
      .mosi_io_num = mosi,
      .miso_io_num = miso,
      .quadwp_io_num = -1,
      .quadhd_io_num = -1,
  };
  ret = spi_bus_initialize(device, &spi_bus_config, SPI_DMA_CH_AUTO);
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "Failed to initialize bus.");
    return;
  }
  ESP_LOGI(TAG, "SPI%d_HOST spi_bus_initialize=%d", device + 1, ret);
}

esp_err_t sd_init() {
  esp_err_t err;
  sdmmc_card_t* card;
  const char mount_point[] = SD_MOUNT;
  sdmmc_host_t host = SDSPI_HOST_DEFAULT();
  sdspi_device_config_t slot_config = SDSPI_DEVICE_CONFIG_DEFAULT();
  esp_vfs_fat_sdmmc_mount_config_t mount_config = {
      .format_if_mount_failed = true,
      .max_files = 5,
      .allocation_unit_size = 16 * 1024,
  }; 
  host.slot = SD_SPI_HOST;//用 SPI 模式 初始化 SD 卡
  slot_config.gpio_cs = CONFIG_SD_NSS_GPIO;
  slot_config.host_id = host.slot;
  err = esp_vfs_fat_sdspi_mount(mount_point, &host, &slot_config, &mount_config, &card);
  if (err != ESP_OK) return err;
  sdmmc_card_print_info(stdout, card);
  return ESP_OK;
}
//掛載 FAT 檔案系統。
//若掛載失敗可自動格式化。
//sdmmc_card_print_info(stdout, card) 會印出 SD 卡資訊

uint32_t bsp_current_time() { return xTaskGetTickCount(); }
//回傳 FreeRTOS 的系統 tick，等價於時間戳記

// x    : input
// y    : last output
// fc   : cut-off frequency
// fs   : sampling frequency
float lpf(float x, float y, float fc, float fs) {
  float tau = 1 / (2.0 * M_PI * fc);
  float alpha = (1.0 / fs) / (tau + (1.0 / fs));
  return y + alpha * (x - y);
}//一階低通濾波器

float iir_1st(float x, float y, float a) { return (1 - a) * x + a * y; }//一階I2R濾波器

QueueHandle_t* fetch_uart_queue() { return &uart_queue; }
//回傳指向 uart_queue 的指標，用於任務之間傳遞 UART 事件
```

## components/lora
```cpp
#include "ra01s.h"

#define TAG "RA01S"

static spi_device_handle_t spi_handle;
static uint8_t PacketParams[6];
static bool txActive;
static bool debugPrint;
static int SX126x_SPI_SELECT;
static int SX126x_RESET;
static int SX126x_BUSY;
static int SX126x_TXEN;
static int SX126x_RXEN;

// Arduino compatible macros
#define delay(ms) vTaskDelay(pdMS_TO_TICKS(ms))
//偵測是否有錯誤 錯誤則進無限迴圈
void LoRaError(int error) {
  if (debugPrint) {
    ESP_LOGE(TAG, "LoRaErrorDefault=%d", error);
  }
  while (true) {
    vTaskDelay(1);
  }
}
//初始化
void LoRaInit(void) {
  spi_device_interface_config_t devcfg;
  memset(&devcfg, 0, sizeof(spi_device_interface_config_t));
  devcfg.clock_speed_hz = SPI_FREQ_HZ;
  devcfg.spics_io_num = -1;
  devcfg.queue_size = 5;
  devcfg.mode = 0;
  devcfg.flags = SPI_DEVICE_NO_DUMMY;
  spi_bus_add_device(LORA_SPI_HOST, &devcfg, &spi_handle);

  ESP_LOGI(TAG, "CONFIG_LORA_NSS_GPIO=%d", CONFIG_LORA_NSS_GPIO);
  ESP_LOGI(TAG, "CONFIG_RST_GPIO=%d", CONFIG_RST_GPIO);
  ESP_LOGI(TAG, "CONFIG_BUSY_GPIO=%d", CONFIG_BUSY_GPIO);
  ESP_LOGI(TAG, "CONFIG_TXEN_GPIO=%d", CONFIG_TXEN_GPIO);
  ESP_LOGI(TAG, "CONFIG_RXEN_GPIO=%d", CONFIG_RXEN_GPIO);

  SX126x_SPI_SELECT = CONFIG_LORA_NSS_GPIO;
  SX126x_RESET = CONFIG_RST_GPIO;
  SX126x_BUSY = CONFIG_BUSY_GPIO;
  SX126x_TXEN = CONFIG_TXEN_GPIO;
  SX126x_RXEN = CONFIG_RXEN_GPIO;

  txActive = false;
  debugPrint = false;

  gpio_reset_pin(SX126x_SPI_SELECT);
  gpio_set_direction(SX126x_SPI_SELECT, GPIO_MODE_OUTPUT);
  gpio_set_level(SX126x_SPI_SELECT, 1);

  gpio_reset_pin(SX126x_RESET);
  gpio_set_direction(SX126x_RESET, GPIO_MODE_OUTPUT);

  gpio_reset_pin(SX126x_BUSY);
  gpio_set_direction(SX126x_BUSY, GPIO_MODE_INPUT);

  if (SX126x_TXEN != -1) {
    gpio_reset_pin(SX126x_TXEN);
    gpio_set_direction(SX126x_TXEN, GPIO_MODE_OUTPUT);
  }

  if (SX126x_RXEN != -1) {
    gpio_reset_pin(SX126x_RXEN);
    gpio_set_direction(SX126x_RXEN, GPIO_MODE_OUTPUT);
  }
}

bool spi_write_byte(uint8_t *Dataout, size_t DataLength) {
  spi_transaction_t SPITransaction;

  if (DataLength > 0) {
    memset(&SPITransaction, 0, sizeof(spi_transaction_t));
    SPITransaction.length = DataLength * 8;
    SPITransaction.tx_buffer = Dataout;
    SPITransaction.rx_buffer = NULL;
    spi_device_transmit(spi_handle, &SPITransaction);
  }

  return true;
}

bool spi_read_byte(uint8_t *Datain, uint8_t *Dataout, size_t DataLength) {
  spi_transaction_t SPITransaction;

  if (DataLength > 0) {
    memset(&SPITransaction, 0, sizeof(spi_transaction_t));
    SPITransaction.length = DataLength * 8;
    SPITransaction.tx_buffer = Dataout;
    SPITransaction.rx_buffer = Datain;
    spi_device_transmit(spi_handle, &SPITransaction);
  }

  return true;
}

uint8_t spi_transfer(uint8_t address) {
  uint8_t datain[1];
  uint8_t dataout[1];
  dataout[0] = address;
  // spi_write_byte(dataout, 1 );
  spi_read_byte(datain, dataout, 1);
  return datain[0];
}
//LORA 設定配置
int16_t LoRaBegin(uint32_t frequencyInHz, int8_t txPowerInDbm, float tcxoVoltage, bool useRegulatorLDO) {
  //設定發射功率在-3~22 dBm之間  
  if (txPowerInDbm > 22) txPowerInDbm = 22;
  if (txPowerInDbm < -3) txPowerInDbm = -3;

  Reset();
  ESP_LOGI(TAG, "Reset");

  uint8_t wk[2];
  ReadRegister(SX126X_REG_LORA_SYNC_WORD_MSB, wk, 2);  // 0x0740
  uint16_t syncWord = (wk[0] << 8) + wk[1];
  ESP_LOGI(TAG, "syncWord=0x%x", syncWord);
  if (syncWord != SX126X_SYNC_WORD_PUBLIC && syncWord != SX126X_SYNC_WORD_PRIVATE) {
    ESP_LOGE(TAG, "SX126x error, maybe no SPI connection");
    return ERR_INVALID_MODE;
  }

  ESP_LOGI(TAG, "SX126x installed");
  SetStandby(SX126X_STANDBY_RC);

  SetDio2AsRfSwitchCtrl(true);
  ESP_LOGI(TAG, "tcxoVoltage=%f", tcxoVoltage);
  // set TCXO control, if requested
  if (tcxoVoltage > 0.0) {
    SetDio3AsTcxoCtrl(tcxoVoltage, RADIO_TCXO_SETUP_TIME);  // Configure the radio to use a TCXO controlled by DIO3
  }

  Calibrate(SX126X_CALIBRATE_IMAGE_ON | SX126X_CALIBRATE_ADC_BULK_P_ON | SX126X_CALIBRATE_ADC_BULK_N_ON | SX126X_CALIBRATE_ADC_PULSE_ON |
            SX126X_CALIBRATE_PLL_ON | SX126X_CALIBRATE_RC13M_ON | SX126X_CALIBRATE_RC64K_ON);
 //設定穩壓器模式(LDO or DC-DC)
  ESP_LOGI(TAG, "useRegulatorLDO=%d", useRegulatorLDO);
  if (useRegulatorLDO) {
    SetRegulatorMode(SX126X_REGULATOR_LDO);  // set regulator mode: LDO
  } else {
    SetRegulatorMode(SX126X_REGULATOR_DC_DC);  // set regulator mode: DC-DC
  }

  SetBufferBaseAddress(0, 0);
#if 0
	// SX1261_TRANCEIVER
	SetPaConfig(0x06, 0x00, 0x01, 0x01); // PA Optimal Settings +15 dBm
	// SX1262_TRANCEIVER
	SetPaConfig(0x04, 0x07, 0x00, 0x01); // PA Optimal Settings +22 dBm
	// SX1268_TRANCEIVER
	SetPaConfig(0x04, 0x07, 0x00, 0x01); // PA Optimal Settings +22 dBm
#endif
  SetPaConfig(0x04, 0x07, 0x00, 0x01);                // PA Optimal Settings +22 dBm
  SetOvercurrentProtection(60.0);                     // current max 60mA for the whole device
  SetPowerConfig(txPowerInDbm, SX126X_PA_RAMP_200U);  // 0 fuer Empfaenger
  SetRfFrequency(frequencyInHz);
  return ERR_NONE;
}

void FixInvertedIQ(uint8_t iqConfig) {
  // fixes IQ configuration for inverted IQ
  // see SX1262/SX1268 datasheet, chapter 15 Known Limitations, section 15.4 for details
  // When exchanging LoRa packets with inverted IQ polarity, some packet losses may be observed for longer packets.
  // Workaround: Bit 2 at address 0x0736 must be set to:
  // “0” when using inverted IQ polarity (see the SetPacketParam(...) command)
  // “1” when using standard IQ polarity

  // read current IQ configuration
  uint8_t iqConfigCurrent = 0;
  ReadRegister(SX126X_REG_IQ_POLARITY_SETUP, &iqConfigCurrent, 1);  // 0x0736

  // set correct IQ configuration
  // if(iqConfig == SX126X_LORA_IQ_STANDARD) {
  if (iqConfig == SX126X_LORA_IQ_INVERTED) {
    iqConfigCurrent &= 0xFB;  // using inverted IQ polarity
  } else {
    iqConfigCurrent |= 0x04;  // using standard IQ polarity
  }

  // update with the new value
  WriteRegister(SX126X_REG_IQ_POLARITY_SETUP, &iqConfigCurrent, 1);  // 0x0736
}

void LoRaConfig(uint8_t spreadingFactor, uint8_t bandwidth, uint8_t codingRate, uint16_t preambleLength, uint8_t payloadLen, bool crcOn,
                bool invertIrq) {
  SetStopRxTimerOnPreambleDetect(false);
  SetLoRaSymbNumTimeout(0);
  SetPacketType(SX126X_PACKET_TYPE_LORA);  // SX126x.ModulationParams.PacketType : MODEM_LORA
  uint8_t ldro = 0;                        // LowDataRateOptimize OFF
  SetModulationParams(spreadingFactor, bandwidth, codingRate, ldro);

  PacketParams[0] = (preambleLength >> 8) & 0xFF;
  PacketParams[1] = preambleLength;
  if (payloadLen) {
    PacketParams[2] = 0x01;  // Fixed length packet (implicit header)
    PacketParams[3] = payloadLen;
  } else {
    PacketParams[2] = 0x00;  // Variable length packet (explicit header)
    PacketParams[3] = 0xFF;
  }

  if (crcOn)
    PacketParams[4] = SX126X_LORA_IQ_INVERTED;
  else
    PacketParams[4] = SX126X_LORA_IQ_STANDARD;

  if (invertIrq)
    PacketParams[5] = 0x01;  // Inverted LoRa I and Q signals setup
  else
    PacketParams[5] = 0x00;  // Standard LoRa I and Q signals setup

  // fixes IQ configuration for inverted IQ
  FixInvertedIQ(PacketParams[5]);

  WriteCommand(SX126X_CMD_SET_PACKET_PARAMS, PacketParams, 6);  // 0x8C

  // Do not use DIO interruptst
  SetDioIrqParams(SX126X_IRQ_ALL,    // all interrupts enabled
                  SX126X_IRQ_NONE,   // interrupts on DIO1
                  SX126X_IRQ_NONE,   // interrupts on DIO2
                  SX126X_IRQ_NONE);  // interrupts on DIO3

  // Receive state no receive timeoout
  SetRx(0xFFFFFF);
}

void LoRaDebugPrint(bool enable) { debugPrint = enable; }

uint8_t LoRaReceive(uint8_t *pData, uint16_t len) {
  uint8_t rxLen = 0;
  uint16_t irqRegs = GetIrqStatus();
  // uint8_t status = GetStatus();

  if (irqRegs & SX126X_IRQ_RX_DONE) {
    // ClearIrqStatus(SX126X_IRQ_RX_DONE);
    ClearIrqStatus(SX126X_IRQ_ALL);
    rxLen = ReadBuffer(pData, len);
  }

  return rxLen;
}

bool LoRaSend(uint8_t *pData, uint8_t len, uint8_t mode) {
  uint16_t irqStatus;
  bool rv = false;

  if (txActive == false) {
    txActive = true;
    PacketParams[2] = 0x00;  // Variable length packet (explicit header)
    PacketParams[3] = len;
    WriteCommand(SX126X_CMD_SET_PACKET_PARAMS, PacketParams, 6);  // 0x8C

    // ClearIrqStatus(SX126X_IRQ_TX_DONE | SX126X_IRQ_TIMEOUT);
    ClearIrqStatus(SX126X_IRQ_ALL);

    WriteBuffer(pData, len);
    SetTx(500);

    if (mode & SX126x_TXMODE_SYNC) {
      irqStatus = GetIrqStatus();
      while ((!(irqStatus & SX126X_IRQ_TX_DONE)) && (!(irqStatus & SX126X_IRQ_TIMEOUT))) {
        delay(1);
        irqStatus = GetIrqStatus();
      }
      if (debugPrint) {
        ESP_LOGI(TAG, "irqStatus=0x%x", irqStatus);
        if (irqStatus & SX126X_IRQ_TX_DONE) {
          ESP_LOGI(TAG, "SX126X_IRQ_TX_DONE");
        }
        if (irqStatus & SX126X_IRQ_TIMEOUT) {
          ESP_LOGI(TAG, "SX126X_IRQ_TIMEOUT");
        }
      }
      txActive = false; //發射結束

      SetRx(0xFFFFFF); //切回連續接收模式

      if (irqStatus & SX126X_IRQ_TX_DONE) {
        rv = true; //發射成功
      }
    } else {
      rv = true;
    }
  }
  if (debugPrint) {
    ESP_LOGI(TAG, "Send rv=0x%x", rv);
  }
  return rv;
}
//檢查是否從發設模式轉為接收模式
bool ReceiveMode(void) {
  uint16_t irq;
  bool rv = false;

  if (txActive == false) {
    rv = true;
  } else {
    irq = GetIrqStatus();
    if (irq & (SX126X_IRQ_TX_DONE | SX126X_IRQ_TIMEOUT)) {
      SetRx(0xFFFFFF); //切回接收
      txActive = false;
      rv = true;
    }
  }

  return rv;
}
//獲取接收的封包狀態 SNR與RSSI
void GetPacketStatus(int8_t *rssiPacket, int8_t *snrPacket) {
  uint8_t buf[4];
  ReadCommand(SX126X_CMD_GET_PACKET_STATUS, buf, 4);  // 0x14
  *rssiPacket = (buf[3] >> 1) * -1; // 計算RSSI
  (buf[2] < 128) ? (*snrPacket = buf[2] >> 2) : (*snrPacket = ((buf[2] - 256) >> 2));
}
//設定發射功率
void SetTxPower(int8_t txPowerInDbm) { SetPowerConfig(txPowerInDbm, SX126X_PA_RAMP_200U); }

void Reset(void) {
  delay(10);
  gpio_set_level(SX126x_RESET, 0);
  delay(20);
  gpio_set_level(SX126x_RESET, 1);
  delay(10);
  // ensure BUSY is low (state meachine ready)
  WaitForIdle(BUSY_WAIT);
}

void Wakeup(void) { GetStatus(); }

void SetStandby(uint8_t mode) {
  uint8_t data = mode;
  WriteCommand(SX126X_CMD_SET_STANDBY, &data, 1);  // 0x80
}

uint8_t GetStatus(void) {
  uint8_t rv;
  ReadCommand(SX126X_CMD_GET_STATUS, &rv, 1);  // 0xC0
  return rv;
}

void SetDio3AsTcxoCtrl(float voltage, uint32_t delay) {
  uint8_t buf[4];

  // buf[0] = tcxoVoltage & 0x07;
  if (fabs(voltage - 1.6) <= 0.001) {
    buf[0] = SX126X_DIO3_OUTPUT_1_6;
  } else if (fabs(voltage - 1.7) <= 0.001) {
    buf[0] = SX126X_DIO3_OUTPUT_1_7;
  } else if (fabs(voltage - 1.8) <= 0.001) {
    buf[0] = SX126X_DIO3_OUTPUT_1_8;
  } else if (fabs(voltage - 2.2) <= 0.001) {
    buf[0] = SX126X_DIO3_OUTPUT_2_2;
  } else if (fabs(voltage - 2.4) <= 0.001) {
    buf[0] = SX126X_DIO3_OUTPUT_2_4;
  } else if (fabs(voltage - 2.7) <= 0.001) {
    buf[0] = SX126X_DIO3_OUTPUT_2_7;
  } else if (fabs(voltage - 3.0) <= 0.001) {
    buf[0] = SX126X_DIO3_OUTPUT_3_0;
  } else {
    buf[0] = SX126X_DIO3_OUTPUT_3_3;
  }

  uint32_t delayValue = (float)delay / 15.625;
  buf[1] = (uint8_t)((delayValue >> 16) & 0xFF);
  buf[2] = (uint8_t)((delayValue >> 8) & 0xFF);
  buf[3] = (uint8_t)(delayValue & 0xFF);

  WriteCommand(SX126X_CMD_SET_DIO3_AS_TCXO_CTRL, buf, 4);  // 0x97
}

void Calibrate(uint8_t calibParam) {
  uint8_t data = calibParam;
  WriteCommand(SX126X_CMD_CALIBRATE, &data, 1);  // 0x89
}

void SetDio2AsRfSwitchCtrl(uint8_t enable) {
  uint8_t data = enable;
  WriteCommand(SX126X_CMD_SET_DIO2_AS_RF_SWITCH_CTRL, &data, 1);  // 0x9D
}

void SetRfFrequency(uint32_t frequency) {
  uint8_t buf[4];
  uint32_t freq = 0;

  CalibrateImage(frequency);

  freq = (uint32_t)((double)frequency / (double)FREQ_STEP);
  buf[0] = (uint8_t)((freq >> 24) & 0xFF);
  buf[1] = (uint8_t)((freq >> 16) & 0xFF);
  buf[2] = (uint8_t)((freq >> 8) & 0xFF);
  buf[3] = (uint8_t)(freq & 0xFF);
  WriteCommand(SX126X_CMD_SET_RF_FREQUENCY, buf, 4);  // 0x86
}

void CalibrateImage(uint32_t frequency) {
  uint8_t calFreq[2];

  if (frequency > 900000000) {
    calFreq[0] = 0xE1;
    calFreq[1] = 0xE9;
  } else if (frequency > 850000000) {
    calFreq[0] = 0xD7;
    calFreq[1] = 0xD8;
  } else if (frequency > 770000000) {
    calFreq[0] = 0xC1;
    calFreq[1] = 0xC5;
  } else if (frequency > 460000000) {
    calFreq[0] = 0x75;
    calFreq[1] = 0x81;
  } else if (frequency > 425000000) {
    calFreq[0] = 0x6B;
    calFreq[1] = 0x6F;
  }
  WriteCommand(SX126X_CMD_CALIBRATE_IMAGE, calFreq, 2);  // 0x98
}

void SetRegulatorMode(uint8_t mode) {
  uint8_t data = mode;
  WriteCommand(SX126X_CMD_SET_REGULATOR_MODE, &data, 1);  // 0x96
}

void SetBufferBaseAddress(uint8_t txBaseAddress, uint8_t rxBaseAddress) {
  uint8_t buf[2];

  buf[0] = txBaseAddress;
  buf[1] = rxBaseAddress;
  WriteCommand(SX126X_CMD_SET_BUFFER_BASE_ADDRESS, buf, 2);  // 0x8F
}

void SetPowerConfig(int8_t power, uint8_t rampTime) {
  uint8_t buf[2];

  if (power > 22) {
    power = 22;
  } else if (power < -3) {
    power = -3;
  }

  buf[0] = power;
  buf[1] = (uint8_t)rampTime;
  WriteCommand(SX126X_CMD_SET_TX_PARAMS, buf, 2);  // 0x8E
}

void SetPaConfig(uint8_t paDutyCycle, uint8_t hpMax, uint8_t deviceSel, uint8_t paLut) {
  uint8_t buf[4];

  buf[0] = paDutyCycle;
  buf[1] = hpMax;
  buf[2] = deviceSel;
  buf[3] = paLut;
  WriteCommand(SX126X_CMD_SET_PA_CONFIG, buf, 4);  // 0x95
}

void SetOvercurrentProtection(float currentLimit) {
  if ((currentLimit >= 0.0) && (currentLimit <= 140.0)) {
    uint8_t buf[1];
    buf[0] = (uint8_t)(currentLimit / 2.5);
    WriteRegister(SX126X_REG_OCP_CONFIGURATION, buf, 1);  // 0x08E7
  }
}

void SetSyncWord(int16_t sync) {
  uint8_t buf[2];

  buf[0] = (uint8_t)((sync >> 8) & 0x00FF);
  buf[1] = (uint8_t)(sync & 0x00FF);
  WriteRegister(SX126X_REG_LORA_SYNC_WORD_MSB, buf, 2);  // 0x0740
}

void SetDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask) {
  uint8_t buf[8];

  buf[0] = (uint8_t)((irqMask >> 8) & 0x00FF);
  buf[1] = (uint8_t)(irqMask & 0x00FF);
  buf[2] = (uint8_t)((dio1Mask >> 8) & 0x00FF);
  buf[3] = (uint8_t)(dio1Mask & 0x00FF);
  buf[4] = (uint8_t)((dio2Mask >> 8) & 0x00FF);
  buf[5] = (uint8_t)(dio2Mask & 0x00FF);
  buf[6] = (uint8_t)((dio3Mask >> 8) & 0x00FF);
  buf[7] = (uint8_t)(dio3Mask & 0x00FF);
  WriteCommand(SX126X_CMD_SET_DIO_IRQ_PARAMS, buf, 8);  // 0x08
}

void SetStopRxTimerOnPreambleDetect(bool enable) {
  ESP_LOGI(TAG, "SetStopRxTimerOnPreambleDetect enable=%d", enable);
  // uint8_t data = (uint8_t)enable;
  uint8_t data = 0;
  if (enable) data = 1;
  WriteCommand(SX126X_CMD_STOP_TIMER_ON_PREAMBLE, &data, 1);  // 0x9F
}

void SetLoRaSymbNumTimeout(uint8_t SymbNum) {
  uint8_t data = SymbNum;
  WriteCommand(SX126X_CMD_SET_LORA_SYMB_NUM_TIMEOUT, &data, 1);  // 0xA0
}

void SetPacketType(uint8_t packetType) {
  uint8_t data = packetType;
  WriteCommand(SX126X_CMD_SET_PACKET_TYPE, &data, 1);  // 0x01
}

void SetModulationParams(uint8_t spreadingFactor, uint8_t bandwidth, uint8_t codingRate, uint8_t lowDataRateOptimize) {
  uint8_t data[4];
  // currently only LoRa supported
  data[0] = spreadingFactor;
  data[1] = bandwidth;
  data[2] = codingRate;
  data[3] = lowDataRateOptimize;
  WriteCommand(SX126X_CMD_SET_MODULATION_PARAMS, data, 4);  // 0x8B
}

void SetCadParams(uint8_t cadSymbolNum, uint8_t cadDetPeak, uint8_t cadDetMin, uint8_t cadExitMode, uint32_t cadTimeout) {
  uint8_t data[7];
  data[0] = cadSymbolNum;
  data[1] = cadDetPeak;
  data[2] = cadDetMin;
  data[3] = cadExitMode;
  data[4] = (uint8_t)((cadTimeout >> 16) & 0xFF);
  data[5] = (uint8_t)((cadTimeout >> 8) & 0xFF);
  data[6] = (uint8_t)(cadTimeout & 0xFF);
  WriteCommand(SX126X_CMD_SET_CAD_PARAMS, data, 7);  // 0x88
}

void SetCad() {
  uint8_t data = 0;
  WriteCommand(SX126X_CMD_SET_CAD, &data, 0);  // 0xC5
}

uint16_t GetIrqStatus(void) {
  uint8_t data[3];
  ReadCommand(SX126X_CMD_GET_IRQ_STATUS, data, 3);  // 0x12
  return (data[1] << 8) | data[2];
}

void ClearIrqStatus(uint16_t irq) {
  uint8_t buf[2];

  buf[0] = (uint8_t)(((uint16_t)irq >> 8) & 0x00FF);
  buf[1] = (uint8_t)((uint16_t)irq & 0x00FF);
  WriteCommand(SX126X_CMD_CLEAR_IRQ_STATUS, buf, 2);  // 0x02
}

void SetRx(uint32_t timeout) {
  if (debugPrint) {
    ESP_LOGI(TAG, "----- SetRx timeout=%" PRIu32, timeout);
  }
  SetStandby(SX126X_STANDBY_RC);
  SetRxEnable();
  uint8_t buf[3];
  buf[0] = (uint8_t)((timeout >> 16) & 0xFF);
  buf[1] = (uint8_t)((timeout >> 8) & 0xFF);
  buf[2] = (uint8_t)(timeout & 0xFF);
  WriteCommand(SX126X_CMD_SET_RX, buf, 3);  // 0x82

  for (int retry = 0; retry < 10; retry++) {
    if ((GetStatus() & 0x70) == 0x50) break;
    delay(1);
  }
  if ((GetStatus() & 0x70) != 0x50) {
    ESP_LOGE(TAG, "SetRx Illegal Status");
    LoRaError(ERR_INVALID_SETRX_STATE);
  }
}

void SetRxEnable(void) {
  if (debugPrint) {
    ESP_LOGI(TAG, "SetRxEnable:SX126x_TXEN=%d SX126x_RXEN=%d", SX126x_TXEN, SX126x_RXEN);
  }
  if ((SX126x_TXEN != -1) && (SX126x_RXEN != -1)) {
    gpio_set_level(SX126x_RXEN, HIGH);
    gpio_set_level(SX126x_TXEN, LOW);
  }
}

void SetTx(uint32_t timeoutInMs) {
  if (debugPrint) {
    ESP_LOGI(TAG, "----- SetTx timeoutInMs=%" PRIu32, timeoutInMs);
  }
  SetStandby(SX126X_STANDBY_RC);
  SetTxEnable();
  uint8_t buf[3];
  uint32_t tout = timeoutInMs;
  if (timeoutInMs != 0) {
    uint32_t timeoutInUs = timeoutInMs * 1000;
    tout = (uint32_t)(timeoutInUs / 0.015625);
  }
  if (debugPrint) {
    ESP_LOGI(TAG, "SetTx timeoutInMs=%" PRIu32 " tout=%" PRIu32, timeoutInMs, tout);
  }
  buf[0] = (uint8_t)((tout >> 16) & 0xFF);
  buf[1] = (uint8_t)((tout >> 8) & 0xFF);
  buf[2] = (uint8_t)(tout & 0xFF);
  WriteCommand(SX126X_CMD_SET_TX, buf, 3);  // 0x83

  for (int retry = 0; retry < 10; retry++) {
    if ((GetStatus() & 0x70) == 0x60) break;
    delay(1);
  }
  if ((GetStatus() & 0x70) != 0x60) {
    ESP_LOGE(TAG, "SetTx Illegal Status");
    LoRaError(ERR_INVALID_SETTX_STATE);
  }
}

void SetTxEnable(void) {
  if (debugPrint) {
    ESP_LOGI(TAG, "SetTxEnable:SX126x_TXEN=%d SX126x_RXEN=%d", SX126x_TXEN, SX126x_RXEN);
  }
  if ((SX126x_TXEN != -1) && (SX126x_RXEN != -1)) {
    gpio_set_level(SX126x_RXEN, LOW);
    gpio_set_level(SX126x_TXEN, HIGH);
  }
}

uint8_t GetRssiInst() {
  uint8_t buf[2];
  ReadCommand(SX126X_CMD_GET_RSSI_INST, buf, 2);  // 0x15
  return buf[1];
}

void GetRxBufferStatus(uint8_t *payloadLength, uint8_t *rxStartBufferPointer) {
  uint8_t buf[3];
  ReadCommand(SX126X_CMD_GET_RX_BUFFER_STATUS, buf, 3);  // 0x13
  *payloadLength = buf[1];
  *rxStartBufferPointer = buf[2];
}

void WaitForIdle(unsigned long timeout) {
  TickType_t start = xTaskGetTickCount();
  while (gpio_get_level(SX126x_BUSY)) {
    if (xTaskGetTickCount() - start > (timeout / portTICK_PERIOD_MS)) {
      ESP_LOGE(TAG, "WaitForIdle Timeout timeout=%lu", timeout);
      LoRaError(ERR_IDLE_TIMEOUT);
      return;
    }
  }
}

uint8_t ReadBuffer(uint8_t *rxData, uint8_t maxLen) {
  uint8_t offset = 0;
  uint8_t payloadLength = 0;
  GetRxBufferStatus(&payloadLength, &offset);
  if (payloadLength > maxLen) {
    ESP_LOGW(TAG, "ReadBuffer maxLen too small");
    return 0;
  }

  // ensure BUSY is low (state meachine ready)
  WaitForIdle(BUSY_WAIT);

  // start transfer
  gpio_set_level(SX126x_SPI_SELECT, LOW);

  spi_transfer(SX126X_CMD_READ_BUFFER);  // 0x1E
  spi_transfer(offset);
  spi_transfer(SX126X_CMD_NOP);
  for (uint16_t i = 0; i < payloadLength; i++) {
    rxData[i] = spi_transfer(SX126X_CMD_NOP);
  }

  // stop transfer
  gpio_set_level(SX126x_SPI_SELECT, HIGH);

  // wait for BUSY to go low
  WaitForIdle(BUSY_WAIT);

  return payloadLength;
}

void WriteBuffer(uint8_t *txData, uint8_t txDataLen) {
  // ensure BUSY is low (state meachine ready)
  WaitForIdle(BUSY_WAIT);

  // start transfer
  gpio_set_level(SX126x_SPI_SELECT, LOW);

  spi_transfer(SX126X_CMD_WRITE_BUFFER);  // 0x0E
  spi_transfer(0);                        // offset in tx fifo
  for (uint16_t i = 0; i < txDataLen; i++) {
    spi_transfer(txData[i]);
  }

  // stop transfer
  gpio_set_level(SX126x_SPI_SELECT, HIGH);

  // wait for BUSY to go low
  WaitForIdle(BUSY_WAIT);
}

void WriteRegister(uint16_t reg, uint8_t *data, uint8_t numBytes) {
  // ensure BUSY is low (state meachine ready)
  WaitForIdle(BUSY_WAIT);

  if (debugPrint) {
    ESP_LOGI(TAG, "WriteRegister: REG=0x%02x", reg);
  }
  // start transfer
  gpio_set_level(SX126x_SPI_SELECT, LOW);

  // send command byte
  spi_transfer(SX126X_CMD_WRITE_REGISTER);  // 0x0D
  spi_transfer((reg & 0xFF00) >> 8);
  spi_transfer(reg & 0xff);

  for (uint8_t n = 0; n < numBytes; n++) {
    uint8_t in = spi_transfer(data[n]);
    (void)in;
    if (debugPrint) {
      ESP_LOGI(TAG, "%02x --> %02x", data[n], in);
      // ESP_LOGI(TAG, "DataOut:%02x ", data[n]);
    }
  }

  // stop transfer
  gpio_set_level(SX126x_SPI_SELECT, HIGH);

  // wait for BUSY to go low
  WaitForIdle(BUSY_WAIT);
#if 0
	if(waitForBusy) {
		WaitForIdle(BUSY_WAIT);
	}
#endif
}

void ReadRegister(uint16_t reg, uint8_t *data, uint8_t numBytes) {
  // ensure BUSY is low (state meachine ready)
  WaitForIdle(BUSY_WAIT);

  if (debugPrint) {
    ESP_LOGI(TAG, "ReadRegister: REG=0x%02x", reg);
  }

  // start transfer
  gpio_set_level(SX126x_SPI_SELECT, LOW);

  // send command byte
  spi_transfer(SX126X_CMD_READ_REGISTER);  // 0x1D
  spi_transfer((reg & 0xFF00) >> 8);
  spi_transfer(reg & 0xff);
  spi_transfer(SX126X_CMD_NOP);

  for (uint8_t n = 0; n < numBytes; n++) {
    data[n] = spi_transfer(SX126X_CMD_NOP);
    if (debugPrint) {
      ESP_LOGI(TAG, "DataIn:%02x ", data[n]);
    }
  }

  // stop transfer
  gpio_set_level(SX126x_SPI_SELECT, HIGH);

  // wait for BUSY to go low
  WaitForIdle(BUSY_WAIT);
#if 0
	if(waitForBusy) {
		WaitForIdle(BUSY_WAIT);
	}
#endif
}

// WriteCommand with retry
void WriteCommand(uint8_t cmd, uint8_t *data, uint8_t numBytes) {
  uint8_t status;
  for (int retry = 1; retry < 10; retry++) {
    status = WriteCommand2(cmd, data, numBytes);
    ESP_LOGD(TAG, "status=%02x", status);
    if (status == 0) break;
    ESP_LOGW(TAG, "WriteCommand2 status=%02x retry=%d", status, retry);
  }
  if (status != 0) {
    ESP_LOGE(TAG, "SPI Transaction error:0x%02x", status);
    LoRaError(ERR_SPI_TRANSACTION);
  }
}

uint8_t WriteCommand2(uint8_t cmd, uint8_t *data, uint8_t numBytes) {
  // ensure BUSY is low (state meachine ready)
  WaitForIdle(BUSY_WAIT);

  // start transfer
  gpio_set_level(SX126x_SPI_SELECT, LOW);

  // send command byte
  if (debugPrint) {
    ESP_LOGI(TAG, "WriteCommand: CMD=0x%02x", cmd);
  }
  spi_transfer(cmd);

  // variable to save error during SPI transfer
  uint8_t status = 0;

  // send/receive all bytes
  for (uint8_t n = 0; n < numBytes; n++) {
    uint8_t in = spi_transfer(data[n]);
    if (debugPrint) {
      ESP_LOGI(TAG, "%02x --> %02x", data[n], in);
    }

    // check status
    if (((in & 0b00001110) == SX126X_STATUS_CMD_TIMEOUT) || ((in & 0b00001110) == SX126X_STATUS_CMD_INVALID) ||
        ((in & 0b00001110) == SX126X_STATUS_CMD_FAILED)) {
      status = in & 0b00001110;
      break;
    } else if (in == 0x00 || in == 0xFF) {
      status = SX126X_STATUS_SPI_FAILED;
      break;
    }
  }

  // stop transfer
  gpio_set_level(SX126x_SPI_SELECT, HIGH);

  // wait for BUSY to go low
  WaitForIdle(BUSY_WAIT);
#if 0
	if(waitForBusy) {
		WaitForIdle(BUSY_WAIT);
	}
#endif

#if 0
	if (status != 0) {
		ESP_LOGE(TAG, "SPI Transaction error:0x%02x", status);
		LoRaError(ERR_SPI_TRANSACTION);
	}
#endif
  return status;
}

void ReadCommand(uint8_t cmd, uint8_t *data, uint8_t numBytes) {
  // ensure BUSY is low (state meachine ready)
  WaitForIdle(BUSY_WAIT);

  // start transfer
  gpio_set_level(SX126x_SPI_SELECT, LOW);

  // send command byte
  if (debugPrint) {
    ESP_LOGI(TAG, "ReadCommand: CMD=0x%02x", cmd);
  }
  spi_transfer(cmd);

  // send/receive all bytes
  for (uint8_t n = 0; n < numBytes; n++) {
    data[n] = spi_transfer(SX126X_CMD_NOP);
    if (debugPrint) {
      ESP_LOGI(TAG, "DataIn:%02x", data[n]);
    }
  }

  // stop transfer
  gpio_set_level(SX126x_SPI_SELECT, HIGH);

  // wait for BUSY to go low
  WaitForIdle(BUSY_WAIT);
#if 0
	if(waitForBusy) {
		WaitForIdle(BUSY_WAIT);
	}
#endif
}

void lora_init() {
  const static uint32_t frequencyInHz = 433E6;
  const static int8_t txPowerInDbm = 22;
  const static float tcxoVoltage = 3.3;
  const static bool useRegulatorLDO = true;

  LoRaInit();
  int ret = LoRaBegin(frequencyInHz, txPowerInDbm, tcxoVoltage, useRegulatorLDO);
  ESP_LOGI(TAG, "LoRaBegin=%d", ret);
  if (ret != 0) {
    ESP_LOGE(pcTaskGetName(NULL), "Does not recognize the module");
  }

  uint8_t spreadingFactor = 7;
  uint8_t bandwidth = SX126X_LORA_BW_125_0;
  uint8_t codingRate = SX126X_LORA_CR_4_5;
  uint16_t preambleLength = 8;
  uint8_t payloadLen = 0;
  bool crcOn = true;
  bool invertIrq = false;
#if CONFIF_ADVANCED
  spreadingFactor = CONFIG_SF_RATE;
  bandwidth = CONFIG_BANDWIDTH;
  codingRate = CONFIG_CODING_RATE;
#endif
  LoRaConfig(spreadingFactor, bandwidth, codingRate, preambleLength, payloadLen, crcOn, invertIrq);
    
  //SetSyncWord(0x1424);  //可更改 syncword  0x0000 ~ 0xFFFF
}
```

## components/sensors
#### IMU
```cpp=
#include "imu.h"

static float rotation[3][3] = {
    {1, 0, 0},
    {0, 1, 0},
    {0, 0, 1},
}; //校正板子方向所用的陣列

static calibration_t cal = {._ascale = AFS_16G,
                            ._gscale = GFS_2000DPS,
                            ._aodr = ODR_6660Hz,
                            ._godr = ODR_6660Hz,
                            ._ares = 0,
                            ._gres = 0,
                            ._accelBias = {0, 0, 0},
                            ._gyroBias = {0, 0, 0}};
//校正及設定初始數值

static imu_t imu_instance;

static inline void transform_accel_gyro(vector_t *);
static inline void transform_mag(vector_t *);
static inline void rotate(vector_t *);

void imu_init() { //初始化IMU
  /* Init the MPU and AHRS */
  printf("IMU init...\n");
  lsm6dsm_init(&cal);    //初始化lsm6dsm
  MadgwickAHRSinit(100, 0.8);    //初始化MadgwickAHRS
  imu_instance.freq = 100;    //設定基礎數值(頻率、速度、角度)
  imu_instance.velocity.x = 0;
  imu_instance.velocity.y = 0;
  imu_instance.velocity.z = 0;
  imu_instance.heading = 0;
  imu_instance.pitch = 0;
  imu_instance.roll = 0;

  /* Start timer task for precise frequency */
  xTimerStart(xTimerCreate("imu_update", pdMS_TO_TICKS(10), pdTRUE, (void *)0, imu_update), 0); 
//設定Timer來控制imu_update，這裡是每10ms更新一次
}

void imu_update() { //隨時間更新IMU收到的數值
  readData(&imu_instance, &cal);//讀取來自IMU的資料

  transform_accel_gyro(&imu_instance.a);//轉換數值
  transform_accel_gyro(&imu_instance.g);

  rotate(&imu_instance.a);//轉換數值
  rotate(&imu_instance.g);

  // Apply the AHRS algorithm
  MadgwickAHRSupdate(DEG2RAD(imu_instance.g.x), DEG2RAD(imu_instance.g.y), DEG2RAD(imu_instance.g.z), imu_instance.a.x, imu_instance.a.y,
                     imu_instance.a.z, 0, 0, 0);//更新數值

  MadgwickGetEulerAnglesDegrees(&imu_instance.heading, &imu_instance.pitch, &imu_instance.roll);
  imu_instance.velocity.x += imu_instance.a.x / imu_instance.freq;
  imu_instance.velocity.y += imu_instance.a.y / imu_instance.freq;
  imu_instance.velocity.z += imu_instance.a.z / imu_instance.freq;
}//轉換數值為歐拉角

imu_t *imu_fetch() { return &imu_instance; }//呼叫所有的imu數值

/**
 * Transformation:
 *  - Rotate around Z axis 180 degrees
 *  - Rotate around X axis -90 degrees
 * @param  {object} s {x,y,z} sensor
 * @return {object}   {x,y,z} transformed
 */
static inline void transform_accel_gyro(vector_t *v) {
  float x = v->x;
  float y = v->y;
  float z = v->z;

  v->x = -x;
  v->y = -z;
  v->z = -y;
}

/**
 * Transformation: to get magnetometer aligned
 * @param  {object} s {x,y,z} sensor
 * @return {object}   {x,y,z} transformed
 */
static inline void transform_mag(vector_t *v) {
  float x = v->x;
  float y = v->y;
  float z = v->z;

  v->x = -y;
  v->y = z;
  v->z = -x;
}

static inline void rotate(vector_t *v) {
  const float x = v->x * rotation[0][0] + v->y * rotation[0][1] + v->z * rotation[0][2];
  const float y = v->x * rotation[1][0] + v->y * rotation[1][1] + v->z * rotation[1][2];
  const float z = v->x * rotation[2][0] + v->y * rotation[2][1] + v->z * rotation[2][2];
  v->x = x;
  v->y = y;
  v->z = z;
}
```
#### BMP280
```cpp
#include "bmp280.h"
#include "bsp.h"
#include <stdint.h>

#define TAG "BMP280"

static pressure_sensor_t pressure_sensor_instance;

void bmp280_init() {//初始化
  esp_err_t err;
  // use the "handheld device dynamic" optimal setting (see datasheet)
  uint8_t buf[2];

  const uint8_t reg_config_val = ((0x00 << 5) | (0x00 << 2)) & 0xFC;

  // send register number followed by its corresponding value
  buf[0] = REG_CONFIG;
  buf[1] = reg_config_val;
  err = i2c_master_write_to_device(I2C_MASTER_NUM, BMP280_SENSOR_ADDRESS, &buf, 2, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "bmp280 error %d", err);
    while (1);
  }

  // osrs_t x2 osrs_p x1, normal mode operation
  const uint8_t reg_ctrl_meas_val = (0x2 << 5) | (0x1 << 2) | (0x03);
  buf[0] = REG_CTRL_MEAS;
  buf[1] = reg_ctrl_meas_val;
  err = i2c_master_write_to_device(I2C_MASTER_NUM, BMP280_SENSOR_ADDRESS, &buf, 2, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "bmp280 error %d", err);
    while (1);
  }

  pressure_sensor_instance.init_altitude = 0;
  bmp280_get_calib_params(&pressure_sensor_instance.params);

  bmp280_update();
  pressure_sensor_instance.init_altitude = pressure_sensor_instance.altitude;

  /* Start timer task for precise frequency */
  xTimerStart(xTimerCreate("bmp280_update", pdMS_TO_TICKS(10), pdTRUE, (void*)0, bmp280_update), 0);
}

void bmp280_read_raw(int32_t* temp, int32_t* pressure) {
  // BMP280 data registers are auto-incrementing and we have 3 temperature and
  // pressure registers each, so we start at 0xF7 and read 6 bytes to 0xFC
  // note: normal mode does not require further ctrl_meas and config register writes

  uint8_t buf[6];
  uint8_t reg = REG_PRESSURE_MSB;
  esp_err_t err =
      i2c_master_write_read_device(I2C_MASTER_NUM, BMP280_SENSOR_ADDRESS, &reg, 1, buf, 6, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);

  // store the 20 bit read in a 32 bit signed integer for conversion
  *pressure = (buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4);
  *temp = (buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4);
}

void bmp280_reset() {
  // reset the device with the power-on-reset procedure
  uint8_t buf[2] = {REG_RESET, 0xB6};
  i2c_master_write_to_device(I2C_MASTER_NUM, BMP280_SENSOR_ADDRESS, &buf, 2, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
}

// intermediate function that calculates the fine resolution temperature
// used for both pressure and temperature conversions
int32_t bmp280_convert(int32_t temp, struct bmp280_calib_param* params) {
  // use the 32-bit fixed point compensation implementation given in the
  // datasheet

  int32_t var1, var2;
  var1 = ((((temp >> 3) - ((int32_t)params->dig_t1 << 1))) * ((int32_t)params->dig_t2)) >> 11;
  var2 =
      (((((temp >> 4) - ((int32_t)params->dig_t1)) * ((temp >> 4) - ((int32_t)params->dig_t1))) >> 12) * ((int32_t)params->dig_t3)) >> 14;
  return var1 + var2;
}

int32_t bmp280_convert_temp(int32_t temp, struct bmp280_calib_param* params) {
  // uses the BMP280 calibration parameters to compensate the temperature value read from its registers
  int32_t t_fine = bmp280_convert(temp, params);
  return (t_fine * 5 + 128) >> 8;
}

int32_t bmp280_convert_pressure(int32_t pressure, int32_t temp, struct bmp280_calib_param* params) {
  // uses the BMP280 calibration parameters to compensate the pressure value read from its registers

  int32_t t_fine = bmp280_convert(temp, params);

  int32_t var1, var2;
  uint32_t converted = 0.0;
  var1 = (((int32_t)t_fine) >> 1) - (int32_t)64000;
  var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * ((int32_t)params->dig_p6);
  var2 += ((var1 * ((int32_t)params->dig_p5)) << 1);
  var2 = (var2 >> 2) + (((int32_t)params->dig_p4) << 16);
  var1 = (((params->dig_p3 * (((var1 >> 2) * (var1 >> 2)) >> 13)) >> 3) + ((((int32_t)params->dig_p2) * var1) >> 1)) >> 18;
  var1 = ((((32768 + var1)) * ((int32_t)params->dig_p1)) >> 15);
  if (var1 == 0) {
    return 0;  // avoid exception caused by division by zero
  }
  converted = (((uint32_t)(((int32_t)1048576) - pressure) - (var2 >> 12))) * 3125;
  if (converted < 0x80000000) {
    converted = (converted << 1) / ((uint32_t)var1);
  } else {
    converted = (converted / (uint32_t)var1) * 2;
  }
  var1 = (((int32_t)params->dig_p9) * ((int32_t)(((converted >> 3) * (converted >> 3)) >> 13))) >> 12;
  var2 = (((int32_t)(converted >> 2)) * ((int32_t)params->dig_p8)) >> 13;
  converted = (uint32_t)((int32_t)converted + ((var1 + var2 + params->dig_p7) >> 4));
  return converted;
}

void bmp280_get_calib_params(struct bmp280_calib_param* params) {
  // raw temp and pressure values need to be calibrated according to
  // parameters generated during the manufacturing of the sensor
  // there are 3 temperature params, and 9 pressure params, each with a LSB
  // and MSB register, so we read from 24 registers

  uint8_t buf[NUM_CALIB_PARAMS] = {0};
  uint8_t reg = REG_DIG_T1_LSB;
  i2c_master_write_read_device(I2C_MASTER_NUM, BMP280_SENSOR_ADDRESS, &reg, 1, buf, NUM_CALIB_PARAMS,
                               I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);

  // store these in a struct for later use
  params->dig_t1 = (uint16_t)(buf[1] << 8) | buf[0];
  params->dig_t2 = (int16_t)(buf[3] << 8) | buf[2];
  params->dig_t3 = (int16_t)(buf[5] << 8) | buf[4];

  params->dig_p1 = (uint16_t)(buf[7] << 8) | buf[6];
  params->dig_p2 = (int16_t)(buf[9] << 8) | buf[8];
  params->dig_p3 = (int16_t)(buf[11] << 8) | buf[10];
  params->dig_p4 = (int16_t)(buf[13] << 8) | buf[12];
  params->dig_p5 = (int16_t)(buf[15] << 8) | buf[14];
  params->dig_p6 = (int16_t)(buf[17] << 8) | buf[16];
  params->dig_p7 = (int16_t)(buf[19] << 8) | buf[18];
  params->dig_p8 = (int16_t)(buf[21] << 8) | buf[20];
  params->dig_p9 = (int16_t)(buf[23] << 8) | buf[22];
}

static inline float pressure2altitude(int32_t pressure) {
  float mbar = (float)pressure / 100.0;
  return 145366.45 * (1 - pow(mbar / 1013.25, 0.190284)) * 0.3084;
}//高度轉氣壓

void bmp280_update() {
  static int32_t raw_temperature, raw_pressure;
  static float last_altitude_iir = 0;
  static float last_altitude_lpf = 0;
  static float last_velocity_lpf = 0;
  static float last_velocity_iir = 0;
  static float iir_altitude = 0;
  bmp280_read_raw(&raw_temperature, &raw_pressure);
  pressure_sensor_instance.temperature = bmp280_convert_temp(raw_temperature, &pressure_sensor_instance.params);
  pressure_sensor_instance.pressure = bmp280_convert_pressure(raw_pressure, raw_temperature, &pressure_sensor_instance.params);
  pressure_sensor_instance.altitude = pressure2altitude(pressure_sensor_instance.pressure);
  pressure_sensor_instance.last_update = bsp_current_time();

  pressure_sensor_instance.relative_altitude = pressure_sensor_instance.altitude - pressure_sensor_instance.init_altitude;
  pressure_sensor_instance.relative_altitude = lpf(pressure_sensor_instance.relative_altitude, last_altitude_lpf, 20, 100);
  last_altitude_lpf = pressure_sensor_instance.relative_altitude;

  iir_altitude = iir_1st(pressure_sensor_instance.relative_altitude, last_altitude_iir, 0.98);
  pressure_sensor_instance.velocity = 100 * (iir_altitude - last_altitude_iir);
  pressure_sensor_instance.velocity = lpf(pressure_sensor_instance.velocity, last_velocity_lpf, 10, 100);
  last_velocity_lpf = pressure_sensor_instance.velocity;
  pressure_sensor_instance.velocity = iir_1st(pressure_sensor_instance.velocity, last_velocity_iir, 0.8);

  last_altitude_iir = iir_altitude;
  last_velocity_iir = pressure_sensor_instance.velocity;
}

pressure_sensor_t* bmp_fetch() { return &pressure_sensor_instance; }//回傳所有bmp280程式裡的參數
```
#### GPS
```cpp
#include "gps.h"

#define TAG "GPS"

static gps_t gps_instance;

void gps_init() {
  /* Start timer task for precise frequency */
  xTaskCreate(gps_parse_task, "gps_parse_task", 8192, NULL, 12, NULL);
} //初始化並創造任務

static inline void gps_parser(uint8_t *raw) {
  // printf("%s", raw);
  /* Only prase GGA message */
  if (strstr((char *)raw, "GGA") == NULL) return; //篩選出$GPGGA 類型的資料(GPS類型的資料)
  static nmea_t parsed;
  parsed.len = 0;
  parsed.field[parsed.len] = raw;
  parsed.len += 1;
  while ((raw = (uint8_t *)strchr((char *)raw, ',')) != NULL) {
    *raw = 0;
    raw += 1;
    parsed.field[parsed.len] = raw;
    parsed.len += 1;
  } //拆分字節如下

  if (strcmp((char *)&parsed.field[0][3], "GGA") == 0) {
    /* GGA Format
     * $--GGA,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,M,<11>,M,<13>,<14>*xx
     *  <1>: UTC Time: hhmmss.sss
     *  <2>: latitude: ddmm.mmmm(m)
     *  <3>: latitude direction: N+/S-
     *  <4>: longitude: ddmm.mmmm(m)
     *  <5>: longitude direction: E+/W-
     *  <6>: status: ref: https://docs.novatel.com/OEM7/Content/Logs/GPGGA.htm#GPSQualityIndicators
     *  <7>: number of satellites
     *  <8>: horizontal precision
     *  <9>: altitude
     *    M: unit of altitude (M for metres)
     * <11>: undulation: geoid undulation in WGS84
     *    M: unit of undulation (M for metres)
     * <13>: age of differential corrections
     * <14>: differential reference station ID
     *  */

    /* UTC time */
    gps_instance.time.hour = CHAR2INT(parsed.field[1][0]) * 10 + CHAR2INT(parsed.field[1][1]);
    gps_instance.time.minute = CHAR2INT(parsed.field[1][2]) * 10 + CHAR2INT(parsed.field[1][3]);
    gps_instance.time.second = CHAR2INT(parsed.field[1][4]) * 10 + CHAR2INT(parsed.field[1][5]);

    /* latitude: Parsed string to minutes format */
    if (parsed.field[2][0] != 0) {
      gps_instance.latitude = (CHAR2INT(parsed.field[2][0]) * 10 + CHAR2INT(parsed.field[2][1])) * 600000 +
                              CHAR2INT(parsed.field[2][2]) * 100000 + CHAR2INT(parsed.field[2][3]) * 10000 +
                              CHAR2INT(parsed.field[2][5]) * 1000 + CHAR2INT(parsed.field[2][6]) * 100 + CHAR2INT(parsed.field[2][7]) * 10 +
                              CHAR2INT(parsed.field[2][8]);
      gps_instance.latitude *= parsed.field[3][0] == 'N' ? 1 : -1;
    }

    /* longitude: Parsed string to minutes format */
    if (parsed.field[4][0] != 0) {
      gps_instance.longitude =
          (CHAR2INT(parsed.field[4][0]) * 100 + CHAR2INT(parsed.field[4][1]) * 10 + CHAR2INT(parsed.field[4][2])) * 600000 +
          CHAR2INT(parsed.field[4][3]) * 100000 + CHAR2INT(parsed.field[4][4]) * 10000 + CHAR2INT(parsed.field[4][6]) * 1000 +
          CHAR2INT(parsed.field[4][7]) * 100 + CHAR2INT(parsed.field[4][8]) * 10 + CHAR2INT(parsed.field[4][9]);
      gps_instance.longitude *= parsed.field[5][0] == 'E' ? 1 : -1;
    }

    gps_instance.altitude = strtof((char *)parsed.field[9], NULL);

    /* Set ready to true */
    gps_instance.ready = CHAR2INT(parsed.field[6][0]);

    /* Number of satellites */
    gps_instance.satellites = CHAR2INT(parsed.field[7][0]) * 10 + CHAR2INT(parsed.field[7][1]);
  }
}

void gps_parse_task() {
  static uart_event_t event;
  static uint8_t buffer[256];
  static QueueHandle_t *uart_queue;
  static int8_t pos;

  uart_queue = fetch_uart_queue();//接收來自uart的事件
  esp_log_level_set(TAG, ESP_LOG_INFO);
  while (1) { //偵錯
    if (xQueueReceive(*uart_queue, &event, pdMS_TO_TICKS(100))) {
      switch (event.type) {
        case UART_DATA:
          break;
        case UART_FIFO_OVF:
          ESP_LOGW(TAG, "HW FIFO Overflow");
          uart_flush(GPS_UART_NUM);
          xQueueReset(*uart_queue);
          break;
        case UART_BUFFER_FULL:
          ESP_LOGW(TAG, "Ring Buffer Full");
          uart_flush(GPS_UART_NUM);
          xQueueReset(*uart_queue);
          break;
        case UART_BREAK:
          ESP_LOGW(TAG, "Rx Break");
          break;
        case UART_PARITY_ERR:
          ESP_LOGE(TAG, "Parity Error");
          break;
        case UART_FRAME_ERR:
          ESP_LOGE(TAG, "Frame Error");
          break;
        case UART_PATTERN_DET: {
          // printf("Enter UART parse case\n");
          pos = uart_pattern_pop_pos(GPS_UART_NUM);
          if (pos != -1) {
            int read_len = uart_read_bytes(GPS_UART_NUM, buffer, pos + 1, 100 / portTICK_PERIOD_MS);
            buffer[read_len] = '\0';
            gps_parser((uint8_t *)buffer);
          } else {
            ESP_LOGW(TAG, "Pattern Queue Size too small");
            uart_flush_input(GPS_UART_NUM);
          }
          break;
        }
        default:
          ESP_LOGW(TAG, "unknown uart event type: %d", event.type);
          break;
      }
    }
  }
}

gps_t *gps_fetch() { return &gps_instance; } //用位址獲取gps的數值
```

## components/storage
```cpp

```

## components/slave
```cpp

```

---
# main
```cpp

```

## main/comm
```cpp
#include <esp_log.h>
#include <esp_vfs.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <inttypes.h>
#include <stdio.h>

#include "include/comm.h"
#include "include/fsm.h"
#include "include/recv.h"
#include "include/sensors.h"

static int log_vprintf(const char *fmt, va_list arguments) { //va_list原本是<stdarg.h>的，但ESP-IDF的標頭檔已經引入，故不需重新include
  static FILE *f = NULL;
  static int ret;
  f = storage_fetch();//找到SD卡檔案的pointer (FILE *)
  if (f != NULL) //找得到pointer->寫入SD卡
    ret = vfprintf(f, fmt, arguments);
  else //找不到pointer->輸出到串口（電腦）
    ret = vprintf(fmt, arguments);
  if (ret < 0) printf("Logging error: %d\n", ret);//ret回傳負數代表寫入失敗，則回報錯誤
  storage_flush();//強制將緩衝區寫入SD卡
  return ret;//回傳結果
}
```

## main/fsm
```cpp

```

## main/recv
```cpp

```

## main/sensors
```cpp

```



