# 「[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, ®, 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, ®, 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
```
Sign in with Wallet
Connect another wallet