diff --git a/src/shared/radio/SX1278/SX1278Common.cpp b/src/shared/radio/SX1278/SX1278Common.cpp index 0e744281360ab844f7eb93aad5192109f7698984..91d3b0b914dc89ff183f2efcf83d9d6652454a7e 100644 --- a/src/shared/radio/SX1278/SX1278Common.cpp +++ b/src/shared/radio/SX1278/SX1278Common.cpp @@ -53,11 +53,10 @@ void SX1278Common::setDefaultMode(Mode mode, DioMapping mapping, mutex.unlock(); } -void SX1278Common::waitForIrq(LockMode &_guard, IrqFlags irq, bool unlock) +ISX1278::IrqFlags SX1278Common::waitForIrq(LockMode &guard, IrqFlags set_irq, + IrqFlags reset_irq, bool unlock) { - // Take a reference to a _guard to MAKE SURE that the mutex is locked, but - // otherwise don't do anything with it - (void)_guard; + IrqFlags ret_irq = 0; do { @@ -68,45 +67,31 @@ void SX1278Common::waitForIrq(LockMode &_guard, IrqFlags irq, bool unlock) } // Check that this hasn't already happened - if (checkForIrqAndReset(irq)) + if ((ret_irq = checkForIrqAndReset(set_irq, reset_irq)) != 0) { - return; + break; } - if (unlock) - { - mutex.unlock(); - } - - { - miosix::FastInterruptDisableLock dLock; - while (state.irq_wait_thread) - { - miosix::Thread::IRQwait(); - { - miosix::FastInterruptEnableLock eLock(dLock); - miosix::Thread::yield(); - } - } - } + waitForIrqInner(guard, unlock); - // Regain ownership of the lock - if (unlock) - { - mutex.lock(); - } + // TODO: Check state of the device, and reset if needed! // Protect against sporadic IRQs - } while (!checkForIrqAndReset(irq)); + } while ((ret_irq = checkForIrqAndReset(set_irq, reset_irq)) == 0); + + return ret_irq; } -bool SX1278Common::waitForIrqBusy(LockMode &_guard, IrqFlags irq, int timeout) +ISX1278::IrqFlags SX1278Common::waitForIrqBusy(LockMode &_guard, + IrqFlags set_irq, + IrqFlags reset_irq, int timeout) { // Take a reference to a _guard to MAKE SURE that the mutex is locked, but // otherwise don't do anything with it (void)_guard; - long long start = miosix::getTick(); + long long start = miosix::getTick(); + IrqFlags ret_irq = 0; while ((miosix::getTick() - start) < timeout) { @@ -116,34 +101,63 @@ bool SX1278Common::waitForIrqBusy(LockMode &_guard, IrqFlags irq, int timeout) // Tight loop on IRQ register for (unsigned int i = 0; i < 1000 / DELAY; i++) { - if (checkForIrqAndReset(irq)) + // Check if some of the interrupts triggered + if ((ret_irq = checkForIrqAndReset(set_irq, reset_irq)) != 0) { - return true; + return ret_irq; } miosix::delayUs(DELAY); } } - return false; + return 0; } -bool SX1278Common::checkForIrqAndReset(IrqFlags irq) +void SX1278Common::waitForIrqInner(LockMode &_guard, bool unlock) { - IrqFlags cur_irq = getIrqFlags(); - if (cur_irq & irq) + // Take a reference to a _guard to MAKE SURE that the mutex is locked, but + // otherwise don't do anything with it + (void)_guard; + + // Release the lock for others to take + if (unlock) { - // Reset all of the interrupts we have detected - resetIrqFlags(cur_irq & irq); + mutex.unlock(); + } - return true; + { + miosix::FastInterruptDisableLock dLock; + while (state.irq_wait_thread) + { + miosix::Thread::IRQwait(); + { + miosix::FastInterruptEnableLock eLock(dLock); + miosix::Thread::yield(); + } + } } - else + + // Regain ownership of the lock + if (unlock) { - return false; + mutex.lock(); } } +ISX1278::IrqFlags SX1278Common::checkForIrqAndReset(IrqFlags set_irq, + IrqFlags reset_irq) +{ + IrqFlags cur_irq = getIrqFlags(); + if (cur_irq & set_irq) + { + // Reset all of the interrupts we have detected + resetIrqFlags(cur_irq & set_irq); + } + + return (cur_irq & set_irq) | (~cur_irq & reset_irq); +} + ISX1278Frontend &SX1278Common::getFrontend() { return *frontend; } SPISlave &SX1278Common::getSpiSlave() { return slave; } diff --git a/src/shared/radio/SX1278/SX1278Common.h b/src/shared/radio/SX1278/SX1278Common.h index 605c97c34c9511884a8877c5487352067547b33e..744f3f7cd4813358555af57806811c67a994aee7 100644 --- a/src/shared/radio/SX1278/SX1278Common.h +++ b/src/shared/radio/SX1278/SX1278Common.h @@ -26,8 +26,8 @@ #include <miosix.h> #include <radio/Transceiver.h> -#include <memory> #include <cmath> +#include <memory> #include "SX1278Defs.h" @@ -51,12 +51,14 @@ public: virtual float getLastRxRssi() = 0; /** - * @brief Get the frequency error index in Hz, during last packet receive (NaN if not available). + * @brief Get the frequency error index in Hz, during last packet receive + * (NaN if not available). */ virtual float getLastRxFei() { return std::nanf(""); } /** - * @brief Get the signal to noise ratio, during last packet receive (NaN if not available). + * @brief Get the signal to noise ratio, during last packet receive (NaN if + * not available). */ virtual float getLastRxSnr() { return std::nanf(""); } @@ -178,25 +180,37 @@ protected: /** * @brief Wait for generic irq. */ - void waitForIrq(LockMode &guard, IrqFlags irq, bool unlock = false); + IrqFlags waitForIrq(LockMode &guard, IrqFlags set_irq, IrqFlags reset_irq, + bool unlock = false); /** * @brief Busy waits for an interrupt by polling the irq register. * * USE ONLY DURING INITIALIZATION! BAD THINGS *HAVE* HAPPENED DUE TO THIS! */ - bool waitForIrqBusy(LockMode &guard, IrqFlags irq, int timeout); + IrqFlags waitForIrqBusy(LockMode &guard, IrqFlags set_irq, + IrqFlags reset_irq, int timeout); /** - * @brief Returns if an interrupt happened, and clears it if it did. + * @brief Returns a mask containing triggered interrupts. + * + * NOTE: This function checks both set irqs (rising edge), and reset irqs + * (falling edge). But it only resets set interrupts. + * + * @param set_irq Mask containing set (rising edge) interrupts. + * @param reset_irq Mask containing reset (falling edge) interrupts. + * @return Mask containing all triggered interrupts (both rising and + * falling) */ - bool checkForIrqAndReset(IrqFlags irq); + IrqFlags checkForIrqAndReset(IrqFlags set_irq, IrqFlags reset_irq); ISX1278Frontend &getFrontend(); SPISlave &getSpiSlave(); private: + void waitForIrqInner(LockMode &guard, bool unlock); + DeviceState lockMode(Mode mode, DioMapping mapping, bool set_tx_frontend_on, bool set_rx_frontend_on); void unlockMode(DeviceState old_state); diff --git a/src/shared/radio/SX1278/SX1278Fsk.cpp b/src/shared/radio/SX1278/SX1278Fsk.cpp index 0677a3edef766ef4b56e53e24e345108f98e4cb8..d7cd936d76a382cf496fb04a3caadb4f77807363 100644 --- a/src/shared/radio/SX1278/SX1278Fsk.cpp +++ b/src/shared/radio/SX1278/SX1278Fsk.cpp @@ -69,13 +69,12 @@ bool SX1278Fsk::checkVersion() SX1278Fsk::Error SX1278Fsk::configure(const Config &config) { - // First cycle the device to bring it into Fsk mode (can only be changed in - // sleep) - setDefaultMode(RegOpMode::MODE_SLEEP, DEFAULT_MAPPING, false, false); - miosix::Thread::sleep(1); + // First make sure the device is in fsk and in standby + enterFskMode(); - // Make sure the device remains in RX and not in sleep - setDefaultMode(RegOpMode::MODE_RX, DEFAULT_MAPPING, false, false); + // Set default mode to standby, that way we reset the fifo every time we + // enter receive + setDefaultMode(RegOpMode::MODE_STDBY, DEFAULT_MAPPING, false, false); miosix::Thread::sleep(1); // Lock the bus @@ -83,7 +82,7 @@ SX1278Fsk::Error SX1278Fsk::configure(const Config &config) LockMode guard_mode(*this, guard, RegOpMode::MODE_STDBY, DEFAULT_MAPPING); // The datasheet lies, this IRQ is unreliable, it doesn't always trigger - // if (!waitForIrqBusy(guard_mode, RegIrqFlags::MODE_READY, 1000)) + // if (!waitForIrqBusy(guard_mode, RegIrqFlags::MODE_READY, 0, 1000)) // return Error::IRQ_TIMEOUT; int bitrate = config.bitrate; @@ -99,6 +98,8 @@ SX1278Fsk::Error SX1278Fsk::configure(const Config &config) RegPacketConfig1::DcFree dc_free = static_cast<RegPacketConfig1::DcFree>(config.dc_free); + crc_enabled = config.enable_crc; + { SPITransaction spi(getSpiSlave()); @@ -200,18 +201,23 @@ SX1278Fsk::Error SX1278Fsk::configure(const Config &config) REG_PACKET_CONFIG_1, RegPacketConfig1::make( RegPacketConfig1::CRC_WHITENING_TYPE_CCITT_CRC, - RegPacketConfig1::ADDRESS_FILTERING_NONE, false, true, dc_free, - RegPacketConfig1::PACKET_FORMAT_VARIABLE_LENGTH)); + RegPacketConfig1::ADDRESS_FILTERING_NONE, true, crc_enabled, + dc_free, RegPacketConfig1::PACKET_FORMAT_VARIABLE_LENGTH)); spi.writeRegister( REG_PACKET_CONFIG_2, RegPacketConfig2::make(false, false, false, RegPacketConfig2::DATA_MODE_PACKET)); + // Set maximum payload length + spi.writeRegister(REG_PACKET_PAYLOAD_LENGTH, MTU); + + // Setup threshold to half of the fifo spi.writeRegister( REG_FIFO_THRESH, RegFifoThresh::make( - 0x0f, RegFifoThresh::TX_START_CONDITION_FIFO_NOT_EMPTY)); + FIFO_LEN / 2, + RegFifoThresh::TX_START_CONDITION_FIFO_NOT_EMPTY)); spi.writeRegister(REG_NODE_ADRS, 0x00); spi.writeRegister(REG_BROADCAST_ADRS, 0x00); @@ -226,31 +232,77 @@ ssize_t SX1278Fsk::receive(uint8_t *pkt, size_t max_len) LockMode guard_mode(*this, guard, RegOpMode::MODE_RX, DEFAULT_MAPPING, false, true); + // Save the packet locally, we always want to read it all + uint8_t tmp_pkt[MTU]; + + // TODO: Maybe flush stuff? + uint8_t len = 0; + bool crc_ok = false; + do { - // Special wait for payload ready - waitForIrq(guard_mode, RegIrqFlags::PAYLOAD_READY, true); + uint8_t cur_len = 0; + + // Special wait for fifo level/payload ready, release the lock at this + // stage + if ((waitForIrq(guard_mode, + RegIrqFlags::FIFO_LEVEL | RegIrqFlags::PAYLOAD_READY, 0, + true) & + RegIrqFlags::PAYLOAD_READY) != 0 && + crc_enabled) + { + crc_ok = checkForIrqAndReset(RegIrqFlags::CRC_OK, 0) != 0; + } + last_rx_rssi = getRssi(); + // Now first packet bit { SPITransaction spi(getSpiSlave()); len = spi.readRegister(REG_FIFO); - if (len > max_len) - return -1; - spi.readRegisters(REG_FIFO, pkt, len); + int read_size = std::min((int)len, FIFO_LEN / 2); + // Skip 0 sized read + if (read_size != 0) + spi.readRegisters(REG_FIFO, &tmp_pkt[cur_len], read_size); + + cur_len += read_size; + } + + // Then read the other chunks + while (cur_len < len) + { + if ((waitForIrq( + guard_mode, + RegIrqFlags::FIFO_LEVEL | RegIrqFlags::PAYLOAD_READY, 0) & + RegIrqFlags::PAYLOAD_READY) != 0 && + crc_enabled) + { + crc_ok = checkForIrqAndReset(RegIrqFlags::CRC_OK, 0) != 0; + } + + SPITransaction spi(getSpiSlave()); + + int read_size = std::min((int)(len - cur_len), FIFO_LEN / 2); + spi.readRegisters(REG_FIFO, &tmp_pkt[cur_len], read_size); + + cur_len += read_size; } // For some reason this sometimes happen? } while (len == 0); + if (len > max_len || (!crc_ok && crc_enabled)) + return -1; + + // Finally copy the packet to the destination + memcpy(pkt, tmp_pkt, len); return len; } bool SX1278Fsk::send(uint8_t *pkt, size_t len) { - // Packets longer than FIFO_LEN (-1 for the len byte) are not supported if (len > MTU) return false; @@ -262,17 +314,39 @@ bool SX1278Fsk::send(uint8_t *pkt, size_t len) LockMode guard_mode(*this, guard, RegOpMode::MODE_TX, DEFAULT_MAPPING, true, false); - waitForIrq(guard_mode, RegIrqFlags::TX_READY); + waitForIrq(guard_mode, RegIrqFlags::TX_READY, 0); + // Send first segment { SPITransaction spi(getSpiSlave()); spi.writeRegister(REG_FIFO, static_cast<uint8_t>(len)); - spi.writeRegisters(REG_FIFO, pkt, len); + + int write_size = std::min((int)len, FIFO_LEN - 1); + spi.writeRegisters(REG_FIFO, pkt, write_size); + + pkt += write_size; + len -= write_size; } + // Then send the rest using fifo control + while (len > 0) + { + // Wait for FIFO_LEVEL to go down + waitForIrq(guard_mode, 0, RegIrqFlags::FIFO_LEVEL); + + SPITransaction spi(getSpiSlave()); + + int write_size = std::min((int)len, FIFO_LEN / 2); + spi.writeRegisters(REG_FIFO, pkt, write_size); + + pkt += write_size; + len -= write_size; + } + + // Finally wait for packet sent // Wait for packet sent - waitForIrq(guard_mode, RegIrqFlags::PACKET_SENT); + waitForIrq(guard_mode, RegIrqFlags::PACKET_SENT, 0); last_tx = now(); return true; @@ -292,6 +366,24 @@ float SX1278Fsk::getCurRssi() return getRssi(); } +void SX1278Fsk::enterFskMode() +{ + Lock guard(*this); + SPITransaction spi(getSpiSlave()); + + // First enter Fsk sleep + spi.writeRegister(REG_OP_MODE, + RegOpMode::make(RegOpMode::MODE_SLEEP, true, + RegOpMode::MODULATION_TYPE_FSK)); + miosix::Thread::sleep(1); + + // Then transition to standby + spi.writeRegister(REG_OP_MODE, + RegOpMode::make(RegOpMode::MODE_STDBY, true, + RegOpMode::MODULATION_TYPE_FSK)); + miosix::Thread::sleep(1); +} + void SX1278Fsk::rateLimitTx() { const long long RATE_LIMIT = 2; diff --git a/src/shared/radio/SX1278/SX1278Fsk.h b/src/shared/radio/SX1278/SX1278Fsk.h index 153fe72b71c445767cc288612b7d25461f6e1c9a..994de6ddb65d7e2d0f3598c96229dce3b756c2ea 100644 --- a/src/shared/radio/SX1278/SX1278Fsk.h +++ b/src/shared/radio/SX1278/SX1278Fsk.h @@ -41,7 +41,7 @@ public: /** * @brief Maximum transmittable unit of this driver. */ - static constexpr size_t MTU = SX1278::Fsk::FIFO_LEN - 1; + static constexpr size_t MTU = 255; /** * @brief Requested SX1278 configuration. @@ -117,7 +117,8 @@ public: // full power). Shaping shaping = Shaping::GAUSSIAN_BT_1_0; //< Shaping applied to // the modulation stream. - DcFree dc_free = DcFree::WHITENING; //< Dc free encoding scheme. + DcFree dc_free = DcFree::WHITENING; //< Dc free encoding scheme. + bool enable_crc = true; //< Enable hardware CRC calculation/checking }; /** @@ -137,7 +138,8 @@ public: explicit SX1278Fsk(SPIBus &bus, miosix::GpioPin cs, SPI::ClockDivider clock_divider, std::unique_ptr<SX1278::ISX1278Frontend> frontend) - : SX1278Common(bus, cs, clock_divider, std::move(frontend)) + : SX1278Common(bus, cs, clock_divider, std::move(frontend)), + crc_enabled(false) { } @@ -161,7 +163,7 @@ public: * * @param pkt Buffer to store the received packet into. * @param pkt_len Maximum length of the received data. - * @return Size of the data received or -1 if failure + * @return Size of the data received or -1 on failure */ ssize_t receive(uint8_t *pkt, size_t max_len) override; @@ -192,6 +194,8 @@ public: float getLastRxFei() override; private: + void enterFskMode(); + void rateLimitTx(); IrqFlags getIrqFlags() override; @@ -203,6 +207,7 @@ private: void setMode(Mode mode) override; void setMapping(SX1278::DioMapping mapping) override; + bool crc_enabled; long long last_tx = 0; float last_rx_rssi = 0.0f; PrintLogger logger = Logging::getLogger("sx1278"); diff --git a/src/shared/radio/SX1278/SX1278Lora.cpp b/src/shared/radio/SX1278/SX1278Lora.cpp index 682f1dc39daf9eadf063b14cedb6f819fd32ccbe..744f8d06720cfa657a0d04a19ff49b3597193b5c 100644 --- a/src/shared/radio/SX1278/SX1278Lora.cpp +++ b/src/shared/radio/SX1278/SX1278Lora.cpp @@ -159,10 +159,10 @@ bool SX1278Lora::checkVersion() SX1278Lora::Error SX1278Lora::configure(const Config &config) { - // First cycle the device to bring it into LoRa mode (can only be changed in - // sleep) - setDefaultMode(RegOpMode::MODE_SLEEP, DEFAULT_MAPPING, false, false); - // Make sure the device remains in standby and not in sleep + // First make sure the device is in lora mode and in standby + enterLoraMode(); + + // Then make sure the device remains in standby and not in sleep setDefaultMode(RegOpMode::MODE_STDBY, DEFAULT_MAPPING, false, false); // Lock the bus @@ -190,6 +190,8 @@ SX1278Lora::Error SX1278Lora::configure(const Config &config) ErrataRegistersValues errata_values = ErrataRegistersValues::calculate(bw, freq_rf); + crc_enabled = config.enable_crc; + { SPITransaction spi(getSpiSlave()); @@ -244,7 +246,7 @@ SX1278Lora::Error SX1278Lora::configure(const Config &config) spi.writeRegister(REG_MODEM_CONFIG_1, RegModemConfig1::make(false, cr, bw)); spi.writeRegister(REG_MODEM_CONFIG_2, - RegModemConfig2::make(false, false, sf)); + RegModemConfig2::make(crc_enabled, false, sf)); spi.writeRegister(REG_MODEM_CONFIG_3, RegModemConfig3::make(true, low_data_rate_optimize)); @@ -277,24 +279,26 @@ ssize_t SX1278Lora::receive(uint8_t *pkt, size_t max_len) { Lock guard(*this); - SPITransaction spi(getSpiSlave()); + // Use continuous because it doesn't go into timeout (and you cannot + // disable it...) + LockMode mode_guard(*this, guard, RegOpMode::MODE_RXCONTINUOUS, + DioMapping(0, 0, 0, 0, 0, 0), false, true); - do - { - // Use continuous because it doesn't go into timeout (and you cannot - // disable it...) - LockMode mode_guard(*this, guard, RegOpMode::MODE_RXCONTINUOUS, - DioMapping(0, 0, 0, 0, 0, 0), false, true); + waitForIrq(mode_guard, RegIrqFlags::RX_DONE, 0, true); - waitForIrq(mode_guard, RegIrqFlags::RX_DONE, true); - } while (checkForIrqAndReset(RegIrqFlags::PAYLOAD_CRC_ERROR)); + uint8_t len; + { + SPITransaction spi(getSpiSlave()); + len = spi.readRegister(REG_RX_NB_BYTES); + } - uint8_t len = spi.readRegister(REG_RX_NB_BYTES); - if (len > max_len) + if (len > max_len || + (crc_enabled && + checkForIrqAndReset(RegIrqFlags::PAYLOAD_CRC_ERROR, 0) != 0)) return -1; + // Finally read the contents of the fifo readFifo(FIFO_RX_BASE_ADDR, pkt, len); - return len; } @@ -316,7 +320,7 @@ bool SX1278Lora::send(uint8_t *pkt, size_t len) DioMapping(1, 0, 0, 0, 0, 0), true, false); // Wait for the transmission to end - waitForIrq(mode_guard, RegIrqFlags::TX_DONE); + waitForIrq(mode_guard, RegIrqFlags::TX_DONE, 0); } return true; @@ -349,6 +353,22 @@ float SX1278Lora::getLastRxSnr() 4.0f; } +void SX1278Lora::enterLoraMode() +{ + Lock guard(*this); + SPITransaction spi(getSpiSlave()); + + // First enter LoRa sleep + spi.writeRegister(REG_OP_MODE, + RegOpMode::make(RegOpMode::MODE_SLEEP, true, false)); + miosix::Thread::sleep(1); + + // Then transition to standby + spi.writeRegister(REG_OP_MODE, + RegOpMode::make(RegOpMode::MODE_STDBY, true, false)); + miosix::Thread::sleep(1); +} + void SX1278Lora::readFifo(uint8_t addr, uint8_t *dst, uint8_t size) { SPITransaction spi(getSpiSlave()); diff --git a/src/shared/radio/SX1278/SX1278Lora.h b/src/shared/radio/SX1278/SX1278Lora.h index e7e04b7a8cb0d44ddd6b749968323fd1dfbccdcb..da4a1905b591741972e4adddfc204844a9498f78 100644 --- a/src/shared/radio/SX1278/SX1278Lora.h +++ b/src/shared/radio/SX1278/SX1278Lora.h @@ -105,6 +105,7 @@ public: 13; //< Output power in dB (between +2 and +17 with pa_boost = on, // and between +0 and +14 with pa_boost = off, +20 for +20dBm // max power ignoring pa_boost). + bool enable_crc = true; //< Enable hardware CRC calculation/checking /** * @brief Calculates effective and usable bitrate. @@ -138,7 +139,8 @@ public: explicit SX1278Lora(SPIBus &bus, miosix::GpioPin cs, SPI::ClockDivider clock_divider, std::unique_ptr<SX1278::ISX1278Frontend> frontend) - : SX1278Common(bus, cs, clock_divider, std::move(frontend)) + : SX1278Common(bus, cs, clock_divider, std::move(frontend)), + crc_enabled(false) { } @@ -188,6 +190,8 @@ public: float getLastRxSnr() override; private: + void enterLoraMode(); + void readFifo(uint8_t addr, uint8_t *dst, uint8_t size); void writeFifo(uint8_t addr, uint8_t *src, uint8_t size); @@ -198,6 +202,8 @@ private: void setMapping(SX1278::DioMapping mapping) override; void setFreqRF(int freq_rf); + + bool crc_enabled; }; } // namespace Boardcore diff --git a/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp index 0772d57e40dd48b53ec4892625809ba690b3aaaf..4c2508ea283c97135dc1c91213eeb9a145a7830d 100644 --- a/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp +++ b/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp @@ -90,16 +90,16 @@ void initBoard() GpioPin dio3_pin = dio3::getPin(); enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(), - InterruptTrigger::RISING_EDGE); + InterruptTrigger::RISING_FALLING_EDGE); enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(), - InterruptTrigger::RISING_EDGE); + InterruptTrigger::RISING_FALLING_EDGE); enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(), - InterruptTrigger::RISING_EDGE); + InterruptTrigger::RISING_FALLING_EDGE); } void recvLoop() { - char buf[64]; + char buf[SX1278Fsk::MTU]; while (1) { ssize_t res = @@ -116,7 +116,7 @@ void recvLoop() void sendLoop(int interval, const char *data) { - char buf[64]; + char buf[SX1278Fsk::MTU]; strncpy(buf, data, sizeof(buf) - 1); while (1) @@ -157,8 +157,15 @@ int main() printf("\n[sx1278] Initialization complete!\n"); + const char *msg = + "Very very very very very very very very very very very " + "very very very very very very very very very very very " + "very very very very very very very very very very very " + "very very very very very very very very very very very " + "long message"; + // Spawn all threads - std::thread send([]() { sendLoop(1000, "Sample radio message"); }); + std::thread send([msg]() { sendLoop(3333, msg); }); std::thread recv([]() { recvLoop(); }); // sx1278->debugDumpRegisters(); diff --git a/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp index 095dc3076805fd4532ac3f73671ccafd06835dca..0d430f6952e32e4465453827f9ac6db892706914 100644 --- a/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp +++ b/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp @@ -41,7 +41,7 @@ using namespace Boardcore; using namespace miosix; -constexpr uint32_t RADIO_PKT_LENGTH = 63; +constexpr uint32_t RADIO_PKT_LENGTH = SX1278Fsk::MTU; constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 10; constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 200; @@ -101,11 +101,11 @@ void initBoard() GpioPin dio3_pin = dio3::getPin(); enableExternalInterrupt(dio0_pin.getPort(), dio0_pin.getNumber(), - InterruptTrigger::RISING_EDGE); + InterruptTrigger::RISING_FALLING_EDGE); enableExternalInterrupt(dio1_pin.getPort(), dio1_pin.getNumber(), - InterruptTrigger::RISING_EDGE); + InterruptTrigger::RISING_FALLING_EDGE); enableExternalInterrupt(dio3_pin.getPort(), dio3_pin.getNumber(), - InterruptTrigger::RISING_EDGE); + InterruptTrigger::RISING_FALLING_EDGE); } Mav* channel; diff --git a/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp b/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp index ec794656690fe720404fccb7317a8342d89286f3..0185e0167cd6e0093baf741c9e1f5c4b0ef2d580 100644 --- a/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp +++ b/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp @@ -157,7 +157,7 @@ void recvLoop() void sendLoop(int interval, const char *data) { - char buf[64]; + char buf[SX1278Lora::MTU]; strncpy(buf, data, sizeof(buf) - 1); while (1) @@ -176,8 +176,8 @@ int main() SX1278Lora::Config config = {}; config.power = 10; config.ocp = 0; - config.coding_rate = SX1278Lora::Config::Cr::CR_4; - config.spreading_factor = SX1278Lora::Config::Sf::SF_12; + config.coding_rate = SX1278Lora::Config::Cr::CR_2; + config.spreading_factor = SX1278Lora::Config::Sf::SF_8; SX1278Lora::Error err; @@ -203,8 +203,15 @@ int main() printf("\n[sx1278] Initialization complete!\n"); + const char *msg = + "Very very very very very very very very very very very " + "very very very very very very very very very very very " + "very very very very very very very very very very very " + "very very very very very very very very very very very " + "long message"; + // Spawn all threads - std::thread send([]() { sendLoop(3333, "Sample radio message"); }); + std::thread send([msg]() { sendLoop(3333, msg); }); std::thread recv([]() { recvLoop(); }); // sx1278->debugDumpRegisters(); diff --git a/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp b/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp index a35320666c5e340ec32787578df59f1ace39710b..bac40989bcbdf9c822dd2f61631024c4d54957c4 100644 --- a/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp +++ b/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp @@ -39,7 +39,7 @@ using namespace Boardcore; using namespace miosix; -constexpr uint32_t RADIO_PKT_LENGTH = 255; +constexpr uint32_t RADIO_PKT_LENGTH = SX1278Lora::MTU; constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 10; constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 0; diff --git a/src/tests/radio/sx1278/test-sx1278-bench.cpp b/src/tests/radio/sx1278/test-sx1278-bench.cpp index fd0607ad35230b67ee6b50a5091883a401a9baf7..67d02cecb25bae77506ce10aeb8832d7ae819249 100644 --- a/src/tests/radio/sx1278/test-sx1278-bench.cpp +++ b/src/tests/radio/sx1278/test-sx1278-bench.cpp @@ -47,7 +47,7 @@ uint32_t xorshift32() struct TestMsg { - static constexpr int WORD_COUNT = 14; + static constexpr int WORD_COUNT = 60; uint32_t payload[WORD_COUNT];