From 45a60d64b9223a926b05476ef039645f9ea045ee Mon Sep 17 00:00:00 2001 From: shinya Date: Fri, 22 May 2026 14:23:37 +0200 Subject: [PATCH] lora ai changes not fixed probably --- chip_test_example/deploy.sh | 63 ++++ chip_test_example/icm20948.hpp | 45 ++- chip_test_example/imu_test.cpp | 6 +- chip_test_example/lora_tx.cpp | 5 + chip_test_example/lr1121_malnus.hpp | 514 +++++++++++++++++----------- 5 files changed, 411 insertions(+), 222 deletions(-) create mode 100755 chip_test_example/deploy.sh diff --git a/chip_test_example/deploy.sh b/chip_test_example/deploy.sh new file mode 100755 index 0000000..352a7d3 --- /dev/null +++ b/chip_test_example/deploy.sh @@ -0,0 +1,63 @@ +#!/bin/sh + +KEY="/home/shinya/.ssh/malnus" +SRC="../chip_test_example" +DST="/home/me/" + +deploy_and_build() { + HOST=$1 + + TOTAL_START=$(date +%s) + + echo "[$HOST] Copying..." + SCP_START=$(date +%s) + + scp -i "$KEY" -r "$SRC" me@$HOST:$DST || return 1 + + SCP_END=$(date +%s) + SCP_TIME=$((SCP_END - SCP_START)) + + echo "[$HOST] Copy completed in ${SCP_TIME}s" + + echo "[$HOST] Building..." + BUILD_START=$(date +%s) + + ssh -i "$KEY" me@$HOST \ + "cd $DST/chip_test_example && make -j4" + + BUILD_STATUS=$? + + BUILD_END=$(date +%s) + BUILD_TIME=$((BUILD_END - BUILD_START)) + + TOTAL_END=$(date +%s) + TOTAL_TIME=$((TOTAL_END - TOTAL_START)) + + echo "[$HOST] Build completed in ${BUILD_TIME}s" + echo "[$HOST] Total time: ${TOTAL_TIME}s" + + return $BUILD_STATUS +} + +deploy_and_build 10.91.51.165 & +PID1=$! + +deploy_and_build 10.91.51.166 & +PID2=$! + +wait $PID1 +STATUS1=$? + +wait $PID2 +STATUS2=$? + +echo "----------------------------------------" +echo "10.91.51.165 status: $STATUS1" +echo "10.91.51.166 status: $STATUS2" + +if [ $STATUS1 -ne 0 ] || [ $STATUS2 -ne 0 ]; then + echo "One or more builds failed" + exit 1 +fi + +echo "All builds completed successfully" diff --git a/chip_test_example/icm20948.hpp b/chip_test_example/icm20948.hpp index c4494bf..acd3627 100644 --- a/chip_test_example/icm20948.hpp +++ b/chip_test_example/icm20948.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -36,6 +37,7 @@ constexpr uint8_t REG_ACCEL_XOUT_H = 0x2D; constexpr uint8_t REG_GYRO_XOUT_H = 0x33; constexpr uint8_t REG_TEMP_OUT_H = 0x39; constexpr uint8_t REG_INT_STATUS = 0x19; +constexpr uint8_t REG_INT_STATUS_1 = 0x1A; // bit0 = RAW_DATA_0_RDY_INT constexpr uint8_t REG_BANK_SEL = 0x7F; // ---- Register map (Bank 2) ------------------------------------------------- @@ -113,7 +115,12 @@ inline bool Imu::writeReg(uint8_t reg, uint8_t val) i2c_rdwr_ioctl_data io{}; io.msgs = &msg; io.nmsgs = 1; - return ioctl(fd_, I2C_RDWR, &io) == 1; + if (ioctl(fd_, I2C_RDWR, &io) == 1) + return true; + if (cfg_.verbose) + std::fprintf(stderr, "[icm20948] write 0x%02X=0x%02X failed at 0x%02X: %s\n", + reg, val, addr_, std::strerror(errno)); + return false; } inline bool Imu::readRegs(uint8_t reg, uint8_t *buf, uint8_t n) @@ -131,13 +138,19 @@ inline bool Imu::readRegs(uint8_t reg, uint8_t *buf, uint8_t n) i2c_rdwr_ioctl_data io{}; io.msgs = msgs; io.nmsgs = 2; - return ioctl(fd_, I2C_RDWR, &io) == 2; + if (ioctl(fd_, I2C_RDWR, &io) == 2) + return true; + if (cfg_.verbose) + std::fprintf(stderr, "[icm20948] read 0x%02X (%u B) failed at 0x%02X: %s\n", + reg, n, addr_, std::strerror(errno)); + return false; } inline uint8_t Imu::readReg(uint8_t reg) { uint8_t v = 0; - readRegs(reg, &v, 1); + if (!readRegs(reg, &v, 1) && cfg_.verbose) + std::fprintf(stderr, "[icm20948] readReg(0x%02X) → no data (check NACK / wiring)\n", reg); return v; } @@ -162,8 +175,11 @@ inline bool Imu::begin(const Config &c) for (uint8_t a : {(uint8_t)0x68, (uint8_t)0x69}) { addr_ = a; // Ensure bank 0 before reading WHO_AM_I - writeReg(REG_BANK_SEL, 0x00); - uint8_t id = readReg(REG_WHO_AM_I); + if (!writeReg(REG_BANK_SEL, 0x00)) + continue; + uint8_t id = 0; + if (!readRegs(REG_WHO_AM_I, &id, 1)) + continue; if (c.verbose) std::fprintf(stderr, "[icm20948] probe 0x%02X → WHO_AM_I = 0x%02X\n", a, id); if (id == WHO_AM_I_VAL) { found = true; break; } @@ -198,6 +214,9 @@ inline bool Imu::begin(const Config &c) return false; } + // Disable internal I2C master (mag interface) — matches Adafruit begin_I2C(). + writeReg(REG_USER_CTRL, 0x00); + // Wake up: auto-select clock source (best practice per datasheet). writeReg(REG_PWR_MGMT_1, 0x01); std::this_thread::sleep_for(std::chrono::milliseconds(30)); @@ -230,19 +249,25 @@ inline void Imu::end() inline bool Imu::read(RawSample &raw) { selectBank(0); + // Wait for fresh accel/gyro/temp (datasheet: INT_STATUS_1 bit0 RAW_DATA_0_RDY_INT). + for (int i = 0; i < 50; ++i) { + if (readReg(REG_INT_STATUS_1) & 0x01) break; + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } uint8_t buf[14]{}; if (!readRegs(REG_ACCEL_XOUT_H, buf, 14)) return false; auto s16 = [](uint8_t hi, uint8_t lo) -> int16_t { return (int16_t)((uint16_t)hi << 8 | lo); }; + // Burst from 0x2D: accel (6) → gyro (6) → temp (2) raw.ax = s16(buf[0], buf[1]); raw.ay = s16(buf[2], buf[3]); raw.az = s16(buf[4], buf[5]); - raw.temp_raw = s16(buf[6], buf[7]); - raw.gx = s16(buf[8], buf[9]); - raw.gy = s16(buf[10], buf[11]); - raw.gz = s16(buf[12], buf[13]); + raw.gx = s16(buf[6], buf[7]); + raw.gy = s16(buf[8], buf[9]); + raw.gz = s16(buf[10], buf[11]); + raw.temp_raw = s16(buf[12], buf[13]); return true; } @@ -258,7 +283,7 @@ inline bool Imu::read(Sample &s) s.gx = raw.gx * gs; s.gy = raw.gy * gs; s.gz = raw.gz * gs; - // Temp formula from ICM-20948 datasheet: T°C = (raw - 0) / 333.87 + 21.0 + // Temp formula from ICM-20948 datasheet: T°C = TEMP_OUT / 333.87 + 21.0 s.temp_c = (float)raw.temp_raw / 333.87f + 21.0f; return true; } diff --git a/chip_test_example/imu_test.cpp b/chip_test_example/imu_test.cpp index 1b58b89..804e16a 100644 --- a/chip_test_example/imu_test.cpp +++ b/chip_test_example/imu_test.cpp @@ -35,10 +35,10 @@ int main(int argc, char **argv) if (!imu.begin(cfg)) { std::fprintf(stderr, "ERROR: IMU init failed\n" " Check: I2C enabled? (sudo raspi-config)\n" - " Check: sudo i2cdetect -y 1\n" - " Check: wiring SDA=GPIO2 SCL=GPIO3\n" + " Check: sudo i2cdetect -y 1 (expect 68 or 69)\n" + " Check: wiring SDA=GPIO2 SCL=GPIO3, 3.3V, GND\n" " Check: AD0 pin → 0x68 (GND) or 0x69 (VCC)\n" - " Run with -v for detailed debug output\n"); + " WHO_AM_I=0x00 usually means NACK (no chip) — run with -v\n"); return 1; } diff --git a/chip_test_example/lora_tx.cpp b/chip_test_example/lora_tx.cpp index a9d2005..0ce7b5e 100644 --- a/chip_test_example/lora_tx.cpp +++ b/chip_test_example/lora_tx.cpp @@ -42,9 +42,14 @@ int main(int argc, char **argv) if (!radio.begin(cfg)) { std::fprintf(stderr, "ERROR: radio init failed\n" " Check: SPI enabled? wiring? DIO5/DIO6 connected?\n" + " If fw looks like bootloader: sudo ./lora_rx --reset\n" " Run with -v for step-by-step output\n"); return 1; } + auto ver = radio.getVersion(); + if (ver.fw_hi < 0x02) + std::fprintf(stderr, "hint: fw=0x%02X%02X — try: sudo ./lora_rx --reset\n", + ver.fw_hi, ver.fw_lo); std::printf("Radio OK — sending every second\n"); for (int n = 0; ; ++n) { diff --git a/chip_test_example/lr1121_malnus.hpp b/chip_test_example/lr1121_malnus.hpp index 5326489..2a42728 100644 --- a/chip_test_example/lr1121_malnus.hpp +++ b/chip_test_example/lr1121_malnus.hpp @@ -1,15 +1,18 @@ -// lr1121_malnus.hpp — LR1121 LoRa driver, tuned for this exact board +// lr1121_malnus.hpp — LR1121 LoRa driver for this exact board // -// Hardware (hardwired): -// /dev/spidev0.0 CE0=GPIO8, MISO=GPIO9, MOSI=GPIO10, SCK=GPIO11 -// GPIO24 LR_DIO0/BUSY -// GPIO25 LR_nRESET -// DIO5 RF switch RFSW0 — driven by chip (HIGH in RX) -// DIO6 RF switch RFSW1 — driven by chip (HIGH in TX) +// Hardware: +// /dev/spidev0.0 SCK=GPIO11, MOSI=GPIO10, MISO=GPIO9, NSS=GPIO8 +// GPIO24 BUSY (DIO0) +// GPIO25 nRESET +// DIO5 RF-switch RFSW0 — chip-driven (HIGH in RX) +// DIO6 RF-switch RFSW1 — chip-driven (HIGH in TX/TX_HP) // -// Crystal oscillator — no TCXO, no SetTcxoMode, no calibration guessing. -// RF switch is configured via SetDioAsRfSwitch (opcode 0x022D) so DIO5/DIO6 -// are driven automatically by the chip in the correct state for TX/RX/standby. +// Crystal oscillator module (no TCXO). SetDioAsRfSwitch configures DIO5/DIO6 +// so the chip drives the module's RF switch automatically. +// +// Opcodes verified against: +// LR1121 User Manual Rev 1.2 (UM.LR1121.W.APP, Table 13) +// Semtech SWDR001 (https://github.com/Lora-net/SWDR001) #pragma once #include @@ -25,79 +28,81 @@ namespace lr1121 { -// ---- Opcodes (SWDR001 confirmed) ------------------------------------------- -// system -constexpr uint16_t OC_GET_STATUS = 0x0100; -constexpr uint16_t OC_GET_VERSION = 0x0101; // → [hw,type,fw_hi,fw_lo] +// ── Opcodes ────────────────────────────────────────────────────────────────── +// System +constexpr uint16_t OC_GET_VERSION = 0x0101; +constexpr uint16_t OC_GET_ERRORS = 0x010D; constexpr uint16_t OC_CLEAR_ERRORS = 0x010E; constexpr uint16_t OC_CALIBRATE = 0x010F; // 1B bitmask -constexpr uint16_t OC_SET_REGMODE = 0x0110; // 1B: 0=LDO, 1=DCDC -constexpr uint16_t OC_CALIBRATE_IMAGE = 0x0111; // 2B: freq1, freq2 (× 4 MHz) -constexpr uint16_t OC_SET_DIOIRQ = 0x0113; // 4B dio9_mask + 4B dio8_mask -constexpr uint16_t OC_CLEAR_IRQ = 0x0114; -constexpr uint16_t OC_REBOOT = 0x0118; // 1B: 0=app, 1=stay bootloader -constexpr uint16_t OC_SET_STANDBY = 0x011C; // 1B: 0=RC, 1=XOSC -// regmem / buffer -constexpr uint16_t OC_WRITE_BUF8 = 0x0109; -constexpr uint16_t OC_READ_BUF8 = 0x010A; // 1B offset + 1B len → N bytes -// radio -constexpr uint16_t OC_GET_PKT_STATUS = 0x0204; // LoRa → [rssi,snr,sig_rssi] -constexpr uint16_t OC_GET_RXBUF_STA = 0x0203; // → [payload_len, rx_ptr] -constexpr uint16_t OC_SET_LORA_NET = 0x0208; // 1B: 0=private, 1=LoRaWAN -constexpr uint16_t OC_SET_RX = 0x0209; // 3B timeout RTC steps (32768/s) -constexpr uint16_t OC_SET_TX = 0x020A; // 3B timeout (0=until TxDone) +constexpr uint16_t OC_SET_REGMODE = 0x0110; // 1B: 0=LDO 1=DCDC +constexpr uint16_t OC_CALIBRATE_IMAGE = 0x0111; // 2B: f1,f2 in 4-MHz steps +constexpr uint16_t OC_SET_DIO_AS_RFSW = 0x0112; // 8B: enable + 7 mode bytes +constexpr uint16_t OC_SET_DIOIRQ = 0x0113; // 8B: irq1(4B) + irq2(4B) +constexpr uint16_t OC_CLEAR_IRQ = 0x0114; // 4B mask +constexpr uint16_t OC_REBOOT = 0x0118; // 1B: 0=app 1=bootloader +constexpr uint16_t OC_GET_VBAT = 0x0119; // → 1B raw ADC (VBAT ≈ raw/34.0 V) +constexpr uint16_t OC_SET_STANDBY = 0x011C; // 1B: 0=RC 1=XOSC +// Buffer +constexpr uint16_t OC_WRITE_BUF8 = 0x0109; // N bytes +constexpr uint16_t OC_READ_BUF8 = 0x010A; // 1B offset + 1B len → data +constexpr uint16_t OC_CLEAR_RXBUF = 0x010B; +// Radio +constexpr uint16_t OC_GET_RXBUF_STA = 0x0203; // → [len, ptr] +constexpr uint16_t OC_GET_PKT_STATUS = 0x0204; // → [rssi, snr, sig_rssi] +constexpr uint16_t OC_SET_LORA_NET = 0x0208; // 1B: 0=private 1=public +constexpr uint16_t OC_SET_RX = 0x0209; // 3B timeout in RTC steps +constexpr uint16_t OC_SET_TX = 0x020A; // 3B timeout (0 = no timeout) constexpr uint16_t OC_SET_RF_FREQ = 0x020B; // 4B Hz big-endian constexpr uint16_t OC_SET_PKT_TYPE = 0x020E; // 1B: 0x02=LoRa constexpr uint16_t OC_SET_MOD_PARAM = 0x020F; // SF,BW,CR,LDRO constexpr uint16_t OC_SET_PKT_PARAM = 0x0210; // preamble(2B),hdr,len,crc,iq -constexpr uint16_t OC_SET_TX_PARAMS = 0x0211; // 1B pwr(int8) + 1B ramp -constexpr uint16_t OC_SET_PKT_ADRS = 0x0212; // 1B tx_base + 1B rx_base -constexpr uint16_t OC_SET_PA_CFG = 0x0215; // pa_sel,pa_supply,duty,hp_max -constexpr uint16_t OC_SET_FALLBACK_MODE = 0x020C; // 1B: 0x01=STBY_RC, 0x02=STBY_XOSC -// RF switch — DIO5/DIO6 driven by chip based on TX/RX/STBY state -constexpr uint16_t OC_SET_DIO_AS_RFSW = 0x022D; // 8B: enable + 7 mode masks +constexpr uint16_t OC_SET_TX_PARAMS = 0x0211; // pwr(int8),ramp +constexpr uint16_t OC_SET_PKT_ADRS = 0x0212; // tx_base,rx_base +constexpr uint16_t OC_SET_FALLBACK_MODE = 0x0213; // 1B: 0x01=STBY_RC +constexpr uint16_t OC_SET_PA_CFG = 0x0215; // pa_sel,supply,duty,hp_max -// ---- IRQ bit masks --------------------------------------------------------- -constexpr uint32_t IRQ_TX_DONE = (1u << 2); -constexpr uint32_t IRQ_RX_DONE = (1u << 3); -constexpr uint32_t IRQ_CRC_ERR = (1u << 7); +// ── Constants ───────────────────────────────────────────────────────────────── +// IRQ bits (LR1121 UM Table GetStatus / SetDioIrqParams) +constexpr uint32_t IRQ_TX_DONE = (1u << 2); +constexpr uint32_t IRQ_RX_DONE = (1u << 3); +constexpr uint32_t IRQ_CRC_ERR = (1u << 7); constexpr uint32_t IRQ_TIMEOUT = (1u << 10); -constexpr uint32_t IRQ_ALL = 0x07FF'FFFFu; +constexpr uint32_t IRQ_ALL = 0x1BF80FFCu; // all defined IRQ bits -// ---- Calibration bitmask --------------------------------------------------- -constexpr uint8_t CALIB_LF_RC = (1 << 0); -constexpr uint8_t CALIB_HF_RC = (1 << 1); -constexpr uint8_t CALIB_PLL = (1 << 2); -constexpr uint8_t CALIB_ADC = (1 << 3); -constexpr uint8_t CALIB_IMAGE = (1 << 4); -constexpr uint8_t CALIB_PLL_TX = (1 << 5); -constexpr uint8_t CALIB_ALL = 0x3F; +// Calibration bitmask (Calibrate command) +constexpr uint8_t CALIB_ALL = 0x3F; // LF_RC|HF_RC|PLL|ADC|IMG|PLL_TX -// ---- Modulation constants -------------------------------------------------- -// SF: 0x05=SF5 .. 0x0C=SF12 (0x07=SF7 is good for short-range tests) -// BW: 0x01=15.6k 0x02=31.2k 0x03=62.5k 0x04=125k 0x05=250k 0x06=500k -// CR: 0x01=4/5 0x02=4/6 0x03=4/7 0x04=4/8 -// PA: 0x00=LP (≤15 dBm), 0x01=HP (≤22 dBm) — most modules use HP +// Modulation +// SF: 0x05=SF5 .. 0x0C=SF12 +// BW: 0x01=15.6kHz 0x02=31.2 0x03=62.5 0x04=125 0x05=250 0x06=500 +// CR: 0x01=4/5 0x02=4/6 0x03=4/7 0x04=4/8 +// PA: 0x00=LP (≤15 dBm) 0x01=HP (≤22 dBm) -constexpr uint32_t FREQ_433 = 433'050'000; -constexpr uint32_t FREQ_868 = 868'000'000; -constexpr uint32_t FREQ_2400 = 2'403'000'000; +constexpr uint32_t FREQ_433 = 433'050'000u; +constexpr uint32_t FREQ_868 = 868'000'000u; +constexpr uint32_t FREQ_2400 = 2'403'000'000u; +// ── Public types ────────────────────────────────────────────────────────────── struct Config { - const char *spi_path = "/dev/spidev0.0"; - uint32_t spi_hz = 8'000'000; - const char *gpio_chip = "/dev/gpiochip0"; - unsigned busy_gpio = 24; // LR_DIO0/BUSY - unsigned reset_gpio = 25; // LR_nRESET - uint32_t freq_hz = FREQ_433; // match your antenna - uint8_t sf = 0x07; // SF7 - uint8_t bw = 0x04; // 125 kHz - uint8_t cr = 0x01; // 4/5 - int8_t tx_dbm = 14; // −17..+22 dBm - uint8_t pa_sel = 0x01; // 0x00=LP, 0x01=HP - bool use_dcdc = true; - bool lora_wan = false; // false = private sync word (0x12) - bool verbose = false; + const char *spi_path = "/dev/spidev0.0"; + uint32_t spi_hz = 8'000'000; + const char *gpio_chip = "/dev/gpiochip0"; + unsigned busy_gpio = 24; // DIO0 / BUSY + unsigned reset_gpio = 25; // nRESET + uint32_t freq_hz = FREQ_433; + uint8_t sf = 0x07; // SF7 + uint8_t bw = 0x04; // 125 kHz + uint8_t cr = 0x01; // 4/5 + int8_t tx_dbm = 14; // -17..+22 dBm + uint8_t pa_sel = 0x01; // 0x00=LP(≤15dBm) 0x01=HP(≤22dBm) + // pa_supply: 0x00=internal VREG (use for ≤14 dBm), 0x01=VBAT direct (>14 dBm) + // RadioLib: "Must be set to PA_SUPPLY_VBAT when output power is more than 14 dBm." + uint8_t pa_supply = 0x00; + // use_dcdc: false=LDO (safe default for Pi; no DCDC inductor required) + // true=DCDC (higher efficiency; only if module has external DCDC components) + bool use_dcdc = false; + bool lora_wan = false; // false = private sync word + bool verbose = false; }; struct RxInfo { @@ -108,29 +113,27 @@ struct RxInfo { struct ChipVersion { uint8_t hw; - uint8_t type; // 0x03 = LR1121 + uint8_t type; // 0x03 = LR1121 uint8_t fw_hi; uint8_t fw_lo; }; +// ── Radio class ─────────────────────────────────────────────────────────────── class Radio { public: bool begin(const Config &cfg); void end(); - // Returns true on TX done, false on timeout. - bool send(const uint8_t *data, uint8_t n); + bool send(const uint8_t *data, uint8_t n); // true = TX_DONE, false = timeout - // Returns bytes received; -1=timeout, -2=CRC error. - int receive(uint8_t *buf, uint8_t cap, uint32_t timeout_ms, - RxInfo *rx_info = nullptr); + // Returns byte count received; -1 = timeout, -2 = CRC error. + int receive(uint8_t *buf, uint8_t cap, uint32_t timeout_ms, + RxInfo *rx_info = nullptr); - uint32_t getIrq(); - void clearIrq(uint32_t mask); ChipVersion getVersion(); - // Open SPI + GPIOs and hard-reset the chip, skip calibration. - // Useful for sending diagnostic commands if begin() hangs. + // Open SPI + GPIOs and hard-reset only — no calibration. + // Use for --reset / diagnostic commands when begin() would hang. bool beginRaw(const Config &cfg); void reboot(bool stay_in_bootloader = false); @@ -145,17 +148,32 @@ private: void wcmd(uint16_t op, const uint8_t *p = nullptr, size_t n = 0); void rcmd(uint16_t op, const uint8_t *params, size_t np, uint8_t *out, size_t nr); - void waitBusy(); - void hardReset(); - bool openGpio(unsigned line, bool out, int &fd_out); - void setGpioLine(int fd, int val); - int getGpioLine(int fd); + uint32_t getIrq(); // NOP-poll: stat1,stat2,irq (no BUSY wait) + void clearIrq(uint32_t mask); + uint16_t getErrors(); // returns ErrorStat bits + bool waitBusy(); + void hardReset(); + bool openGpio(unsigned line, bool out, int &fd_out); + void setGpioLine(int fd, int val); + int getGpioLine(int fd); + bool openSpi(const Config &c); + + // Verbose-mode helpers + static const char *modeName(uint8_t stat2); + static const char *cmdName(uint8_t stat1); static void imgCalFreqs(uint32_t hz, uint8_t &f1, uint8_t &f2); static uint8_t computeLDRO(uint8_t sf, uint8_t bw); + + // Maps target output power to the recommended (PaDutyCycle, PaHpMax) per the + // LR1121 datasheet PA Settings table. For HP PA: using max drive (duty=4, + // hp_max=7) at low power draws 22 dBm worth of VBAT current even if SetTxParams + // limits output — this droops VBAT and triggers LBD. For LP PA: PaHpMax is 0. + static void computePaConfig(uint8_t pa_sel, int8_t dbm, + uint8_t &duty, uint8_t &hp_max); }; -// ---- Implementation --------------------------------------------------------- +// ── Implementation ──────────────────────────────────────────────────────────── inline void Radio::spiTransfer(uint8_t *buf, size_t n) { @@ -168,40 +186,40 @@ inline void Radio::spiTransfer(uint8_t *buf, size_t n) ioctl(spi_fd_, SPI_IOC_MESSAGE(1), &tr); } +// Write command: opcode [+ params]. Waits for BUSY before and after. inline void Radio::wcmd(uint16_t op, const uint8_t *p, size_t n) { - waitBusy(); + if (!waitBusy()) return; uint8_t buf[260]; - buf[0] = op >> 8; buf[1] = op & 0xFF; + buf[0] = op >> 8; buf[1] = op & 0xFF; if (p && n) std::memcpy(buf + 2, p, n); spiTransfer(buf, 2 + n); waitBusy(); } +// Read command: opcode [+ params], then clock NOP bytes to retrieve response. +// Response byte 0 is stat1 (skipped); bytes 1..nr go to out[0..nr-1]. inline void Radio::rcmd(uint16_t op, const uint8_t *params, size_t np, uint8_t *out, size_t nr) { - waitBusy(); + if (!waitBusy()) return; uint8_t cbuf[16]{}; - cbuf[0] = op >> 8; cbuf[1] = op & 0xFF; + cbuf[0] = op >> 8; cbuf[1] = op & 0xFF; if (params && np) std::memcpy(cbuf + 2, params, np); spiTransfer(cbuf, 2 + np); - waitBusy(); + if (!waitBusy()) return; uint8_t rbuf[260]{}; - spiTransfer(rbuf, nr + 1); // byte 0 is a dummy status + spiTransfer(rbuf, nr + 1); // rbuf[0] = stat1 (skip) std::memcpy(out, rbuf + 1, nr); } -// GetStatus: the LR1121 outputs [stat1, stat2, irq31:24, irq23:16, irq15:8, irq7:0] -// on the first 6 MISO bytes of ANY SPI transaction — regardless of what opcode is sent. -// (Confirmed in RadioLib source: it sends 6 null bytes with no opcode and reads buff[2..5].) -// Does NOT call waitBusy() — designed to be polled freely during TX/RX. +// Poll current status via 6 NOP bytes (valid at any time, including during TX/RX). +// Returns irq register [31:0]. MISO: [stat1, stat2, irq31:24, irq23:16, irq15:8, irq7:0]. inline uint32_t Radio::getIrq() { - uint8_t b[6] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + uint8_t b[6]{}; spiTransfer(b, 6); - // b[0]=stat1, b[1]=stat2, b[2..5]=irq[31:24..7:0] return ((uint32_t)b[2] << 24) | ((uint32_t)b[3] << 16) | ((uint32_t)b[4] << 8) | (uint32_t)b[5]; } @@ -213,14 +231,22 @@ inline void Radio::clearIrq(uint32_t mask) wcmd(OC_CLEAR_IRQ, d, 4); } -inline void Radio::waitBusy() +inline uint16_t Radio::getErrors() +{ + uint8_t e[2]{}; + rcmd(OC_GET_ERRORS, nullptr, 0, e, 2); + return (uint16_t)((e[0] << 8) | e[1]); +} + +inline bool Radio::waitBusy() { for (int i = 0; i < 200'000; ++i) { - if (!getGpioLine(busy_fd_)) return; + if (!getGpioLine(busy_fd_)) return true; std::this_thread::sleep_for(std::chrono::microseconds(50)); } if (cfg_.verbose) - std::fprintf(stderr, "[lr1121] waitBusy TIMEOUT after 10s — chip hung?\n"); + std::fprintf(stderr, "[lr1121] waitBusy: TIMEOUT — chip hung?\n"); + return false; } inline bool Radio::openGpio(unsigned line, bool out, int &fd_out) @@ -276,31 +302,84 @@ inline ChipVersion Radio::getVersion() return { v[0], v[1], v[2], v[3] }; } +inline const char *Radio::modeName(uint8_t stat2) +{ + switch ((stat2 >> 1) & 0x07) { + case 0: return "SLEEP"; + case 1: return "STBY_RC"; + case 2: return "STBY_XOSC"; + case 3: return "FS"; + case 4: return "RX"; + case 5: return "TX"; + default: return "?"; + } +} + +inline const char *Radio::cmdName(uint8_t stat1) +{ + switch ((stat1 >> 1) & 0x07) { + case 0: return "CMD_FAIL"; + case 1: return "CMD_PERR"; + case 2: return "CMD_OK"; + case 3: return "CMD_DAT"; + default: return "?"; + } +} + inline void Radio::imgCalFreqs(uint32_t hz, uint8_t &f1, uint8_t &f2) { + // Frequency ranges per LR1121 UM Table CalibImage; steps = 4 MHz. uint32_t mhz = hz / 1'000'000u; uint32_t lo, hi; - if (mhz < 446) { lo = 430; hi = 440; } - else if (mhz < 740) { lo = 470; hi = 510; } - else if (mhz < 890) { lo = 860; hi = 876; } - else { lo = 900; hi = 928; } + if (mhz < 446) { lo = 430; hi = 440; } + else if (mhz < 740) { lo = 470; hi = 510; } + else if (mhz < 890) { lo = 860; hi = 876; } + else if (mhz < 2000) { lo = 900; hi = 928; } + else { lo = 2400; hi = 2500; } f1 = (uint8_t)(lo / 4); f2 = (uint8_t)((hi + 3) / 4); } inline uint8_t Radio::computeLDRO(uint8_t sf, uint8_t bw) { - static const uint32_t bw_khz[] = { 0, 15625, 31250, 62500, 125000, - 250000, 500000 }; - uint32_t bw_hz = (bw < 7) ? bw_khz[bw] : 125000; - uint32_t sym_ms = (1u << sf) * 1000u / bw_hz; - return (sym_ms > 16) ? 1 : 0; + // LDRO required when symbol duration > 16 ms. + static const uint32_t bw_hz[] = { 0, 15625, 31250, 62500, 125000, 250000, 500000 }; + uint32_t bw_val = (bw < 7) ? bw_hz[bw] : 125000; + return ((1u << sf) * 1000u / bw_val > 16) ? 1 : 0; } -inline bool Radio::begin(const Config &c) +// Maps target dBm to the LR1121 datasheet recommended (PaDutyCycle, PaHpMax). +// +// HP PA (pa_sel=0x01, sub-GHz) recommended table from LR1121 datasheet §PA config: +// ≥22 dBm → duty=4, hp_max=7 ≥20 → 3,5 ≥17 → 2,3 +// ≥14 dBm → duty=2, hp_max=2 ≥10 → 1,1 < 10 → 0,0 +// +// Using duty=4, hp_max=7 for lower power levels drives all 7 PA stages at full +// current even though SetTxParams limits the output — VBAT droops under the +// excess load and triggers LBD, aborting TX before the preamble starts. +// +// LP PA (pa_sel=0x00): PaHpMax is always 0; duty controls drive, hp_max unused. +inline void Radio::computePaConfig(uint8_t pa_sel, int8_t dbm, + uint8_t &duty, uint8_t &hp_max) { - cfg_ = c; + if (pa_sel == 0x01) { // HP PA + if (dbm >= 22) { duty = 4; hp_max = 7; } + else if (dbm >= 20) { duty = 3; hp_max = 5; } + else if (dbm >= 17) { duty = 2; hp_max = 3; } + else if (dbm >= 14) { duty = 2; hp_max = 2; } + else if (dbm >= 10) { duty = 1; hp_max = 1; } + else { duty = 0; hp_max = 0; } + } else { // LP PA — PaHpMax not applicable + hp_max = 0; + if (dbm >= 14) { duty = 7; } + else if (dbm >= 10) { duty = 4; } + else if (dbm >= 0) { duty = 2; } + else { duty = 0; } + } +} +inline bool Radio::openSpi(const Config &c) +{ spi_fd_ = ::open(c.spi_path, O_RDWR); if (spi_fd_ < 0) { if (c.verbose) @@ -311,7 +390,13 @@ inline bool Radio::begin(const Config &c) ioctl(spi_fd_, SPI_IOC_WR_MODE, &mode); ioctl(spi_fd_, SPI_IOC_WR_BITS_PER_WORD, &bits); ioctl(spi_fd_, SPI_IOC_WR_MAX_SPEED_HZ, &c.spi_hz); + return true; +} +inline bool Radio::begin(const Config &c) +{ + cfg_ = c; + if (!openSpi(c)) return false; if (!openGpio(c.reset_gpio, true, reset_fd_)) return false; if (!openGpio(c.busy_gpio, false, busy_fd_)) return false; @@ -325,66 +410,92 @@ inline bool Radio::begin(const Config &c) ver.hw, ver.type, ver.fw_hi, ver.fw_lo); if (ver.type != 0x03) { if (c.verbose) - std::fprintf(stderr, "[lr1121] unexpected chip type 0x%02X (want 0x03=LR1121)\n", + std::fprintf(stderr, "[lr1121] unexpected type 0x%02X (want 0x03=LR1121)\n", ver.type); return false; } + if (ver.fw_hi < 0x02 && c.verbose) + std::fprintf(stderr, "[lr1121] warning: fw=0x%02X%02X — try: sudo ./lora_rx --reset\n", + ver.fw_hi, ver.fw_lo); -#define LR_STEP(msg) do { if (c.verbose) std::fprintf(stderr, "[lr1121] -> " msg "\n"); } while(0) + if (c.verbose) { + uint8_t raw = 0; + rcmd(OC_GET_VBAT, nullptr, 0, &raw, 1); + // VBAT ≈ raw/34.0 V (from LR1121 formula: raw*5/(4*255)/1.091) + float vbat = raw / 34.0f; + std::fprintf(stderr, "[lr1121] VBAT raw=0x%02X (~%.2fV)%s\n", raw, vbat, + (vbat < 2.5f && raw > 0) ? " ← LOW: check VBAT supply" : ""); + } - LR_STEP("SetStandby(RC)"); +#define V(msg) do { if (c.verbose) std::fprintf(stderr, "[lr1121] -> " msg "\n"); } while(0) + + // ── Init sequence (RadioLib config() order, then per-band radio config) ── + + V("SetStandby(RC)"); { const uint8_t a[] = {0x00}; wcmd(OC_SET_STANDBY, a, 1); } - // Crystal oscillator — no TCXO, no SetTcxoMode. - // Calibrate from RC clock (required), then switch to XOSC standby so the - // crystal stays running during radio configuration and TX/RX ramp-up. - // Without STBY_XOSC here the PA cold-starts the crystal on every TX and - // silently fails to ramp up. - LR_STEP("Calibrate(ALL)"); + // SetFallbackMode before Calibrate — matches RadioLib config() order. + V("SetFallbackMode(STBY_RC)"); + { const uint8_t a[] = {0x01}; wcmd(OC_SET_FALLBACK_MODE, a, 1); } + + V("ClearIrq / SetDioIrqParams(none)"); + clearIrq(IRQ_ALL); + { const uint8_t z[8]{}; wcmd(OC_SET_DIOIRQ, z, 8); } + + V("Calibrate(ALL)"); { const uint8_t a[] = {CALIB_ALL}; wcmd(OC_CALIBRATE, a, 1); } + // Calibrate can take up to several hundred ms. RadioLib does delay(5) + waitBusy. + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + if (!waitBusy()) { + if (c.verbose) std::fprintf(stderr, "[lr1121] Calibrate: BUSY stuck\n"); + return false; + } - LR_STEP("ClearErrors"); wcmd(OC_CLEAR_ERRORS); + { + uint16_t errs = getErrors(); + if (errs && c.verbose) + std::fprintf(stderr, "[lr1121] errors after calibrate: 0x%04X%s\n", errs, + (errs & (1u<<5)) ? " (HF_XOSC_START_ERR — check crystal)" : ""); + } - LR_STEP("SetStandby(XOSC)"); - { const uint8_t a[] = {0x01}; wcmd(OC_SET_STANDBY, a, 1); } // 0x01 = XOSC, not RC - -#undef LR_STEP - - // --- Radio configuration ------------------------------------------------ + // ── Radio configuration ────────────────────────────────────────────────── { const uint8_t a[] = {c.use_dcdc ? (uint8_t)0x01 : (uint8_t)0x00}; wcmd(OC_SET_REGMODE, a, 1); } { const uint8_t a[] = {0x02}; wcmd(OC_SET_PKT_TYPE, a, 1); } // LoRa - uint32_t f = c.freq_hz; - { const uint8_t a[] = { (uint8_t)(f >> 24), (uint8_t)(f >> 16), - (uint8_t)(f >> 8), (uint8_t) f }; + { uint32_t f = c.freq_hz; + const uint8_t a[] = { (uint8_t)(f>>24),(uint8_t)(f>>16),(uint8_t)(f>>8),(uint8_t)f }; wcmd(OC_SET_RF_FREQ, a, 4); } - if (f < 1'000'000'000u) { - uint8_t f1, f2; imgCalFreqs(f, f1, f2); - const uint8_t a[] = {f1, f2}; wcmd(OC_CALIBRATE_IMAGE, a, 2); - if (c.verbose) - std::fprintf(stderr, "[lr1121] image cal: 0x%02X 0x%02X\n", f1, f2); - } + { uint8_t f1, f2; imgCalFreqs(c.freq_hz, f1, f2); + const uint8_t a[] = {f1, f2}; wcmd(OC_CALIBRATE_IMAGE, a, 2); + if (c.verbose) + std::fprintf(stderr, "[lr1121] image cal: 0x%02X 0x%02X\n", f1, f2); } { const uint8_t a[] = {0x00, 0x00}; wcmd(OC_SET_PKT_ADRS, a, 2); } - uint8_t ldro = computeLDRO(c.sf, c.bw); - { const uint8_t a[] = {c.sf, c.bw, c.cr, ldro}; + { uint8_t ldro = computeLDRO(c.sf, c.bw); + const uint8_t a[] = {c.sf, c.bw, c.cr, ldro}; wcmd(OC_SET_MOD_PARAM, a, 4); if (c.verbose) std::fprintf(stderr, "[lr1121] SF=%u BW=0x%02X CR=0x%02X LDRO=%u\n", c.sf, c.bw, c.cr, ldro); } { const uint8_t a[] = {0x00, 0x08, 0x00, 0xFF, 0x01, 0x00}; - wcmd(OC_SET_PKT_PARAM, a, 6); } + wcmd(OC_SET_PKT_PARAM, a, 6); } // preamble=8, explicit hdr, maxlen=255, CRC=on - // pa_supply=0x00 → internal DC-DC regulator (correct for most modules). - // 0x01 = VBAT direct — only needed on specific high-power reference designs. - { const uint8_t a[] = {c.pa_sel, 0x00, 0x04, 0x07}; - wcmd(OC_SET_PA_CFG, a, 4); } + // PA configuration: use the power-matched (duty, hp_max) per the LR1121 + // datasheet. Oversized PA drive (duty=4, hp_max=7) at low power draws + // excess VBAT current, droops the supply, and fires LBD aborting TX. + { uint8_t duty, hp_max; + computePaConfig(c.pa_sel, c.tx_dbm, duty, hp_max); + const uint8_t a[] = {c.pa_sel, c.pa_supply, duty, hp_max}; + wcmd(OC_SET_PA_CFG, a, 4); + if (c.verbose) + std::fprintf(stderr, "[lr1121] PA: sel=0x%02X supply=0x%02X duty=%u hp_max=%u\n", + c.pa_sel, c.pa_supply, duty, hp_max); } { const uint8_t a[] = {(uint8_t)(int8_t)c.tx_dbm, 0x02}; wcmd(OC_SET_TX_PARAMS, a, 2); } @@ -392,32 +503,21 @@ inline bool Radio::begin(const Config &c) { const uint8_t a[] = {c.lora_wan ? (uint8_t)0x01 : (uint8_t)0x00}; wcmd(OC_SET_LORA_NET, a, 1); } - // RF switch via SetDioAsRfSwitch (0x022D). - // enable=0x03 → DIO5 and DIO6 are RF switch outputs. - // Bit 0 = DIO5 (RFSW0), bit 1 = DIO6 (RFSW1). - // standby: both LOW | rx: DIO5=HIGH, DIO6=LOW | tx/tx_hp: DIO5=LOW, DIO6=HIGH - { const uint8_t a[] = { 0x03, // enable: DIO5 + DIO6 - 0x00, // standby: both LOW - 0x01, // rx: DIO5=HIGH - 0x02, // tx: DIO6=HIGH - 0x02, // tx_hp: DIO6=HIGH - 0x00, // tx_hf: both LOW (2.4 GHz path unused) - 0x00, // gnss: both LOW - 0x00}; // wifi: both LOW + // DIO5 = RFSW0, DIO6 = RFSW1 (chip-driven RF switch). + // standby: both LOW | rx: DIO5=HIGH | tx/tx_hp: DIO6=HIGH + { const uint8_t a[] = {0x03, 0x00, 0x01, 0x02, 0x02, 0x00, 0x00, 0x00}; wcmd(OC_SET_DIO_AS_RFSW, a, 8); - if (c.verbose) std::fprintf(stderr, "[lr1121] RF switch configured (DIO5/DIO6)\n"); } + if (c.verbose) std::fprintf(stderr, "[lr1121] RF switch: DIO5/DIO6\n"); } - // After TX/RX (or an aborted TX due to EOL), fall back to STBY_RC so the - // chip is in a known state. Without this the fallback is undefined and - // subsequent SetStandby + SetTx sequences can be silently ignored. - { const uint8_t a[] = {0x01}; wcmd(OC_SET_FALLBACK_MODE, a, 1); } + // Switch to crystal oscillator standby so the crystal is already running + // when SetTx is issued — avoids cold-start failure on crystal-based modules. + V("SetStandby(XOSC)"); + { const uint8_t a[] = {0x01}; wcmd(OC_SET_STANDBY, a, 1); } - const uint8_t irq_zero[8]{}; - wcmd(OC_SET_DIOIRQ, irq_zero, 8); - clearIrq(IRQ_ALL); +#undef V if (c.verbose) - std::fprintf(stderr, "[lr1121] init OK: %u Hz, SF%u, PA=%s, crystal osc\n", + std::fprintf(stderr, "[lr1121] init OK: %u Hz SF%u PA=%s\n", c.freq_hz, c.sf, c.pa_sel ? "HP" : "LP"); return true; } @@ -425,21 +525,9 @@ inline bool Radio::begin(const Config &c) inline bool Radio::beginRaw(const Config &c) { cfg_ = c; - - spi_fd_ = ::open(c.spi_path, O_RDWR); - if (spi_fd_ < 0) { - if (c.verbose) - std::fprintf(stderr, "[lr1121] cannot open %s: %m\n", c.spi_path); - return false; - } - uint8_t mode = SPI_MODE_0, bits = 8; - ioctl(spi_fd_, SPI_IOC_WR_MODE, &mode); - ioctl(spi_fd_, SPI_IOC_WR_BITS_PER_WORD, &bits); - ioctl(spi_fd_, SPI_IOC_WR_MAX_SPEED_HZ, &c.spi_hz); - + if (!openSpi(c)) return false; if (!openGpio(c.reset_gpio, true, reset_fd_)) return false; if (!openGpio(c.busy_gpio, false, busy_fd_)) return false; - { uint8_t nop = 0x00; spiTransfer(&nop, 1); } std::this_thread::sleep_for(std::chrono::milliseconds(1)); hardReset(); @@ -462,43 +550,54 @@ inline void Radio::end() inline bool Radio::send(const uint8_t *data, uint8_t n) { - // Return chip to XOSC standby and clear any prior error/IRQ state before TX. + // Crystal must be warm for TX: switch to XOSC standby before SetTx. { const uint8_t a[] = {0x01}; wcmd(OC_SET_STANDBY, a, 1); } - wcmd(OC_CLEAR_ERRORS); wcmd(OC_WRITE_BUF8, data, n); { const uint8_t a[] = {0x00, 0x08, 0x00, n, 0x01, 0x00}; wcmd(OC_SET_PKT_PARAM, a, 6); } - // Enable TX_DONE and TIMEOUT on DIO9 — matches RadioLib's startTransmit(). - // IRQ register is updated regardless, but this also lets DIO9 signal completion. - { const uint8_t a[] = {0x00, 0x00, 0x04, 0x04, // DIO9: TX_DONE(bit2)|TIMEOUT(bit10) - 0x00, 0x00, 0x00, 0x00}; // DIO8: none - wcmd(OC_SET_DIOIRQ, a, 8); } + { const uint8_t a[] = {0x00, 0x00, 0x04, 0x04, 0x00, 0x00, 0x00, 0x00}; + wcmd(OC_SET_DIOIRQ, a, 8); } // DIO9: TX_DONE(bit2) | TIMEOUT(bit10) clearIrq(IRQ_ALL); + { const uint8_t a[] = {0x00, 0x00, 0x00}; wcmd(OC_SET_TX, a, 3); } + // wcmd() already waited for BUSY low (= PA ramped, preamble on air). + + if (cfg_.verbose) { + uint8_t b[6]{}; spiTransfer(b, 6); + uint32_t irq = ((uint32_t)b[2]<<24)|((uint32_t)b[3]<<16)|((uint32_t)b[4]<<8)|b[5]; + std::fprintf(stderr, + "[lr1121] after SetTx: mode=%s cmd=%s irq=0x%08X errs=0x%04X\n", + modeName(b[1]), cmdName(b[0]), irq, getErrors()); + } for (int i = 0; i < 50'000; ++i) { uint32_t irq = getIrq(); if (irq & IRQ_TX_DONE) { clearIrq(IRQ_ALL); - // Restore DIO IRQ mask to silent. - const uint8_t z[8]{}; wcmd(OC_SET_DIOIRQ, z, 8); + const uint8_t z[8]{}; wcmd(OC_SET_DIOIRQ, z, 8); return true; } std::this_thread::sleep_for(std::chrono::microseconds(100)); } - // Read stat bytes alongside IRQ for diagnosis. - uint8_t sb[6] = {}; spiTransfer(sb, 6); - uint32_t irq_now = ((uint32_t)sb[2]<<24)|((uint32_t)sb[3]<<16)|((uint32_t)sb[4]<<8)|sb[5]; - if (cfg_.verbose) - std::fprintf(stderr, "[lr1121] send: TX timeout stat1=0x%02X stat2=0x%02X irq=0x%08X\n", - sb[0], sb[1], irq_now); + // Timeout — print diagnostics then clean up. + if (cfg_.verbose) { + uint8_t b[6]{}; spiTransfer(b, 6); + uint32_t irq = ((uint32_t)b[2]<<24)|((uint32_t)b[3]<<16)|((uint32_t)b[4]<<8)|b[5]; + std::fprintf(stderr, + "[lr1121] TX timeout: mode=%s cmd=%s irq=0x%08X errs=0x%04X\n", + modeName(b[1]), cmdName(b[0]), irq, getErrors()); + if (irq & (1u<<21)) + std::fprintf(stderr, + "[lr1121] ↳ LBD (Low Battery Detect): VBAT drooped under PA load.\n" + "[lr1121] Check VBAT supply / decoupling. Try lower tx_dbm or pa_sel=LP.\n"); + } - const uint8_t z[8]{}; wcmd(OC_SET_DIOIRQ, z, 8); clearIrq(IRQ_ALL); + const uint8_t z[8]{}; wcmd(OC_SET_DIOIRQ, z, 8); { const uint8_t a[] = {0x01}; wcmd(OC_SET_STANDBY, a, 1); } return false; } @@ -506,42 +605,39 @@ inline bool Radio::send(const uint8_t *data, uint8_t n) inline int Radio::receive(uint8_t *buf, uint8_t cap, uint32_t timeout_ms, RxInfo *rx_info) { - { const uint8_t a[] = {0x01}; wcmd(OC_SET_STANDBY, a, 1); } + { const uint8_t a[] = {0x01}; wcmd(OC_SET_STANDBY, a, 1); } // XOSC warm - // Enable RX_DONE(bit3), TIMEOUT(bit10), CRC_ERR(bit7) on DIO9 — matches RadioLib. - { const uint8_t a[] = {0x00, 0x00, 0x04, 0x88, // DIO9: RX_DONE|TIMEOUT|CRC_ERR - 0x00, 0x00, 0x00, 0x00}; - wcmd(OC_SET_DIOIRQ, a, 8); } + { const uint8_t a[] = {0x00, 0x00, 0x04, 0x88, 0x00, 0x00, 0x00, 0x00}; + wcmd(OC_SET_DIOIRQ, a, 8); } // RX_DONE(bit3)|CRC_ERR(bit7)|TIMEOUT(bit10) clearIrq(IRQ_ALL); + uint32_t t = (timeout_ms == 0) ? 0x00FF'FFFFu : (uint32_t)(timeout_ms * 32768ull / 1000); - { const uint8_t a[] = {(uint8_t)(t >> 16), (uint8_t)(t >> 8), (uint8_t)t}; + { const uint8_t a[] = {(uint8_t)(t>>16),(uint8_t)(t>>8),(uint8_t)t}; wcmd(OC_SET_RX, a, 3); } for (;;) { uint32_t irq = getIrq(); if (irq & IRQ_RX_DONE) { - bool crc_err = irq & IRQ_CRC_ERR; - clearIrq(IRQ_ALL); - if (rx_info) { uint8_t ps[3]{}; rcmd(OC_GET_PKT_STATUS, nullptr, 0, ps, 3); - rx_info->rssi_dbm = -(int8_t)(ps[0] >> 1); - rx_info->snr_db = ((int8_t)ps[1] + 2) >> 2; - rx_info->signal_rssi_dbm = -(int8_t)(ps[2] >> 1); + rx_info->rssi_dbm = (int8_t)(-(int)ps[0] / 2); + rx_info->snr_db = (int8_t)((int8_t)ps[1] / 4); + rx_info->signal_rssi_dbm = (int8_t)(-(int)ps[2] / 2); } - - if (crc_err) return -2; + if (irq & IRQ_CRC_ERR) { clearIrq(IRQ_ALL); return -2; } uint8_t stat[2]{}; rcmd(OC_GET_RXBUF_STA, nullptr, 0, stat, 2); uint8_t len = (stat[0] < cap) ? stat[0] : cap; uint8_t p[2] = { stat[1], len }; rcmd(OC_READ_BUF8, p, 2, buf, len); - return len; + wcmd(OC_CLEAR_RXBUF); + clearIrq(IRQ_ALL); + return (int)len; } if (irq & IRQ_TIMEOUT) { clearIrq(IRQ_ALL); return -1; }