diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e5162364a0b16ffce590db067bfe8abc5401265..2ff5af146481747dfdf70125736764aba2ac10d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -195,6 +195,46 @@ sbs_target(test-stepper stm32f429zi_stm32f4discovery) add_executable(test-fsm src/tests/events/fsm/test-fsm.cpp) sbs_target(test-fsm stm32f429zi_stm32f4discovery) +#-----------------------------------------------------------------------------# +# Tests - Radio # +#-----------------------------------------------------------------------------# + +add_executable(test-sx1278-bench + src/tests/drivers/sx1278/test-sx1278-bench.cpp + src/tests/drivers/sx1278/test-sx1278-core.cpp +) +sbs_target(test-sx1278-bench stm32f407vg_stm32f4discovery) + +add_executable(test-sx1278-bidir + src/tests/drivers/sx1278/test-sx1278-bidir.cpp + src/tests/drivers/sx1278/test-sx1278-core.cpp +) +sbs_target(test-sx1278-bidir stm32f407vg_stm32f4discovery) + +add_executable(test-sx1278-tx + src/tests/drivers/sx1278/test-sx1278-tx.cpp + src/tests/drivers/sx1278/test-sx1278-core.cpp +) +sbs_target(test-sx1278-tx stm32f407vg_stm32f4discovery) + +add_executable(test-sx1278-rx + src/tests/drivers/sx1278/test-sx1278-rx.cpp + src/tests/drivers/sx1278/test-sx1278-core.cpp +) +sbs_target(test-sx1278-rx stm32f407vg_stm32f4discovery) + +add_executable(test-sx1278-mavlink + src/tests/drivers/sx1278/test-sx1278-mavlink.cpp + src/tests/drivers/sx1278/test-sx1278-core.cpp +) +sbs_target(test-sx1278-mavlink stm32f407vg_stm32f4discovery) + +add_executable(test-sx1278-serial + src/tests/drivers/sx1278/test-sx1278-serial.cpp + src/tests/drivers/sx1278/test-sx1278-core.cpp +) +sbs_target(test-sx1278-serial stm32f407vg_stm32f4discovery) + #-----------------------------------------------------------------------------# # Tests - Sensors # #-----------------------------------------------------------------------------# diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index 9d99de5a811713715c005a7a24a917ecdb1ff5ae..6a95bd1307e2117c08aedbfba84058e8ab203c81 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -43,6 +43,7 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/drivers/runcam/Runcam.cpp ${SBS_BASE}/src/shared/drivers/servo/Servo.cpp ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp + ${SBS_BASE}/src/shared/drivers/SX1278/SX1278.cpp # Events ${SBS_BASE}/src/shared/events/EventBroker.cpp diff --git a/src/shared/drivers/SX1278/SX1278.cpp b/src/shared/drivers/SX1278/SX1278.cpp new file mode 100644 index 0000000000000000000000000000000000000000..52a91f3ba06520aa4b18974468319a22256b6774 --- /dev/null +++ b/src/shared/drivers/SX1278/SX1278.cpp @@ -0,0 +1,476 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "SX1278.h" + +#include <kernel/scheduler/scheduler.h> + +#include <cmath> + +namespace Boardcore +{ + +using namespace SX1278Defs; + +// Default values for registers +constexpr uint8_t REG_OP_MODE_DEFAULT = RegOpMode::LONG_RANGE_MODE_FSK | + RegOpMode::MODULATION_TYPE_FSK | + RegOpMode::LOW_FREQUENCY_MODE_ON; + +constexpr uint8_t REG_SYNC_CONFIG_DEFAULT = + RegSyncConfig::AUTO_RESTART_RX_MODE_OFF | + RegSyncConfig::PREAMBLE_POLARITY_55 | RegSyncConfig::SYNC_ON; + +SX1278BusManager::SX1278BusManager(SPIBusInterface &bus, miosix::GpioPin cs) + : slave(bus, cs, spiConfig()), mode(SX1278BusManager::Mode::MODE_SLEEP) +{ +} + +void SX1278BusManager::lock(SX1278BusManager::Mode mode) +{ + mutex.lock(); + + enterMode(mode); + irq_wait_thread = nullptr; +} + +void SX1278BusManager::unlock() +{ + if (rx_wait_thread != nullptr) + { + // Restore rx state + irq_wait_thread = rx_wait_thread; + enterMode(SX1278BusManager::Mode::MODE_RX); + } + + mutex.unlock(); +} + +SPISlave &SX1278BusManager::getBus() { return slave; } + +void SX1278BusManager::handleDioIRQ() +{ + if (irq_wait_thread) + { + irq_wait_thread->IRQwakeup(); + if (irq_wait_thread->IRQgetPriority() > + miosix::Thread::IRQgetCurrentThread()->IRQgetPriority()) + { + miosix::Scheduler::IRQfindNextThread(); + } + + // Check if we woke the rx thread + if (irq_wait_thread == rx_wait_thread) + rx_wait_thread = nullptr; + + irq_wait_thread = nullptr; + } +} + +void SX1278BusManager::waitForIrq(uint16_t mask) +{ + // Tight loop on IRQ register + while (!(getIrqFlags() & mask)) + miosix::delayUs(10); +} + +void SX1278BusManager::waitForRxIrq() +{ + // NOTE: waitForRxIrq is only available for RX + + // Check before entering irq mode + if (getIrqFlags() & RegIrqFlags::PAYLOAD_READY) + return; + + miosix::FastInterruptDisableLock dLock; + irq_wait_thread = rx_wait_thread = miosix::Thread::getCurrentThread(); + // Release lock to allow for writers + mutex.unlock(); + + // Avoid spurious wakeups + while (rx_wait_thread != 0) + { + rx_wait_thread->IRQwait(); + { + miosix::FastInterruptEnableLock eLock(dLock); + miosix::Thread::yield(); + } + } + + // Regain ownership of the lock + mutex.lock(); +} + +void SX1278BusManager::enterMode(Mode mode) +{ + // Check if necessary + if (mode == this->mode) + return; + + setMode(mode); + + // BUG: Removing the next line makes it, better?!? + // waitForIrq(RegIrqFlags::MODE_READY); + + this->mode = mode; +} + +uint16_t SX1278BusManager::getIrqFlags() +{ + SPITransaction spi(getBus(), SPITransaction::WriteBit::INVERTED); + + uint8_t flags[2] = {0, 0}; + spi.readRegisters(REG_IRQ_FLAGS_1, flags, 2); + + return (flags[0] | flags[1] << 8); +} + +void SX1278BusManager::setMode(Mode mode) +{ + SPITransaction spi(getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_OP_MODE, REG_OP_MODE_DEFAULT | mode); +} + +SX1278::SX1278(SPIBusInterface &bus, miosix::GpioPin cs) : bus_mgr(bus, cs) {} + +SX1278::Error SX1278::init(Config config) +{ + // Lock the bus + bus_mgr.lock(Mode::MODE_STDBY); + bus_mgr.waitForIrq(RegIrqFlags::MODE_READY); + + if (getVersion() != 0x12) + { + return Error::BAD_VERSION; + } + + setBitrate(config.bitrate); + setFreqDev(config.freq_dev); + setFreqRF(config.freq_rf); + + setRxBw(config.rx_bw); + setAfcBw(config.afc_bw); + + setOcp(config.ocp); + + uint8_t sync_word[2] = {0x12, 0xad}; + setSyncWord(sync_word, 2); + setPreableLen(2); + setPa(config.power, true); + + // Setup generic parameters + { + SPITransaction spi(bus_mgr.getBus(), + SPITransaction::WriteBit::INVERTED); + + spi.writeRegister(REG_PA_RAMP, + RegPaRamp::MODULATION_SHAPING_NONE | 0x09); + spi.writeRegister(REG_RX_CONFIG, + RegRxConfig::AFC_AUTO_ON | RegRxConfig::AGC_AUTO_ON | + RegRxConfig::RX_TRIGGER_PREAMBLE_DETECT | + RegRxConfig::RX_TRIGGER_RSSI_INTERRUPT); + spi.writeRegister(REG_RSSI_THRESH, 0xff); + spi.writeRegister( + REG_PREAMBLE_DETECT, + RegPreambleDetector::PREAMBLE_DETECTOR_ON | + RegPreambleDetector::PREAMBLE_DETECTOR_SIZE_2_BYTES | 0x0a); + spi.writeRegister(REG_RX_TIMEOUT_1, 0x00); + spi.writeRegister(REG_RX_TIMEOUT_2, 0x00); + spi.writeRegister(REG_RX_TIMEOUT_3, 0x00); + spi.writeRegister(REG_PACKET_CONFIG_1, + RegPacketConfig1::PACKET_FORMAT_VARIABLE_LENGTH | + RegPacketConfig1::DC_FREE_NONE | + RegPacketConfig1::CRC_ON | + RegPacketConfig1::ADDRESS_FILTERING_NONE | + RegPacketConfig1::CRC_WHITENING_TYPE_CCITT_CRC); + spi.writeRegister(REG_PACKET_CONFIG_2, + RegPacketConfig2::DATA_MODE_PACKET); + spi.writeRegister( + REG_FIFO_THRESH, + RegFifoThresh::TX_START_CONDITION_FIFO_NOT_EMPTY | 0x0f); + spi.writeRegister(REG_NODE_ADRS, 0x00); + spi.writeRegister(REG_BROADCAST_ADRS, 0x00); + + // Enable PayloadReady, PacketSent on DIO0 + spi.writeRegister(REG_DIO_MAPPING_1, 0x00); + } + + bus_mgr.unlock(); + return Error::NONE; +} + +ssize_t SX1278::receive(uint8_t *pkt, size_t max_len) +{ + bus_mgr.lock(Mode::MODE_RX); + + uint8_t len = 0; + do + { + // Special wait for payload ready + bus_mgr.waitForRxIrq(); + + { + SPITransaction spi(bus_mgr.getBus(), + SPITransaction::WriteBit::INVERTED); + len = spi.readRegister(REG_FIFO); + if (len > max_len) + return -1; + + spi.readRegisters(REG_FIFO, pkt, len); + } + + // For some reason this sometimes happen? + } while (len == 0); + + bus_mgr.unlock(); + return len; +} + +bool SX1278::send(uint8_t *pkt, size_t len) +{ + // Packets longer than 255 are not supported + if (len > 255) + return false; + + bus_mgr.lock(SX1278BusManager::Mode::MODE_TX); + + // Wait for TX ready + bus_mgr.waitForIrq(RegIrqFlags::TX_READY); + + { + SPITransaction spi(bus_mgr.getBus(), + SPITransaction::WriteBit::INVERTED); + + spi.writeRegister(REG_FIFO, static_cast<uint8_t>(len)); + spi.writeRegisters(REG_FIFO, pkt, len); + } + + // Wait for packet sent + bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT); + bus_mgr.unlock(); + return true; +} + +void SX1278::handleDioIRQ() { bus_mgr.handleDioIRQ(); } + +uint8_t SX1278::getVersion() +{ + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + + return spi.readRegister(REG_VERSION); +} + +void SX1278::setBitrate(int bitrate) +{ + uint16_t val = SX1278Defs::FXOSC / bitrate; + + // Update values + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_BITRATE_MSB, val >> 8); + spi.writeRegister(REG_BITRATE_LSB, val); +} + +void SX1278::setFreqDev(int freq_dev) +{ + uint16_t val = freq_dev / FSTEP; + + // Update values + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_FDEV_MSB, (val >> 8) & 0x3f); + spi.writeRegister(REG_FDEV_LSB, val); +} + +void SX1278::setFreqRF(int freq_rf) +{ + uint32_t val = freq_rf / FSTEP; + + // Update values + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_FRF_MSB, val >> 16); + spi.writeRegister(REG_FRF_MID, val >> 8); + spi.writeRegister(REG_FRF_LSB, val); +} + +void SX1278::setOcp(int ocp) +{ + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + if (ocp == 0) + { + spi.writeRegister(REG_OCP, 0); + } + else if (ocp <= 120) + { + uint8_t raw = (ocp - 45) / 5; + spi.writeRegister(REG_OCP, RegOcp::OCP_ON | raw); + } + else + { + uint8_t raw = (ocp + 30) / 10; + spi.writeRegister(REG_OCP, RegOcp::OCP_ON | raw); + } +} + +void SX1278::setSyncWord(uint8_t value[], int size) +{ + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_SYNC_CONFIG, REG_SYNC_CONFIG_DEFAULT | size); + + for (int i = 0; i < size; i++) + { + spi.writeRegister(REG_SYNC_VALUE_1 + i, value[i]); + } +} + +void SX1278::setRxBw(RxBw rx_bw) +{ + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_RX_BW, static_cast<uint8_t>(rx_bw)); +} + +void SX1278::setAfcBw(RxBw afc_bw) +{ + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_AFC_BW, static_cast<uint8_t>(afc_bw)); +} + +void SX1278::setPreableLen(int len) +{ + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + spi.writeRegister(REG_PREAMBLE_MSB, len >> 8); + spi.writeRegister(REG_PREAMBLE_LSB, len); +} + +void SX1278::setPa(int power, bool pa_boost) +{ + // [2, 17] or 20 if PA_BOOST + // [0, 15] if !PA_BOOST + + const uint8_t MAX_POWER = 0b111; + + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + + if (!pa_boost) + { + // Don't use power amplifier boost + power = power - MAX_POWER + 15; + spi.writeRegister(REG_PA_CONFIG, MAX_POWER << 4 | power); + spi.writeRegister(REG_PA_DAC, + RegPaDac::PA_DAC_DEFAULT_VALUE | 0x10 << 3); + } + else if (power != 20) + { + // Run power amplifier boost but not at full power + power = power - 2; + spi.writeRegister(REG_PA_CONFIG, MAX_POWER << 4 | power | + RegPaConfig::PA_SELECT_BOOST); + spi.writeRegister(REG_PA_DAC, RegPaDac::PA_DAC_PA_BOOST | 0x10 << 3); + } + else + { + // Run power amplifier boost at full power + power = 15; + spi.writeRegister(REG_PA_CONFIG, MAX_POWER << 4 | power | + RegPaConfig::PA_SELECT_BOOST); + spi.writeRegister(REG_PA_DAC, RegPaDac::PA_DAC_PA_BOOST | 0x10 << 3); + } +} + +void SX1278::debugDumpRegisters() +{ + struct RegDef + { + const char *name; + int addr; + }; + + const RegDef defs[] = { + RegDef{"REG_OP_MODE", REG_OP_MODE}, + RegDef{"REG_BITRATE_MSB", REG_BITRATE_MSB}, + RegDef{"REG_BITRATE_LSB", REG_BITRATE_LSB}, + RegDef{"REG_FDEV_MSB", REG_FDEV_MSB}, + RegDef{"REG_FDEV_LSB", REG_FDEV_LSB}, + RegDef{"REG_FRF_MSB", REG_FRF_MSB}, RegDef{"REG_FRF_MID", REG_FRF_MID}, + RegDef{"REG_FRF_LSB", REG_FRF_LSB}, + RegDef{"REG_PA_CONFIG", REG_PA_CONFIG}, + RegDef{"REG_PA_RAMP", REG_PA_RAMP}, RegDef{"REG_OCP", REG_OCP}, + RegDef{"REG_LNA", REG_LNA}, RegDef{"REG_RX_CONFIG", REG_RX_CONFIG}, + RegDef{"REG_RSSI_CONFIG", REG_RSSI_CONFIG}, + RegDef{"REG_RSSI_COLLISION", REG_RSSI_COLLISION}, + RegDef{"REG_RSSI_THRESH", REG_RSSI_THRESH}, + // RegDef { "REG_RSSI_VALUE", REG_RSSI_VALUE }, + RegDef{"REG_RX_BW", REG_RX_BW}, RegDef{"REG_AFC_BW", REG_AFC_BW}, + RegDef{"REG_OOK_PEAK", REG_OOK_PEAK}, + RegDef{"REG_OOK_FIX", REG_OOK_FIX}, RegDef{"REG_OOK_AVG", REG_OOK_AVG}, + RegDef{"REG_AFC_FEI", REG_AFC_FEI}, RegDef{"REG_AFC_MSB", REG_AFC_MSB}, + RegDef{"REG_AFC_LSB", REG_AFC_LSB}, RegDef{"REG_FEI_MSB", REG_FEI_MSB}, + RegDef{"REG_FEI_LSB", REG_FEI_LSB}, + RegDef{"REG_PREAMBLE_DETECT", REG_PREAMBLE_DETECT}, + RegDef{"REG_RX_TIMEOUT_1", REG_RX_TIMEOUT_1}, + RegDef{"REG_RX_TIMEOUT_2", REG_RX_TIMEOUT_2}, + RegDef{"REG_RX_TIMEOUT_3", REG_RX_TIMEOUT_3}, + RegDef{"REG_RX_DELAY", REG_RX_DELAY}, RegDef{"REG_OSC", REG_OSC}, + RegDef{"REG_PREAMBLE_MSB", REG_PREAMBLE_MSB}, + RegDef{"REG_PREAMBLE_LSB", REG_PREAMBLE_LSB}, + RegDef{"REG_SYNC_CONFIG", REG_SYNC_CONFIG}, + RegDef{"REG_SYNC_VALUE_1", REG_SYNC_VALUE_1}, + RegDef{"REG_SYNC_VALUE_2", REG_SYNC_VALUE_2}, + RegDef{"REG_SYNC_VALUE_3", REG_SYNC_VALUE_3}, + RegDef{"REG_SYNC_VALUE_4", REG_SYNC_VALUE_4}, + RegDef{"REG_SYNC_VALUE_5", REG_SYNC_VALUE_5}, + RegDef{"REG_SYNC_VALUE_6", REG_SYNC_VALUE_6}, + RegDef{"REG_SYNC_VALUE_7", REG_SYNC_VALUE_7}, + RegDef{"REG_SYNC_VALUE_8", REG_SYNC_VALUE_8}, + RegDef{"REG_PACKET_CONFIG_1", REG_PACKET_CONFIG_1}, + RegDef{"REG_PACKET_CONFIG_2", REG_PACKET_CONFIG_2}, + RegDef{"REG_PACKET_PAYLOAD_LENGTH", REG_PACKET_PAYLOAD_LENGTH}, + RegDef{"REG_NODE_ADRS", REG_NODE_ADRS}, + RegDef{"REG_BROADCAST_ADRS", REG_BROADCAST_ADRS}, + RegDef{"REG_FIFO_THRESH", REG_FIFO_THRESH}, + RegDef{"REG_SEQ_CONFIG_1", REG_SEQ_CONFIG_1}, + RegDef{"REG_SEQ_CONFIG_2", REG_SEQ_CONFIG_2}, + RegDef{"REG_TIMER_RESOL", REG_TIMER_RESOL}, + RegDef{"REG_TIMER_1_COEF", REG_TIMER_1_COEF}, + RegDef{"REG_TIMER_2_COEF", REG_TIMER_2_COEF}, + RegDef{"REG_IMAGE_CAL", REG_IMAGE_CAL}, + // RegDef { "REG_TEMP", REG_TEMP }, + RegDef{"REG_LOW_BAT", REG_LOW_BAT}, + // RegDef { "REG_IRQ_FLAGS_1", REG_IRQ_FLAGS_1 }, + // RegDef { "REG_IRQ_FLAGS_2", REG_IRQ_FLAGS_2 }, + RegDef{"REG_DIO_MAPPING_1", REG_DIO_MAPPING_1}, + RegDef{"REG_DIO_MAPPING_2", REG_DIO_MAPPING_2}, + RegDef{"REG_VERSION", REG_VERSION}, RegDef{NULL, 0}}; + + SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); + + int i = 0; + while (defs[i].name) + { + auto name = defs[i].name; + auto addr = defs[i].addr; + + LOG_DEBUG(logger, "%s: 0x%x\n", name, spi.readRegister(addr)); + + i++; + } +} + +} // namespace Boardcore diff --git a/src/shared/drivers/SX1278/SX1278.h b/src/shared/drivers/SX1278/SX1278.h new file mode 100644 index 0000000000000000000000000000000000000000..6fb4c355d36f81331e7299e6a291e58c737e4156 --- /dev/null +++ b/src/shared/drivers/SX1278/SX1278.h @@ -0,0 +1,247 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <diagnostic/PrintLogger.h> +#include <drivers/spi/SPIDriver.h> +#include <kernel/kernel.h> +#include <radio/Transceiver.h> + +#include <cstdint> + +#include "SX1278Defs.h" + +/* +# Description +## Concurrent access +The device is stateful, in order to send/receive data you need to set in a +particular state. This is handled by SX1278BusManager::setMode. This causes a +lot of issues with multithreaded code, and receiving/transmitting concurrently. + +So the driver wraps the bus in a BusManager for this reason, in order to perform +anything you need to lock the bus. The driver then locks the internal mutex and +sets the device in the correct mode for that operation. + +But this causes problems when receiving, as receive can block indefinetly, and +would prevent any thread from actually sending stuff. The solution is to release +the lock only when waiting for a packet. This allows other threads to lock it +and change mode, and unlocking the bus resets its state back to RX so it can +continue to listen for incoming packets. + +The BusManager also multiplexes who is currently waiting for an interrupt. This +is needed because due to the limited interrupts pins, the device signals +multiples of them on one pin. And for some random reasons, the designers decided +to put "packet received" and "packed sent" on same pin (DIO0)... + +Again, the solution is to track who is actually using the bus, so when sending a +packet that triggers the "packet sent" interrupt, the manager knows to dispatch +it to the currect thread, and not wake up the RX thread. +*/ + +namespace Boardcore +{ + +/** + * @brief SX1278 bus access manager. + */ +class SX1278BusManager +{ +public: + using Mode = SX1278Defs::RegOpMode::Mode; + + SX1278BusManager(SPIBusInterface &bus, miosix::GpioPin cs); + + /** + * @brief Lock bus for exclusive access. + * + * @param mode Device mode requested + */ + void lock(Mode mode); + + /** + * @brief Release buf for exclusive access + */ + void unlock(); + + /** + * @brief Get underlying bus. + */ + SPISlave &getBus(); + + /** + * @brief Handle DIO0 irq. + */ + void handleDioIRQ(); + + /** + * @brief Wait for generic irq (DOES NOT RELEASE LOCK!). + */ + void waitForIrq(uint16_t mask); + + /** + * @brief Wait for RX irq (releases lock safely). + */ + void waitForRxIrq(); + +private: + void enterMode(Mode mode); + + uint16_t getIrqFlags(); + void setMode(Mode mode); + + miosix::Thread *irq_wait_thread = nullptr; + miosix::Thread *rx_wait_thread = nullptr; + miosix::FastMutex mutex; + + SPISlave slave; + Mode mode; +}; + +/** + * @brief Various SX1278 register/enums definitions. + */ +class SX1278 : public Transceiver +{ +public: + /** + * @brief Channel filter bandwidth. + */ + enum class RxBw + { + HZ_2600 = (0b10 << 3) | 7, + HZ_3100 = (0b01 << 3) | 7, + HZ_3900 = (0b00 << 3) | 7, + HZ_5200 = (0b10 << 3) | 6, + HZ_6300 = (0b01 << 3) | 6, + HZ_7800 = (0b00 << 3) | 6, + HZ_10400 = (0b10 << 3) | 5, + HZ_12500 = (0b01 << 3) | 5, + HZ_15600 = (0b00 << 3) | 5, + HZ_20800 = (0b10 << 3) | 4, + HZ_25000 = (0b01 << 3) | 4, + HZ_31300 = (0b00 << 3) | 4, + HZ_41700 = (0b10 << 3) | 3, + HZ_50000 = (0b01 << 3) | 3, + HZ_62500 = (0b00 << 3) | 3, + HZ_83300 = (0b10 << 3) | 2, + HZ_100000 = (0b01 << 3) | 2, + HZ_125000 = (0b00 << 3) | 2, + HZ_166700 = (0b10 << 3) | 1, + HZ_200000 = (0b01 << 3) | 1, + HZ_250000 = (0b00 << 3) | 1, + }; + + /** + * @brief Requested SX1278 configuration. + */ + struct Config + { + int freq_rf = 434000000; //< RF Frequency in Hz. + int freq_dev = 50000; //< Frequency deviation in Hz. + int bitrate = 48000; //< Bitrate in b/s. + RxBw rx_bw = RxBw::HZ_125000; //< Rx filter bandwidth. + RxBw afc_bw = RxBw::HZ_125000; //< Afc filter bandwidth. + int ocp = + 120; //< Over current protection limit in mA (0 for no limit). + int power = 10; //< Output power in dB (Between +2 and +17, or +20 for + // full power). + }; + + /** + * @brief Error enum. + */ + enum class Error + { + NONE, //< No error encountered. + BAD_VALUE, //< A requested value was outside the valid range. + BAD_VERSION, //< Chip didn't report the correct version. + }; + + /** + * @brief Construct a new SX1278 + * + * @param bus SPI bus used. + * @param cs Chip select pin. + */ + SX1278(SPIBusInterface &bus, miosix::GpioPin cs); + + /** + * @brief Setup the device. + */ + Error init(Config config); + + /** + * @brief Wait until a new packet is received. + * + * @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 + */ + ssize_t receive(uint8_t *pkt, size_t max_len); + + /** + * @brief Send a packet. + * The function must block until the packet is sent (successfully or not) + * + * @param pkt Pointer to the packet (needs to be at least pkt_len + * bytes). + * @param pkt_len Lenght of the packet to be sent. + * @return True if the message was sent correctly. + */ + bool send(uint8_t *pkt, size_t len); + + /** + * @brief Return device version. + */ + uint8_t getVersion(); + + /** + * @brief Handle an incoming interrupt. + */ + void handleDioIRQ(); + + /** + * @brief Dump all registers via TRACE. + */ + void debugDumpRegisters(); + +public: + using Mode = SX1278BusManager::Mode; + + void setBitrate(int bitrate); + void setFreqDev(int freq_dev); + void setFreqRF(int freq_rf); + void setOcp(int ocp); + void setSyncWord(uint8_t value[], int size); + void setRxBw(RxBw rx_bw); + void setAfcBw(RxBw afc_bw); + void setPreableLen(int len); + void setPa(int power, bool pa_boost); + + void waitForIrq(uint16_t mask); + + SX1278BusManager bus_mgr; + PrintLogger logger = Logging::getLogger("sx1278"); +}; + +} // namespace Boardcore diff --git a/src/shared/drivers/SX1278/SX1278Defs.h b/src/shared/drivers/SX1278/SX1278Defs.h new file mode 100644 index 0000000000000000000000000000000000000000..cd50d09a8c3294659f1c1186f2ad1fd0512937c6 --- /dev/null +++ b/src/shared/drivers/SX1278/SX1278Defs.h @@ -0,0 +1,356 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/spi/SPIDriver.h> + +namespace Boardcore +{ + +/** + * @brief Various SX1278 register/enums definitions. + */ +namespace SX1278Defs +{ + +/** + * @brief Main oscillator frequency (Hz) + */ +constexpr int FXOSC = 32000000; + +/** + * @brief Frequency step (Hz) used in some calculations. + */ +constexpr int FSTEP = 61; + +constexpr int TS_OSC = 250; +constexpr int TS_FS = 60; + +/** + * @brief Get required spi config + */ +inline SPIBusConfig spiConfig() +{ + SPIBusConfig config = {}; + + // FIXME(davide.mor): This depends on the device + config.clockDivider = SPI::ClockDivider::DIV_64; + config.mode = SPI::Mode::MODE_1; + config.bitOrder = SPI::BitOrder::MSB_FIRST; + // config.cs_setup_time_us = 30; + // config.cs_hold_time_us = 100; + + return config; +} + +namespace RegOpMode +{ + +enum LongRangeMode +{ + LONG_RANGE_MODE_FSK = 0 << 7, + LONG_RANGE_MODE_LORA = 1 << 7 +}; + +enum ModulationType +{ + MODULATION_TYPE_FSK = 0 << 6, + MODULATION_TYPE_OOK = 1 << 6, +}; + +constexpr uint8_t LOW_FREQUENCY_MODE_ON = 1 << 3; + +enum Mode +{ + MODE_SLEEP = 0b000, + MODE_STDBY = 0b001, + MODE_FSTX = 0b010, + MODE_TX = 0b011, + MODE_FSRX = 0b100, + MODE_RX = 0b101, +}; +} // namespace RegOpMode + +namespace RegPaConfig +{ +constexpr uint8_t PA_SELECT_BOOST = 1 << 7; +} + +namespace RegPaRamp +{ +enum ModulationShaping +{ + MODULATION_SHAPING_NONE = 0b00 << 5, + MODULATION_SHAPING_GAUSSIAN_BT_1_0 = 0b01 << 5, + MODULATION_SHAPING_GAUSSIAN_BT_0_5 = 0b10 << 5, + MODULATION_SHAPING_GAUSSIAN_BT_0_3 = 0b11 << 5, +}; +} + +namespace RegOcp +{ +constexpr uint8_t OCP_ON = 1 << 5; +} + +namespace RegRxConfig +{ +constexpr uint8_t RESTART_RX_ON_COLLISION = 1 << 7; +constexpr uint8_t RESTART_RX_WITHOUT_PILL_LOCK = 1 << 6; +constexpr uint8_t RESTART_RX_WITH_PILL_LOCK = 1 << 5; +constexpr uint8_t AFC_AUTO_ON = 1 << 4; +constexpr uint8_t AGC_AUTO_ON = 1 << 3; + +constexpr uint8_t RX_TRIGGER_RSSI_INTERRUPT = 0b001; +constexpr uint8_t RX_TRIGGER_PREAMBLE_DETECT = 0b110; +} // namespace RegRxConfig + +namespace RegPreambleDetector +{ +constexpr uint8_t PREAMBLE_DETECTOR_ON = 1 << 7; + +enum PreambleDetectorSize +{ + PREAMBLE_DETECTOR_SIZE_1_BYTE = 0b00 << 5, + PREAMBLE_DETECTOR_SIZE_2_BYTES = 0b01 << 5, + PREAMBLE_DETECTOR_SIZE_3_BYTES = 0b10 << 5, +}; +} // namespace RegPreambleDetector + +namespace RegSyncConfig +{ +enum AutoRestartRxMode +{ + AUTO_RESTART_RX_MODE_OFF = 0b00 << 6, + AUTO_RESTART_RX_MODE_ON_WITHOUT_PILL_LOCK = 0b01 << 6, + AUTO_RESTART_RX_MODE_ON_WITH_PILL_LOCK = 0b10 << 6, +}; + +enum PreamblePolarity +{ + PREAMBLE_POLARITY_AA = 0 << 5, + PREAMBLE_POLARITY_55 = 1 << 5, +}; + +constexpr uint8_t SYNC_ON = 1 << 4; +} // namespace RegSyncConfig + +namespace RegPacketConfig1 +{ +enum PacketFormat +{ + PACKET_FORMAT_FIXED_LENGTH = 0 << 7, + PACKET_FORMAT_VARIABLE_LENGTH = 1 << 7, +}; + +enum DcFree +{ + DC_FREE_NONE = 0b00 << 5, + DC_FREE_MANCHESTER = 0b01 << 5, + DC_FREE_WHITENING = 0b10 << 5, +}; + +constexpr uint8_t CRC_ON = 1 << 4; +constexpr uint8_t CRC_AUTO_CLEAR_OFF = 1 << 3; + +enum AddressFiltering +{ + ADDRESS_FILTERING_NONE = 0b00 << 1, + ADDRESS_FILTERING_MATCH_NODE = 0b01 << 1, + ADDRESS_FILTERING_MATCH_NODE_OR_BROADCAST = 0b10 << 1, +}; + +enum CrcWhiteningType +{ + CRC_WHITENING_TYPE_CCITT_CRC = 0, + CRC_WHITENING_TYPE_IBM_CRC = 1, +}; +} // namespace RegPacketConfig1 + +namespace RegPacketConfig2 +{ +enum DataMode +{ + DATA_MODE_CONTINUOS = 0 << 6, + DATA_MODE_PACKET = 1 << 6 +}; + +constexpr uint8_t IO_HOME_ON = 1 << 5; +constexpr uint8_t BEACON_ON = 1 << 4; +} // namespace RegPacketConfig2 + +namespace RegFifoThresh +{ +enum TxStartCondition +{ + TX_START_CONDITION_FIFO_LEVEL = 0 << 7, + TX_START_CONDITION_FIFO_NOT_EMPTY = 1 << 7, +}; +} + +namespace RegIrqFlags +{ +constexpr uint16_t MODE_READY = 1 << 7; +constexpr uint16_t RX_READY = 1 << 6; +constexpr uint16_t TX_READY = 1 << 5; +constexpr uint16_t PILL_LOCK = 1 << 4; +constexpr uint16_t RSSI = 1 << 3; +constexpr uint16_t TIMEOUT = 1 << 2; +constexpr uint16_t PREAMBLE_DETECT = 1 << 1; +constexpr uint16_t SYNC_ADDRESS_MATCH = 1 << 0; +constexpr uint16_t FIFO_FULL = 1 << 15; +constexpr uint16_t FIFO_EMPTY = 1 << 14; +constexpr uint16_t FIFO_LEVEL = 1 << 13; +constexpr uint16_t FIFO_OVERRUN = 1 << 12; +constexpr uint16_t PACKET_SENT = 1 << 11; +constexpr uint16_t PAYLOAD_READY = 1 << 10; +constexpr uint16_t CRC_OK = 1 << 9; +constexpr uint16_t LOW_BAT = 1 << 8; + +} // namespace RegIrqFlags + +namespace RegPaDac +{ +enum PaDac +{ + PA_DAC_DEFAULT_VALUE = 0x04, + PA_DAC_PA_BOOST = 0x07 +}; +} + +enum Registers +{ + REG_FIFO = 0x00, + + // Registers for common settings + REG_OP_MODE = 0x01, + REG_BITRATE_MSB = 0x02, + REG_BITRATE_LSB = 0x03, + REG_FDEV_MSB = 0x04, + REG_FDEV_LSB = 0x05, + REG_FRF_MSB = 0x06, + REG_FRF_MID = 0x07, + REG_FRF_LSB = 0x08, + + // Registers for the transmitter + REG_PA_CONFIG = 0x09, + REG_PA_RAMP = 0x0a, + REG_OCP = 0x0b, + + // Registers for the receiver + REG_LNA = 0x0c, + REG_RX_CONFIG = 0x0d, + REG_RSSI_CONFIG = 0x0e, + REG_RSSI_COLLISION = 0x0f, + REG_RSSI_THRESH = 0x10, + REG_RSSI_VALUE = 0x11, + REG_RX_BW = 0x12, + REG_AFC_BW = 0x13, + REG_OOK_PEAK = 0x14, + REG_OOK_FIX = 0x15, + REG_OOK_AVG = 0x16, + // Reserved 17 to 19 + REG_AFC_FEI = 0x1a, + REG_AFC_MSB = 0x1b, + REG_AFC_LSB = 0x1c, + REG_FEI_MSB = 0x1d, + REG_FEI_LSB = 0x1e, + REG_PREAMBLE_DETECT = 0x1f, + REG_RX_TIMEOUT_1 = 0x20, + REG_RX_TIMEOUT_2 = 0x21, + REG_RX_TIMEOUT_3 = 0x22, + REG_RX_DELAY = 0x23, + + // RC Oscillator registers + REG_OSC = 0x24, + + // Packet handling registers + REG_PREAMBLE_MSB = 0x25, + REG_PREAMBLE_LSB = 0x26, + REG_SYNC_CONFIG = 0x27, + REG_SYNC_VALUE_1 = 0x28, + REG_SYNC_VALUE_2 = 0x29, + REG_SYNC_VALUE_3 = 0x2a, + REG_SYNC_VALUE_4 = 0x2b, + REG_SYNC_VALUE_5 = 0x2c, + REG_SYNC_VALUE_6 = 0x2d, + REG_SYNC_VALUE_7 = 0x2e, + REG_SYNC_VALUE_8 = 0x2f, + REG_PACKET_CONFIG_1 = 0x30, + REG_PACKET_CONFIG_2 = 0x31, + REG_PACKET_PAYLOAD_LENGTH = 0x32, + REG_NODE_ADRS = 0x33, + REG_BROADCAST_ADRS = 0x34, + REG_FIFO_THRESH = 0x35, + + // Sequencer registers + REG_SEQ_CONFIG_1 = 0x36, + REG_SEQ_CONFIG_2 = 0x37, + REG_TIMER_RESOL = 0x38, + REG_TIMER_1_COEF = 0x39, + REG_TIMER_2_COEF = 0x3a, + + // Service registers + REG_IMAGE_CAL = 0x3b, + REG_TEMP = 0x3c, + REG_LOW_BAT = 0x3d, + + // Status registers + REG_IRQ_FLAGS_1 = 0x3e, + REG_IRQ_FLAGS_2 = 0x3f, + + // IO Control registers + REG_DIO_MAPPING_1 = 0x40, + REG_DIO_MAPPING_2 = 0x41, + + // Version register + REG_VERSION = 0x42, + + // Additional registers + REG_PILL_HOP = 0x44, + REG_TCXO = 0x4b, + REG_PA_DAC = 0x4d, + REG_FORMER_TEMP = 0x5b, + REG_BITRATE_FRAC = 0x5d, + REG_AGC_REF = 0x61, + REG_AGC_THRESH_1 = 0x62, + REG_AGC_THRESH_2 = 0x63, + REG_AGC_THRESH_3 = 0x64, + + // Low frequency additional registers + REG_AGC_REF_LF = 0x61, + REG_AGC_THRESH_1_LF = 0x62, + REG_AGC_THRESH_2_LF = 0x63, + REG_AGC_THRESH_3_LF = 0x61, + REG_AGC_PILL_LF = 0x70, + + // High frequency additional registers + REG_AGC_REF_HF = 0x61, + REG_AGC_THRESH_1_HF = 0x62, + REG_AGC_THRESH_2_HF = 0x63, + REG_AGC_THRESH_3_HF = 0x61, + REG_AGC_PILL_HF = 0x70, +}; + +} // namespace SX1278Defs + +} // namespace Boardcore diff --git a/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h b/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h deleted file mode 100644 index ff3e1c83d3499f443de147b79996735208713988..0000000000000000000000000000000000000000 --- a/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h +++ /dev/null @@ -1,120 +0,0 @@ -/* Copyright (c) 2021 Skyward Experimental Rocketry - * Author: Damiano Amatruda - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <algorithm> - -namespace Boardcore -{ - -static constexpr uint16_t UBX_MAX_PAYLOAD_LENGTH = 92; -static constexpr uint16_t UBX_MAX_FRAME_LENGTH = UBX_MAX_PAYLOAD_LENGTH + 8; -static constexpr uint8_t UBX_VALID_PREAMBLE[] = {0xb5, 0x62}; - -struct UBXUnpackedFrame -{ - uint8_t preamble[2]; - uint8_t cls; - uint8_t id; - uint8_t payload[UBX_MAX_PAYLOAD_LENGTH]; - uint16_t payloadLength; - uint8_t checksum[2]; - - UBXUnpackedFrame() = default; - - UBXUnpackedFrame(uint8_t cls, uint8_t id, const uint8_t* payload, - uint16_t payloadLength) - : cls(cls), id(id), payloadLength(payloadLength) - { - memcpy(preamble, UBX_VALID_PREAMBLE, 2); - - memcpy(this->payload, payload, - std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH)); - - calcChecksum(checksum); - } - - inline uint16_t getFrameLength() const { return payloadLength + 8; } - - inline bool isValid() const - { - if (memcmp(preamble, UBX_VALID_PREAMBLE, 2) != 0) - { - return false; - } - - if (payloadLength > UBX_MAX_PAYLOAD_LENGTH) - { - return false; - } - - uint8_t validChecksum[2]; - calcChecksum(validChecksum); - return memcmp(checksum, validChecksum, 2) == 0; - } - - inline void writePacked(uint8_t* frame) const - { - memcpy(frame, preamble, 2); - frame[2] = cls; - frame[3] = id; - memcpy(&frame[4], &payloadLength, 2); - memcpy(&frame[6], payload, payloadLength); - memcpy(&frame[6 + payloadLength], checksum, 2); - } - - inline void readPacked(const uint8_t* frame) - { - memcpy(preamble, frame, 2); - cls = frame[2]; - id = frame[3]; - memcpy(&payloadLength, &frame[4], 2); - memcpy(payload, &frame[6], - std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH)); - memcpy(checksum, &frame[6 + payloadLength], 2); - } - - inline void calcChecksum(uint8_t* checksum) const - { - uint8_t data[UBX_MAX_FRAME_LENGTH]; - data[0] = cls; - data[1] = id; - memcpy(&data[2], &payloadLength, 2); - memcpy(&data[4], payload, - std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH)); - - uint16_t dataLength = - std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH) + 4; - - checksum[0] = 0; - checksum[1] = 0; - - for (uint8_t i = 0; i < dataLength; ++i) - { - checksum[0] += data[i]; - checksum[1] += checksum[0]; - } - } -}; - -} // namespace Boardcore diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp index 3bc0879ad7138d4a49b3c5860357a14f431eb3ed..dff24046dfefeca63f88c4499f1d0b2bf4c9f9ce 100644 --- a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp +++ b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda + * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -28,27 +28,39 @@ #include <fcntl.h> #include <filesystem/file_access.h> +using namespace miosix; + namespace Boardcore { -using namespace miosix; +UbloxGPS::UbloxGPS(int baudrate_, uint8_t sampleRate_, int serialPortNum_, + const char* serialPortName_, int defaultBaudrate_) + : baudrate(baudrate_), sampleRate(sampleRate_), + serialPortNumber(serialPortNum_), serialPortName(serialPortName_), + defaultBaudrate(defaultBaudrate_) +{ + // Prepare the gps file path with the specified name + strcpy(gpsFilePath, "/dev/"); + strcat(gpsFilePath, serialPortName); +} bool UbloxGPS::init() { // Change the baud rate from the default value - if (!setupCommunication()) + if (!serialCommuinicationSetup()) { return false; } + Thread::sleep(10); + // Reset configuration to default + // TODO: maybe move this on serial communication setup if (!resetConfiguration()) { return false; } - Thread::sleep(100); - // Disable NMEA messages if (!disableNMEAMessages()) { @@ -73,8 +85,8 @@ bool UbloxGPS::init() Thread::sleep(100); - // Set sample rate - if (!setSampleRate()) + // Set rate + if (!setRate()) { return false; } @@ -86,57 +98,200 @@ bool UbloxGPS::selfTest() { return true; } UbloxGPSData UbloxGPS::sampleImpl() { - Lock<FastMutex> l(sampleMutex); - return lastSample; + Lock<FastMutex> l(mutex); + return threadSample; } void UbloxGPS::run() { - UBXUnpackedFrame frame; + /** + * UBX message structure: + * - 2B: Preamble + * - 1B: Message class + * - 1B: Message id + * - 2B: Payload length + * - lB: Payload + * - 2B: Checksum + */ + uint8_t message[6 + UBX_MAX_PAYLOAD_LENGTH + 2]; + uint16_t payloadLength; while (!shouldStop()) { StackLogger::getInstance().updateStack(THID_GPS); // Try to read the message - if (!readUBXFrame(frame)) + if (!readUBXMessage(message, payloadLength)) { LOG_DEBUG(logger, "Unable to read a UBX message"); continue; } // Parse the message - if (!parseUBXFrame(frame)) + if (!parseUBXMessage(message)) { - LOG_DEBUG( - logger, - "UBX message not recognized (class: {:#02x}, id: {:#02x})", - frame.cls, frame.id); + LOG_DEBUG(logger, + "UBX message not recognized (class:0x{02x}, id: 0x{02x})", + message[2], message[3]); } } } -bool UbloxGPS::resetConfiguration() +void UbloxGPS::ubxChecksum(uint8_t* msg, int len) +{ + uint8_t ck_a = 0, ck_b = 0; + + // The length must be valid, at least 8 bytes (preamble, msg, length, + // checksum) + if (len <= 8) + { + return; + } + + // Checksum calculation from byte 2 to end of payload + for (int i = 2; i < len - 2; i++) + { + ck_a = ck_a + msg[i]; + ck_b = ck_b + ck_a; + } + + msg[len - 2] = ck_a; + msg[len - 1] = ck_b; +} + +bool UbloxGPS::writeUBXMessage(uint8_t* message, int length) +{ + // Compute the checksum + ubxChecksum(message, length); + + // Write configuration + if (write(gpsFile, message, length) < 0) + { + LOG_ERR(logger, + "Failed to write ubx message (class:0x{02x}, id: 0x{02x})", + message[2], message[3]); + return false; + } + + return true; +} + +bool UbloxGPS::serialCommuinicationSetup() { - static constexpr uint16_t payloadLength = 4; + intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs(); + + // Change the baudrate only if it is different than the default + if (baudrate != defaultBaudrate) + { + // Close the gps file if already opened + devFs->remove(serialPortName); + + // Open the serial port device with the default boudrate + if (!devFs->addDevice(serialPortName, + intrusive_ref_ptr<Device>(new STM32Serial( + serialPortNumber, defaultBaudrate)))) + { + LOG_ERR(logger, + "[gps] Faild to open serial port {0} with baudrate {1} as " + "file {2}", + serialPortNumber, defaultBaudrate, serialPortName); + return false; + } + + // Open the gps file + if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0) + { + LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath); + return false; + } + + // Change boudrate + if (!setBaudrate()) + { + return false; + }; + + // Close the gps file + if (close(gpsFile) < 0) + { + LOG_ERR(logger, "Failed to close gps file {}", gpsFilePath); + return false; + } + + // Close the serial port + if (!devFs->remove(serialPortName)) + { + LOG_ERR(logger, "Failed to close serial port {} as file {}", + serialPortNumber, serialPortName); + return false; + } + } + + // Reopen the serial port with the configured boudrate + if (!devFs->addDevice(serialPortName, + intrusive_ref_ptr<Device>( + new STM32Serial(serialPortNumber, baudrate)))) + { + LOG_ERR(logger, + "Faild to open serial port {} with baudrate {} as file {}\n", + serialPortNumber, defaultBaudrate, serialPortName); + return false; + } + + // Reopen the gps file + if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0) + { + LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath); + return false; + } + + return true; +} - uint8_t payload[payloadLength] = { +bool UbloxGPS::resetConfiguration() +{ + uint8_t ubx_cfg_prt[RESET_CONFIG_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x04, // Message UBX-CFG-RST + 0x04, 0x00, // Length 0x00, 0x00, // navBbrMask (Hot start) 0x00, // Hardware reset immediately - 0x00 // Reserved + 0x00, // Reserved + 0xff, 0xff // Checksum }; - UBXUnpackedFrame frame{0x06, 0x04, // Message UBX-CFG-RST - payload, payloadLength}; + return writeUBXMessage(ubx_cfg_prt, RESET_CONFIG_MSG_LEN); +} - return writeUBXFrame(frame); +bool UbloxGPS::setBaudrate() +{ + uint8_t ubx_cfg_prt[SET_BAUDRATE_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x0c, 0x00, // Length + 0x00, // Version + 0xff, // All layers + 0x00, 0x00, // Reserved + 0x01, 0x00, 0x52, 0x40, // Configuration item key ID + 0xff, 0xff, 0xff, 0xff, // Value + 0xff, 0xff // Checksum + }; + + // Prepare boud rate + ubx_cfg_prt[14] = baudrate; + ubx_cfg_prt[15] = baudrate >> 8; + ubx_cfg_prt[16] = baudrate >> 16; + ubx_cfg_prt[17] = baudrate >> 24; + + return writeUBXMessage(ubx_cfg_prt, SET_BAUDRATE_MSG_LEN); } bool UbloxGPS::disableNMEAMessages() { - static constexpr uint16_t payloadLength = 34; - - uint8_t payload[payloadLength] = { + uint8_t ubx_cfg_valset[DISABLE_NMEA_MESSAGES_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x22, 0x00, // Length 0x00, // Version 0xff, // All layers 0x00, 0x00, // Reserved @@ -151,146 +306,208 @@ bool UbloxGPS::disableNMEAMessages() 0xac, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_RMC_UART1 key ID 0x00, // CFG-MSGOUT-NMEA_ID_RMC_UART1 value 0xb1, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_VTG_UART1 key ID - 0x00 // CFG-MSGOUT-NMEA_ID_VTG_UART1 value + 0x00, // CFG-MSGOUT-NMEA_ID_VTG_UART1 value + 0xff, 0xff // Checksum }; - UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET - payload, payloadLength}; - - return writeUBXFrame(frame); + return writeUBXMessage(ubx_cfg_valset, DISABLE_NMEA_MESSAGES_MSG_LEN); } bool UbloxGPS::setGNSSConfiguration() { - static constexpr uint16_t payloadLength = 9; - - uint8_t payload[payloadLength] = { + uint8_t ubx_cfg_valset[SET_GNSS_CONF_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x09, 0x00, // Length 0x00, // Version 0x07, // All layers 0x00, 0x00, // Reserved 0x21, 0x00, 0x11, 0x20, // CFG-NAVSPG-DYNMODEL key ID - 0x08 // CFG-NAVSPG-DYNMODEL value + 0x08, // CFG-NAVSPG-DYNMODEL value + 0xff, 0xff // Checksum }; - UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET - payload, payloadLength}; - - return writeUBXFrame(frame); + return writeUBXMessage(ubx_cfg_valset, SET_GNSS_CONF_LEN); } bool UbloxGPS::enableUBXMessages() { - static constexpr uint16_t payloadLength = 9; - - uint8_t payload[payloadLength] = { + uint8_t ubx_cfg_valset[ENABLE_UBX_MESSAGES_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x09, 0x00, // Length 0x00, // Version 0xff, // All layers 0x00, 0x00, // Reserved 0x07, 0x00, 0x91, 0x20, // CFG-MSGOUT-UBX_NAV_PVT_UART1 key ID - 0x01 // CFG-MSGOUT-UBX_NAV_PVT_UART1 value + 0x01, // CFG-MSGOUT-UBX_NAV_PVT_UART1 value + 0xff, 0xff // Checksum }; - UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET - payload, payloadLength}; - - return writeUBXFrame(frame); + return writeUBXMessage(ubx_cfg_valset, ENABLE_UBX_MESSAGES_MSG_LEN); } -bool UbloxGPS::setSampleRate() +bool UbloxGPS::setRate() { - static constexpr uint16_t payloadLength = 10; + uint16_t rate = 1000 / sampleRate; + LOG_DEBUG(logger, "Rate: {}", rate); - uint8_t payload[payloadLength] = { + uint8_t ubx_cfg_valset[SET_RATE_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x0a, 0x00, // Length 0x00, // Version 0x07, // All layers 0x00, 0x00, // Reserved 0x01, 0x00, 0x21, 0x30, // CFG-RATE-MEAS key ID - 0xff, 0xff // CFG-RATE-MEAS value (placeholder) + 0xff, 0xff, // CFG-RATE-MEAS value + 0xff, 0xff // Checksum }; - memcpy(&payload[8], &samplerate, 2); - UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET - payload, payloadLength}; + // Prepare rate + ubx_cfg_valset[14] = rate; + ubx_cfg_valset[15] = rate >> 8; - return writeUBXFrame(frame); + return writeUBXMessage(ubx_cfg_valset, SET_RATE_MSG_LEN); } -bool UbloxGPS::parseUBXFrame(const UBXUnpackedFrame& frame) +bool UbloxGPS::readUBXMessage(uint8_t* message, uint16_t& payloadLength) { - switch (frame.cls) // Message class + // Read preamble + do + { + // Read util the first byte of the preamble + do + { + if (read(gpsFile, &message[0], 1) <= 0) // No more data available + { + return false; + } + } while (message[0] != PREAMBLE[0]); + + // Read the next byte + if (read(gpsFile, &message[1], 1) <= 0) // No more data available + { + return false; + } + } while (message[1] != PREAMBLE[1]); // Continue + + // Read message class and ID + if (read(gpsFile, &message[2], 1) <= 0) + { + return false; + } + if (read(gpsFile, &message[3], 1) <= 0) + { + return false; + } + + // Read length + if (read(gpsFile, &message[4], 2) <= 0) + { + return false; + } + payloadLength = message[4] | (message[5] << 8); + if (payloadLength > UBX_MAX_PAYLOAD_LENGTH) + { + return false; + } + + // Read paylaod and checksum + for (auto i = 0; i < payloadLength + 2; i++) + { + if (read(gpsFile, &message[6 + i], 1) <= 0) + { + return false; + } + } + + // Verify the checksum + uint8_t msgChecksum1 = message[6 + payloadLength]; + uint8_t msgChecksum2 = message[6 + payloadLength + 1]; + ubxChecksum(message, 6 + payloadLength + 2); + if (msgChecksum1 != message[6 + payloadLength] || + msgChecksum2 != message[6 + payloadLength + 1]) + { + LOG_ERR(logger, "Message checksum verification failed"); + return false; + } + + return true; +} + +bool UbloxGPS::parseUBXMessage(uint8_t* message) +{ + switch (message[2]) // Message class { case 0x01: // UBX-NAV - return parseUBXNAVFrame(frame); + return parseUBXNAVMessage(message); case 0x05: // UBX-ACK - return parseUBXACKFrame(frame); + return parseUBXACKMessage(message); } return false; } -bool UbloxGPS::parseUBXNAVFrame(const UBXUnpackedFrame& frame) +bool UbloxGPS::parseUBXNAVMessage(uint8_t* message) { - switch (frame.id) // Message ID + switch (message[3]) // Message id { case 0x07: // UBX-NAV-PVT - // Lock the lastSample variable - Lock<FastMutex> l(sampleMutex); + // Lock the threadSample variable + Lock<FastMutex> l(mutex); // Latitude - int32_t rawLatitude = frame.payload[28] | frame.payload[29] << 8 | - frame.payload[30] << 16 | - frame.payload[31] << 24; - lastSample.latitude = (float)rawLatitude / 1e7; + int32_t rawLatitude = message[6 + 28] | message[6 + 29] << 8 | + message[6 + 30] << 16 | message[6 + 31] << 24; + threadSample.latitude = (float)rawLatitude / 1e7; // Longitude - int32_t rawLongitude = frame.payload[24] | frame.payload[25] << 8 | - frame.payload[26] << 16 | - frame.payload[27] << 24; - lastSample.longitude = (float)rawLongitude / 1e7; + int32_t rawLongitude = message[6 + 24] | message[6 + 25] << 8 | + message[6 + 26] << 16 | + message[6 + 27] << 24; + threadSample.longitude = (float)rawLongitude / 1e7; // Height - int32_t rawHeight = frame.payload[32] | frame.payload[33] << 8 | - frame.payload[34] << 16 | - frame.payload[35] << 24; - lastSample.height = (float)rawHeight / 1e3; + int32_t rawHeight = message[6 + 32] | message[6 + 33] << 8 | + message[6 + 34] << 16 | message[6 + 35] << 24; + threadSample.height = (float)rawHeight / 1e3; // Velocity north - int32_t rawVelocityNorth = - frame.payload[48] | frame.payload[49] << 8 | - frame.payload[50] << 16 | frame.payload[51] << 24; - lastSample.velocityNorth = (float)rawVelocityNorth / 1e3; + int32_t rawVelocityNorth = message[6 + 48] | message[6 + 49] << 8 | + message[6 + 50] << 16 | + message[6 + 51] << 24; + threadSample.velocityNorth = (float)rawVelocityNorth / 1e3; // Velocity east - int32_t rawVelocityEast = - frame.payload[52] | frame.payload[53] << 8 | - frame.payload[54] << 16 | frame.payload[55] << 24; - lastSample.velocityEast = (float)rawVelocityEast / 1e3; + int32_t rawVelocityEast = message[6 + 52] | message[6 + 53] << 8 | + message[6 + 54] << 16 | + message[6 + 55] << 24; + threadSample.velocityEast = (float)rawVelocityEast / 1e3; // Velocity down - int32_t rawVelocityDown = - frame.payload[56] | frame.payload[57] << 8 | - frame.payload[58] << 16 | frame.payload[59] << 24; - lastSample.velocityDown = (float)rawVelocityDown / 1e3; + int32_t rawVelocityDown = message[6 + 56] | message[6 + 57] << 8 | + message[6 + 58] << 16 | + message[6 + 59] << 24; + threadSample.velocityDown = (float)rawVelocityDown / 1e3; // Speed - int32_t rawSpeed = frame.payload[60] | frame.payload[61] << 8 | - frame.payload[62] << 16 | - frame.payload[63] << 24; - lastSample.speed = (float)rawSpeed / 1e3; + int32_t rawSpeed = message[6 + 60] | message[6 + 61] << 8 | + message[6 + 62] << 16 | message[6 + 63] << 24; + threadSample.speed = (float)rawSpeed / 1e3; // Track (heading of motion) - int32_t rawTrack = frame.payload[64] | frame.payload[65] << 8 | - frame.payload[66] << 16 | - frame.payload[67] << 24; - lastSample.track = (float)rawTrack / 1e5; + int32_t rawTrack = message[6 + 64] | message[6 + 65] << 8 | + message[6 + 66] << 16 | message[6 + 67] << 24; + threadSample.track = (float)rawTrack / 1e5; // Number of satellite - lastSample.satellites = (uint8_t)frame.payload[23]; + threadSample.satellites = (uint8_t)message[6 + 23]; // Fix (every type of fix accepted) - lastSample.fix = frame.payload[20] != 0; + threadSample.fix = message[6 + 20] != 0; // Timestamp - lastSample.gpsTimestamp = + threadSample.gpsTimestamp = TimestampTimer::getInstance().getTimestamp(); return true; @@ -299,209 +516,22 @@ bool UbloxGPS::parseUBXNAVFrame(const UBXUnpackedFrame& frame) return false; } -bool UbloxGPS::parseUBXACKFrame(const UBXUnpackedFrame& frame) +bool UbloxGPS::parseUBXACKMessage(uint8_t* message) { - switch (frame.id) // Message ID + switch (message[3]) // Message id { case 0x00: // UBX-ACK-NAC LOG_DEBUG(logger, - "Received NAC for message (class: {:#02x}, id: {:#02x})", - frame.cls, frame.id); + "Received NAC for message (class:0x{02x}, id: 0x{02x})", + message[6], message[7]); return true; case 0x01: // UBX-ACK-ACK LOG_DEBUG(logger, - "Received ACK for message (class: {:#02x}, id: {:#02x})", - frame.cls, frame.id); + "Received ACK for message (class:0x{02x}, id: 0x{02x})", + message[6], message[7]); return true; } return false; } -bool UbloxGPS::writeUBXFrame(const UBXUnpackedFrame& frame) -{ - if (!frame.isValid()) - { - LOG_ERR(logger, "UBX frame to write is invalid"); - return false; - } - - uint8_t packedFrame[UBX_MAX_FRAME_LENGTH]; - frame.writePacked(packedFrame); - - writeRaw(packedFrame, frame.getFrameLength()); - - return true; -} - -bool UbloxGPS::readUBXFrame(UBXUnpackedFrame& frame) -{ - bool synchronized = false; - while (!synchronized) - { - synchronized = true; - for (uint16_t i = 0; synchronized && i < 2; ++i) - { - if (!readRaw(&frame.preamble[i], 1)) - return false; - - if (frame.preamble[i] != UBX_VALID_PREAMBLE[i]) - synchronized = false; - } - } - - if (!readRaw(&frame.cls, 1) || !readRaw(&frame.id, 1) || - !readRaw((uint8_t*)&frame.payloadLength, 2) || - !readRaw(frame.payload, frame.payloadLength) || - !readRaw(frame.checksum, 2)) - return false; - - if (!frame.isValid()) - { - LOG_ERR(logger, "UBX frame to read is invalid"); - return false; - } - - return true; -} - -UbloxGPSSPI::UbloxGPSSPI(SPIBusInterface& spiBus, GpioPin spiCs, - SPIBusConfig spiConfig, uint8_t samplerate) - : UbloxGPS(samplerate), spiSlave(spiBus, spiCs, spiConfig) -{ -} - -SPIBusConfig UbloxGPSSPI::getDefaultSPIConfig() -{ - SPIBusConfig spiConfig{}; - spiConfig.clockDivider = SPI::ClockDivider::DIV_32; - spiConfig.mode = SPI::Mode::MODE_1; - return spiConfig; -} - -bool UbloxGPSSPI::writeRaw(uint8_t* data, size_t size) -{ - SPITransaction spi{spiSlave}; - spi.write(data, size); - return true; -} - -bool UbloxGPSSPI::readRaw(uint8_t* data, size_t size) -{ - SPITransaction spi{spiSlave}; - spi.read(data, size); - return true; -} - -UbloxGPSSerial::UbloxGPSSerial(int serialBaudrate, uint8_t samplerate, - int serialDefaultBaudrate, int serialPortNumber, - const char* serialPortName) - : UbloxGPS(samplerate), serialPortNumber(serialPortNumber), - serialPortName(serialPortName), serialBaudrate(serialBaudrate), - serialDefaultBaudrate(serialDefaultBaudrate) -{ - strcpy(serialFilePath, "/dev/"); - strcat(serialFilePath, serialPortName); -} - -bool UbloxGPSSerial::setupCommunication() -{ - intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs(); - - // Close the serial file if already opened - devFs->remove(serialPortName); - - // Change the baudrate only if it is different than the default - if (serialBaudrate != serialDefaultBaudrate) - { - // Open the serial port device with the default baudrate - if (!devFs->addDevice(serialPortName, - intrusive_ref_ptr<Device>(new STM32Serial( - serialPortNumber, serialDefaultBaudrate)))) - { - LOG_ERR(logger, - "[gps] Faild to open serial port {} with baudrate {} as " - "file {}", - serialPortNumber, serialDefaultBaudrate, serialPortName); - return false; - } - - // Open the serial file - if ((serialFile = open(serialFilePath, O_RDWR)) < 0) - { - LOG_ERR(logger, "Failed to open serial file {}", serialFilePath); - return false; - } - - // Change baudrate - if (!setBaudrate()) - { - return false; - } - - // Close the serial file - if (close(serialFile) < 0) - { - LOG_ERR(logger, "Failed to close serial file {}", serialFilePath); - return false; - } - - // Close the serial port - if (!devFs->remove(serialPortName)) - { - LOG_ERR(logger, "Failed to close serial port {} as file {}", - serialPortNumber, serialPortName); - return false; - } - } - - // Reopen the serial port with the configured baudrate - if (!devFs->addDevice(serialPortName, - intrusive_ref_ptr<Device>(new STM32Serial( - serialPortNumber, serialBaudrate)))) - { - LOG_ERR(logger, - "Faild to open serial port {} with baudrate {} as file {}\n", - serialPortNumber, serialDefaultBaudrate, serialPortName); - return false; - } - - // Reopen the serial file - if ((serialFile = open(serialFilePath, O_RDWR)) < 0) - { - LOG_ERR(logger, "Failed to open serial file {}", serialFilePath); - return false; - } - - return true; -} - -bool UbloxGPSSerial::setBaudrate() -{ - static constexpr uint16_t payloadLength = 12; - - uint8_t payload[payloadLength] = { - 0x00, // Version - 0xff, // All layers - 0x00, 0x00, // Reserved - 0x01, 0x00, 0x52, 0x40, // Configuration item key ID - 0xff, 0xff, 0xff, 0xff // Value (placeholder) - }; - memcpy(&payload[8], &serialBaudrate, 4); - - UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET - payload, payloadLength}; - - return writeUBXFrame(frame); -} - -bool UbloxGPSSerial::writeRaw(uint8_t* data, size_t size) -{ - return write(serialFile, data, size) >= 0; -} - -bool UbloxGPSSerial::readRaw(uint8_t* data, size_t size) -{ - return read(serialFile, data, size) >= 0; -} - } // namespace Boardcore diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h index 6d980d6420cf64441b46df319d8a904bdd930002..3f7a1616ef1c0c732527b91ed209964d9af3c893 100644 --- a/src/shared/sensors/UbloxGPS/UbloxGPS.h +++ b/src/shared/sensors/UbloxGPS/UbloxGPS.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda + * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -24,57 +24,87 @@ #include <ActiveObject.h> #include <diagnostic/PrintLogger.h> -#include <drivers/spi/SPIDriver.h> +#include <miosix.h> #include <sensors/Sensor.h> -#include "UBXUnpackedFrame.h" #include "UbloxGPSData.h" namespace Boardcore { /** - * @brief Sensor for Ublox GPS. + * @brief Driver for Ublox GPSs * - * This sensor handles communication and setup with Ublox GPSs. It uses the + * This driver handles communication and setup with Ublox GPSs. It uses the * binary UBX protocol to retrieve and parse navigation data quicker than using * the string based NMEA. * - * At initialization it configures the device with the specified sample rate, - * resets the configuration and sets up UBX messages and GNSS parameters. + * At initialization it configures the device with the specified baudrate, reset + * the configuration and sets up UBX messages and GNSS parameters. * - * Communication with the device is performed through SPI. + * Communication with the device is performed through a file, the driver opens + * the serial port under the filepath /dev/<serialPortName>. + * There is no need for the file to be setted up beforhand. * * This driver was written for a NEO-M9N gps with the latest version of UBX. */ class UbloxGPS : public Sensor<UbloxGPSData>, public ActiveObject { public: - explicit UbloxGPS(uint8_t samplerate) : samplerate(samplerate) {} + /** + * @brief Construct a new UbloxGPS object + * + * @param boudrate_ Baudrate to communicate with the device (max: 921600, + * min: 4800 for NEO-M9N) + * @param sampleRate_ GPS sample rate (max: 25 for NEO-M9N) + * @param serialPortName_ Name of the file for the gps device + * @param defaultBaudrate_ Startup baudrate (38400 for NEO-M9N) + */ + UbloxGPS(int boudrate_ = 921600, uint8_t sampleRate_ = 10, + int serialPortNumber_ = 2, const char *serialPortName_ = "gps", + int defaultBaudrate_ = 38400); /** - * @brief Disables the NMEA messages, configures GNSS options and enables - * UBX-PVT message + * @brief Sets up the serial port baudrate, disables the NMEA messages, + * configures GNSS options and enables UBX-PVT message * * @return True if the operation succeeded */ - virtual bool init() override; + bool init() override; /** - * @brief Reads a single message form the GPS, waits 2 sample cycles. + * @brief Read a single message form the GPS, waits 2 sample cycle * * @return True if a message was sampled */ bool selfTest() override; -protected: +private: UbloxGPSData sampleImpl() override; void run() override; + void ubxChecksum(uint8_t *msg, int len); + + /** + * @brief Compute the checksum and write the message to the device + */ + bool writeUBXMessage(uint8_t *message, int length); + + /** + * @brief Sets up the serial port with the correct baudrate + * + * Opens the serial port with the defaul baudrate and changes it to + * the value specified in the constructor, then it reopens the serial port. + * If the device is already using the correct baudrate this won't have + * effect. However if the gps is using a different baudrate the diver won't + * be able to communicate. + */ + bool serialCommuinicationSetup(); + bool resetConfiguration(); - virtual bool setupCommunication() { return true; } + bool setBaudrate(); bool disableNMEAMessages(); @@ -82,101 +112,40 @@ protected: bool enableUBXMessages(); - bool setSampleRate(); - - bool parseUBXFrame(const UBXUnpackedFrame& frame); - - bool parseUBXNAVFrame(const UBXUnpackedFrame& frame); - - bool parseUBXACKFrame(const UBXUnpackedFrame& frame); - - bool writeUBXFrame(const UBXUnpackedFrame& frame); - - bool readUBXFrame(UBXUnpackedFrame& frame); - - virtual bool writeRaw(uint8_t* data, size_t size) = 0; - - virtual bool readRaw(uint8_t* data, size_t size) = 0; - - const uint8_t samplerate; // [Hz] - - mutable miosix::FastMutex sampleMutex; - UbloxGPSData lastSample{}; - - PrintLogger logger = Logging::getLogger("ubloxgps"); -}; + bool setRate(); -class UbloxGPSSPI : public UbloxGPS -{ -public: - /** - * @brief Constructor. - * - * @param bus The Spi bus. - * @param cs The CS pin to lower when we need to sample. - * @param config The SPI configuration. - * @param samplerate Sample rate to communicate with the device - */ - UbloxGPSSPI(SPIBusInterface& spiBus, miosix::GpioPin spiCs, - SPIBusConfig spiConfig = getDefaultSPIConfig(), - uint8_t samplerate = 250); + bool readUBXMessage(uint8_t *message, uint16_t &payloadLength); - /** - * Constructs the default config for SPI Bus. - * - * @returns The default SPIBusConfig. - */ - static SPIBusConfig getDefaultSPIConfig(); + bool parseUBXMessage(uint8_t *message); -private: - virtual bool writeRaw(uint8_t* data, size_t size) override; + bool parseUBXNAVMessage(uint8_t *message); - virtual bool readRaw(uint8_t* data, size_t size) override; + bool parseUBXACKMessage(uint8_t *message); - SPISlave spiSlave; -}; + const int baudrate; // [baud] + const uint8_t sampleRate; // [Hz] + const int serialPortNumber; + const char *serialPortName; + const int defaultBaudrate; // [baud] -class UbloxGPSSerial : public UbloxGPS -{ -public: - /** - * @brief Serial constructor. - * - * @param serialPortNumber Number of the serial port - * @param serialPortName Name of the file for the gps device - * @param serialBaudrate Baudrate to communicate with the device (max: - * 921600, min: 4800 for NEO-M9N) - * @param samplerate GPS sample rate (max: 25 for NEO-M9N) - * @param serialDefaultBaudrate Startup baudrate (38400 for NEO-M9N) - */ - UbloxGPSSerial(int serialBaudrate = 921600, uint8_t samplerate = 10, - int serialDefaultBaudrate = 38400, int serialPortNumber = 2, - const char* serialPortName = "gps"); + char gpsFilePath[16]; ///< Allows for a filename of up to 10 characters + int gpsFile; -private: - /** - * @brief Sets up the serial port with the correct baudrate - * - * Opens the serial port with the default baudrate and changes it to - * the value specified in the constructor, then it reopens the serial port. - * If the device is already using the correct baudrate this won't have - * effect. However if the gps is using a different baudrate the diver won't - * be able to communicate. - */ - virtual bool setupCommunication() override; + mutable miosix::FastMutex mutex; + UbloxGPSData threadSample{}; - bool setBaudrate(); + static constexpr int SET_BAUDRATE_MSG_LEN = 20; + static constexpr int RESET_CONFIG_MSG_LEN = 12; + static constexpr int DISABLE_NMEA_MESSAGES_MSG_LEN = 42; + static constexpr int SET_GNSS_CONF_LEN = 17; + static constexpr int ENABLE_UBX_MESSAGES_MSG_LEN = 17; + static constexpr int SET_RATE_MSG_LEN = 18; - virtual bool writeRaw(uint8_t* data, size_t size) override; + static constexpr uint8_t PREAMBLE[] = {0xb5, 0x62}; - virtual bool readRaw(uint8_t* data, size_t size) override; + static constexpr int UBX_MAX_PAYLOAD_LENGTH = 92; - const int serialPortNumber; - const char* serialPortName; - const int serialBaudrate; // [baud] - const int serialDefaultBaudrate; // [baud] - char serialFilePath[16]; // Allows for a filename of up to 10 characters - int serialFile = -1; + PrintLogger logger = Logging::getLogger("ubloxgps"); }; } // namespace Boardcore diff --git a/src/shared/sensors/UbloxGPS/UbloxGPSData.h b/src/shared/sensors/UbloxGPS/UbloxGPSData.h index f61d4fc00ab8f682c1d4afd18b3cb4d6c1ac1e0a..7f5570000b78b915a956b96e754d784e08f7611f 100644 --- a/src/shared/sensors/UbloxGPS/UbloxGPSData.h +++ b/src/shared/sensors/UbloxGPS/UbloxGPSData.h @@ -31,8 +31,8 @@ struct UbloxGPSData : public GPSData { static std::string header() { - return "gpsTimestamp,latitude,longitude,height,velocityNorth," - "velocityEast,velocityDown,speed,track,satellites,fix\n"; + return "gps_timestamp,latitude,longitude,height,velocity_north," + "velocity_east,velocity_down,speed,track,num_satellites,fix\n"; } void print(std::ostream &os) const diff --git a/src/tests/drivers/sx1278/test-sx1278-bench.cpp b/src/tests/drivers/sx1278/test-sx1278-bench.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5ddb67f00a8800f77f5597fa3c9538fb8496a6e5 --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-bench.cpp @@ -0,0 +1,315 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <drivers/SX1278/SX1278.h> +#include <drivers/interrupt/external_interrupts.h> + +#include <thread> + +#include "test-sx1278-core.h" + +using namespace Boardcore; +using namespace miosix; + +/** + * 0 -> RX + * 1 -> TX + * + * Connection diagram: + * sx1278[0]:nss -> stm32:pa1 + * sx1278[0]:dio0 -> stm32:pc15 + * sx1278[0]:mosi -> stm32:pc12 (SPI3_MOSI) + * sx1278[0]:miso -> stm32:pc11 (SPI3_MISO) + * sx1278[0]:sck -> stm32:pc10 (SPI3_SCK) + + * sx1278[1]:nss -> stm32:pa2 + * sx1278[1]:dio0 -> stm32:pc0 + * sx1278[1]:mosi -> stm32:pb15 (SPI2_MOSI) + * sx1278[1]:miso -> stm32:pb14 (SPI2_MISO) + * sx1278[1]:sck -> stm32:pb13 (SPI2_SCK) + */ + +SPIBus bus0(SPI3); +SPIBus bus1(SPI2); + +GpioPin sck0(GPIOC_BASE, 10); +GpioPin miso0(GPIOC_BASE, 11); +GpioPin mosi0(GPIOC_BASE, 12); +GpioPin cs0(GPIOA_BASE, 1); +GpioPin dio0(GPIOC_BASE, 15); + +GpioPin sck1(GPIOB_BASE, 13); +GpioPin miso1(GPIOB_BASE, 14); +GpioPin mosi1(GPIOB_BASE, 15); +GpioPin cs1(GPIOA_BASE, 2); +GpioPin dio1(GPIOC_BASE, 0); + +struct Stats; + +const char *stringFromErr(SX1278::Error err); +const char *stringFromRxBw(SX1278::RxBw rx_bw); + +void printStats(Stats stats); + +/// Status informations. +struct Stats +{ + int last_recv_packet = 0; //< Last received packet ID. + int corrupted_packets = 0; //< Packets that got mangled during tx. + int send_count = 0; //< Actual number of packets sent. + int recv_count = 0; //< Actual number of packets received. + int recv_errors = 0; //< Number of failed recvs. + + float packet_loss() const + { + if (last_recv_packet != 0) + { + return 1.0f - ((float)recv_count / (float)last_recv_packet); + } + else + { + return 0.0f; + } + } + +} stats; + +/// Interval between transmissions. +const int TX_INTERVAL = 1; + +SX1278 *sx1278[2] = {nullptr, nullptr}; + +struct Msg +{ + int idx; + int dummy_1; + int dummy_2; + int dummy_3; + + const static int DUMMY_1 = 0xdeadbeef; + const static int DUMMY_2 = 0xbeefdead; + const static int DUMMY_3 = 0x1234abcd; +}; + +void recvLoop(int idx) +{ + while (1) + { + Msg msg; + msg.idx = 0; + msg.dummy_1 = 0; + msg.dummy_2 = 0; + msg.dummy_3 = 0; + + int len = sx1278[idx]->receive((uint8_t *)&msg, sizeof(msg)); + if (len != sizeof(msg)) + { + stats.recv_errors++; + } + else if (msg.dummy_1 != Msg::DUMMY_1 || msg.dummy_2 != Msg::DUMMY_2 || + msg.dummy_3 != Msg::DUMMY_3) + { + stats.recv_errors++; + stats.corrupted_packets++; + } + else + { + stats.last_recv_packet = msg.idx; + stats.recv_count++; + } + } +} + +void sendLoop(int idx) +{ + while (1) + { + miosix::Thread::sleep(TX_INTERVAL); + + int next_idx = stats.send_count + 1; + + Msg msg; + msg.idx = next_idx; + msg.dummy_1 = Msg::DUMMY_1; + msg.dummy_2 = Msg::DUMMY_2; + msg.dummy_3 = Msg::DUMMY_3; + + sx1278[idx]->send((uint8_t *)&msg, sizeof(msg)); + stats.send_count = next_idx; + } +} + +/// Get current time +long long now() { return miosix::getTick() * 1000 / miosix::TICK_FREQ; } + +void __attribute__((used)) EXTI15_IRQHandlerImpl() +{ + if (sx1278[0]) + sx1278[0]->handleDioIRQ(); +} + +void __attribute__((used)) EXTI0_IRQHandlerImpl() +{ + if (sx1278[1]) + sx1278[1]->handleDioIRQ(); +} + +/// Initialize stm32f407g board (sx1278[0] only) +void initBoard0() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable SPI3 + RCC->APB1ENR |= RCC_APB1ENR_SPI3EN; + RCC_SYNC(); + + // Setup SPI pins + sck0.mode(miosix::Mode::ALTERNATE); + sck0.alternateFunction(6); + miso0.mode(miosix::Mode::ALTERNATE); + miso0.alternateFunction(6); + mosi0.mode(miosix::Mode::ALTERNATE); + mosi0.alternateFunction(6); + + cs0.mode(miosix::Mode::OUTPUT); + dio0.mode(miosix::Mode::INPUT); + } + + cs0.high(); + enableExternalInterrupt(dio0.getPort(), dio0.getNumber(), + InterruptTrigger::RISING_EDGE); +} + +/// Initialize stm32f407g board (sx1278[1] only) +void initBoard1() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable SPI2 + RCC->APB1ENR |= RCC_APB1ENR_SPI2EN; + RCC_SYNC(); + + sck1.mode(miosix::Mode::ALTERNATE); + sck1.alternateFunction(5); + miso1.mode(miosix::Mode::ALTERNATE); + miso1.alternateFunction(5); + mosi1.mode(miosix::Mode::ALTERNATE); + mosi1.alternateFunction(5); + + cs1.mode(miosix::Mode::OUTPUT); + dio1.mode(miosix::Mode::INPUT); + } + + cs1.high(); + enableExternalInterrupt(dio1.getPort(), dio1.getNumber(), + InterruptTrigger::RISING_EDGE); +} + +int main() +{ +#ifndef DISABLE_RX + initBoard0(); +#endif +#ifndef DISABLE_TX + initBoard1(); +#endif + + // Run default configuration + SX1278::Config config; + SX1278::Error err; + + // Configure them +#ifndef DISABLE_RX + sx1278[0] = new SX1278(bus0, cs0); + + printf("\n[sx1278] Configuring sx1278[0]...\n"); + printConfig(config); + if ((err = sx1278[0]->init(config)) != SX1278::Error::NONE) + { + printf("[sx1278] sx1278[0]->init error: %s\n", stringFromErr(err)); + return -1; + } +#endif + +#ifndef DISABLE_TX + sx1278[1] = new SX1278(bus1, cs1); + + printf("\n[sx1278] Configuring sx1278[1]...\n"); + printConfig(config); + if ((err = sx1278[1]->init(config)) != SX1278::Error::NONE) + { + printf("[sx1278] sx1278[1]->init error: %s\n", stringFromErr(err)); + return -1; + } +#endif + + // Run background threads +#ifndef DISABLE_RX + std::thread recv([]() { recvLoop(0); }); +#endif +#ifndef DISABLE_TX + std::thread send([]() { sendLoop(1); }); +#endif + + // Finish! + long long start = now(); + + printf("\n[sx1278] Initialization complete!\n"); + + miosix::Thread::sleep(4000); + + while (1) + { + long long elapsed = now() - start; + + // Calculate bitrates + float tx_bitrate = (float)(stats.send_count * sizeof(Msg) * 8) / + ((float)elapsed / 1000.0f); + + float rx_bitrate = (float)(stats.recv_count * sizeof(Msg) * 8) / + ((float)elapsed / 1000.0f); + + printf("\n[sx1278] Stats:\n"); + printStats(stats); + printf("tx_bitrate: %.2f kb/s\n", tx_bitrate / 1000.0f); + printf("rx_bitrate: %.2f kb/s\n", rx_bitrate / 1000.0f); + + miosix::Thread::sleep(2000); + } + + return 0; +} + +void printStats(Stats stats) +{ + // Prints are REALLY slow, so take a COPY of stats, so we can print an + // instant in time. + + printf("stats.last_recv_packet = %d\n", stats.last_recv_packet); + printf("stats.corrupted_packets = %d\n", stats.corrupted_packets); + printf("stats.send_count = %d\n", stats.send_count); + printf("stats.recv_count = %d\n", stats.recv_count); + printf("stats.recv_errors = %d\n", stats.recv_errors); + printf("stats.packet_loss = %.2f %%\n", stats.packet_loss() * 100.0f); +} diff --git a/src/tests/drivers/sx1278/test-sx1278-bidir.cpp b/src/tests/drivers/sx1278/test-sx1278-bidir.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6ecf7b42664660dd6331bf8c707f130135216e3b --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-bidir.cpp @@ -0,0 +1,201 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <drivers/SX1278/SX1278.h> +#include <drivers/interrupt/external_interrupts.h> + +#include <cstring> +#include <thread> + +#include "test-sx1278-core.h" + +using namespace Boardcore; +using namespace miosix; + +/** + * Connection diagram: + * sx1278[0]:nss -> stm32:pa1 + * sx1278[0]:dio0 -> stm32:pc15 + * sx1278[0]:mosi -> stm32:pc12 (SPI3_MOSI) + * sx1278[0]:miso -> stm32:pc11 (SPI3_MISO) + * sx1278[0]:sck -> stm32:pc10 (SPI3_SCK) + + * sx1278[1]:nss -> stm32:pa2 + * sx1278[1]:dio0 -> stm32:pc0 + * sx1278[1]:mosi -> stm32:pb15 (SPI2_MOSI) + * sx1278[1]:miso -> stm32:pb14 (SPI2_MISO) + * sx1278[1]:sck -> stm32:pb13 (SPI2_SCK) + */ + +SPIBus bus0(SPI3); +SPIBus bus1(SPI2); + +GpioPin sck0(GPIOC_BASE, 10); +GpioPin miso0(GPIOC_BASE, 11); +GpioPin mosi0(GPIOC_BASE, 12); +GpioPin cs0(GPIOA_BASE, 1); +GpioPin dio0(GPIOC_BASE, 15); + +GpioPin sck1(GPIOB_BASE, 13); +GpioPin miso1(GPIOB_BASE, 14); +GpioPin mosi1(GPIOB_BASE, 15); +GpioPin cs1(GPIOA_BASE, 2); +GpioPin dio1(GPIOC_BASE, 0); + +SX1278 *sx1278[2] = {nullptr, nullptr}; + +void __attribute__((used)) EXTI15_IRQHandlerImpl() +{ + if (sx1278[0]) + sx1278[0]->handleDioIRQ(); +} + +void __attribute__((used)) EXTI0_IRQHandlerImpl() +{ + if (sx1278[1]) + sx1278[1]->handleDioIRQ(); +} + +/// Initialize stm32f407g board (sx1278[0] only) +void initBoard0() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable SPI3 + RCC->APB1ENR |= RCC_APB1ENR_SPI3EN; + RCC_SYNC(); + + // Setup SPI pins + sck0.mode(miosix::Mode::ALTERNATE); + sck0.alternateFunction(6); + miso0.mode(miosix::Mode::ALTERNATE); + miso0.alternateFunction(6); + mosi0.mode(miosix::Mode::ALTERNATE); + mosi0.alternateFunction(6); + + cs0.mode(miosix::Mode::OUTPUT); + dio0.mode(miosix::Mode::INPUT); + } + + cs0.high(); + enableExternalInterrupt(dio0.getPort(), dio0.getNumber(), + InterruptTrigger::RISING_EDGE); +} + +/// Initialize stm32f407g board (sx1278[1] only) +void initBoard1() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable SPI2 + RCC->APB1ENR |= RCC_APB1ENR_SPI2EN; + RCC_SYNC(); + + sck1.mode(miosix::Mode::ALTERNATE); + sck1.alternateFunction(5); + miso1.mode(miosix::Mode::ALTERNATE); + miso1.alternateFunction(5); + mosi1.mode(miosix::Mode::ALTERNATE); + mosi1.alternateFunction(5); + + cs1.mode(miosix::Mode::OUTPUT); + dio1.mode(miosix::Mode::INPUT); + } + + cs1.high(); + enableExternalInterrupt(dio1.getPort(), dio1.getNumber(), + InterruptTrigger::RISING_EDGE); +} + +void recvLoop(int idx) +{ + char buf[256]; + while (1) + { + if (sx1278[idx]->receive((uint8_t *)buf, sizeof(buf)) != -1) + { + // Make sure there is a terminator somewhere + buf[255] = 0; + printf("[sx1278 @ %p] Received '%s'\n", sx1278[idx], buf); + } + } +} + +void sendLoop(int idx, int interval, char *data) +{ + while (1) + { + miosix::Thread::sleep(interval); + + sx1278[idx]->send((uint8_t *)data, strlen(data) + 1); + printf("[sx1278 @ %p] Sent '%s'\n", sx1278[idx], data); + } +} + +int main() +{ + initBoard0(); + initBoard1(); + + // Run default configuration + SX1278::Config config; + SX1278::Error err; + + // Configure them + sx1278[0] = new SX1278(bus0, cs0); + + printf("\n[sx1278] Configuring sx1278[0]...\n"); + printConfig(config); + if ((err = sx1278[0]->init(config)) != SX1278::Error::NONE) + { + printf("[sx1278] sx1278[0]->init error: %s\n", stringFromErr(err)); + return -1; + } + + sx1278[1] = new SX1278(bus1, cs1); + + printf("\n[sx1278] Configuring sx1278[1]...\n"); + printConfig(config); + if ((err = sx1278[1]->init(config)) != SX1278::Error::NONE) + { + printf("[sx1278] sx1278[1]->init error: %s\n", stringFromErr(err)); + return -1; + } + + // Spawn all threads + std::thread send0( + [=]() { sendLoop(0, 1000, const_cast<char *>("Hi from sx1278[0]!")); }); + std::thread send1( + [=]() { sendLoop(1, 3333, const_cast<char *>("Hi from sx1278[1]!")); }); + + std::thread recv0([]() { recvLoop(0); }); + std::thread recv1([]() { recvLoop(1); }); + + printf("\n[sx1278] Initialization complete!\n"); + + while (1) + miosix::Thread::wait(); + + return 0; +} diff --git a/src/tests/drivers/sx1278/test-sx1278-core.cpp b/src/tests/drivers/sx1278/test-sx1278-core.cpp new file mode 100644 index 0000000000000000000000000000000000000000..86d4bc25445433ae4bfdbad5818197f60ca029e1 --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-core.cpp @@ -0,0 +1,123 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "test-sx1278-core.h" + +using namespace Boardcore; + +const char *stringFromErr(SX1278::Error err) +{ + switch (err) + { + case SX1278::Error::BAD_VALUE: + return "Error::BAD_VALUE"; + + case SX1278::Error::BAD_VERSION: + return "Error::BAD_VERSION"; + + default: + return "<unknown>"; + } +} + +const char *stringFromRxBw(SX1278::RxBw rx_bw) +{ + switch (rx_bw) + { + case SX1278::RxBw::HZ_2600: + return "RxBw::HZ_2600"; + + case SX1278::RxBw::HZ_3100: + return "RxBw::HZ_3100"; + + case SX1278::RxBw::HZ_3900: + return "RxBw::HZ_3900"; + + case SX1278::RxBw::HZ_5200: + return "RxBw::HZ_5200"; + + case SX1278::RxBw::HZ_6300: + return "RxBw::HZ_6300"; + + case SX1278::RxBw::HZ_7800: + return "RxBw::HZ_7800"; + + case SX1278::RxBw::HZ_10400: + return "RxBw::HZ_10400"; + + case SX1278::RxBw::HZ_12500: + return "RxBw::HZ_12500"; + + case SX1278::RxBw::HZ_15600: + return "RxBw::HZ_15600"; + + case SX1278::RxBw::HZ_20800: + return "RxBw::HZ_20800"; + + case SX1278::RxBw::HZ_25000: + return "RxBw::HZ_25000"; + + case SX1278::RxBw::HZ_31300: + return "RxBw::HZ_31300"; + + case SX1278::RxBw::HZ_41700: + return "RxBw::HZ_41700"; + + case SX1278::RxBw::HZ_50000: + return "RxBw::HZ_50000"; + + case SX1278::RxBw::HZ_62500: + return "RxBw::HZ_62500"; + + case SX1278::RxBw::HZ_83300: + return "RxBw::HZ_83300"; + + case SX1278::RxBw::HZ_100000: + return "RxBw::HZ_100000"; + + case SX1278::RxBw::HZ_125000: + return "RxBw::HZ_125000"; + + case SX1278::RxBw::HZ_166700: + return "RxBw::HZ_166700"; + + case SX1278::RxBw::HZ_200000: + return "RxBw::HZ_200000"; + + case SX1278::RxBw::HZ_250000: + return "RxBw::HZ_250000"; + + default: + return "<unknown>"; + } +} + +void printConfig(SX1278::Config config) +{ + printf("config.freq_rf = %d\n", config.freq_rf); + printf("config.freq_dev = %d\n", config.freq_dev); + printf("config.bitrate = %d\n", config.bitrate); + printf("config.rx_bw = %s\n", stringFromRxBw(config.rx_bw)); + printf("config.afc_bw = %s\n", stringFromRxBw(config.afc_bw)); + printf("config.ocp = %d\n", config.ocp); + printf("config.power = %d\n", config.power); +} diff --git a/src/tests/drivers/sx1278/test-sx1278-core.h b/src/tests/drivers/sx1278/test-sx1278-core.h new file mode 100644 index 0000000000000000000000000000000000000000..7a5d224a4f887d899389c0dddbfed8788e6c33d5 --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-core.h @@ -0,0 +1,30 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/SX1278/SX1278.h> + +const char *stringFromErr(Boardcore::SX1278::Error err); +const char *stringFromRxBw(Boardcore::SX1278::RxBw rx_bw); + +void printConfig(Boardcore::SX1278::Config config); diff --git a/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp b/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f7b37f60199d64283bee1a24a6b0267ac28de3bb --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp @@ -0,0 +1,162 @@ +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Authors: Alvise de'Faveri Tron, Nuno Barcellos, Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <drivers/SX1278/SX1278.h> +#include <drivers/interrupt/external_interrupts.h> + +#include "test-sx1278-core.h" + +// Ignore warnings, as we don't want to change third party generated files to +// fix them +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" +#include <mavlink_lib/lynx/mavlink.h> +#pragma GCC diagnostic pop + +#include <radio/MavlinkDriver/MavlinkDriver.h> + +using namespace Boardcore; +using namespace miosix; + +static const unsigned int queue_len = 10; +static const unsigned int packet_size = 256; +static const unsigned int silence_after_send = 250; +static const unsigned int max_pkt_age = 1000; +static const unsigned int ping_period = 1000; + +// Mavlink out buffer with 10 packets, 256 bytes each. +using Mav = MavlinkDriver<packet_size, queue_len>; + +Mav* channel; + +SPIBus bus(SPI3); + +GpioPin sck(GPIOC_BASE, 10); +GpioPin miso(GPIOC_BASE, 11); +GpioPin mosi(GPIOC_BASE, 12); +GpioPin cs(GPIOA_BASE, 1); +GpioPin dio(GPIOC_BASE, 15); + +SX1278* sx1278 = nullptr; + +void __attribute__((used)) EXTI15_IRQHandlerImpl() +{ + if (sx1278) + sx1278->handleDioIRQ(); +} + +/// Initialize stm32f407g board. +void initBoard() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable SPI3 + RCC->APB1ENR |= RCC_APB1ENR_SPI3EN; + RCC_SYNC(); + + // Setup SPI pins + sck.mode(miosix::Mode::ALTERNATE); + sck.alternateFunction(6); + miso.mode(miosix::Mode::ALTERNATE); + miso.alternateFunction(6); + mosi.mode(miosix::Mode::ALTERNATE); + mosi.alternateFunction(6); + + cs.mode(miosix::Mode::OUTPUT); + dio.mode(miosix::Mode::INPUT); + } + + cs.high(); + enableExternalInterrupt(dio.getPort(), dio.getNumber(), + InterruptTrigger::RISING_EDGE); +} + +/** + * @brief Receive function: print the received message id and send an ACK. + */ +static void onReceive(Mav* channel, const mavlink_message_t& msg) +{ + printf("[TmtcTest] Received message, id: %d! Sending ack\n", msg.msgid); + + // Prepare ack messages + mavlink_message_t ackMsg; + mavlink_msg_ack_tm_pack(1, 1, &ackMsg, msg.msgid, msg.seq); + + // Send the ack back to the sender + bool ackSent = channel->enqueueMsg(ackMsg); + + if (!ackSent) + printf("[Receiver] Could not enqueue ack\n"); +} + +/** + * @brief This test enqueues a ping message every second and replies to every + * received message with an ACK. + */ +int main() +{ + initBoard(); + + SX1278::Config config; + SX1278::Error err; + + sx1278 = new SX1278(bus, cs); + + printf("\n[sx1278] Configuring sx1278...\n"); + printConfig(config); + if ((err = sx1278->init(config)) != SX1278::Error::NONE) + { + printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err)); + return -1; + } + + channel = new Mav(sx1278, &onReceive, silence_after_send, max_pkt_age); + + channel->start(); + + // Send function: enqueue a ping every second + while (1) + { + printf("[TmtcTest] Enqueueing ping\n"); + + // Create a Mavlink message + mavlink_message_t pingMsg; + mavlink_msg_ping_tc_pack(1, 1, &pingMsg, miosix::getTick()); + + // Send the message + bool ackSent = channel->enqueueMsg(pingMsg); + + if (!ackSent) + printf("[TmtcTest] Could not enqueue ping\n"); + + // LED blink + ledOn(); + miosix::delayMs(200); + ledOff(); + + miosix::Thread::sleep(ping_period); + } + + return 0; +} diff --git a/src/tests/drivers/sx1278/test-sx1278-rx.cpp b/src/tests/drivers/sx1278/test-sx1278-rx.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a32b475743e974bc6d64689c58e61692539020eb --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-rx.cpp @@ -0,0 +1,24 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#define DISABLE_TX +#include "test-sx1278-bench.cpp" diff --git a/src/tests/drivers/sx1278/test-sx1278-serial.cpp b/src/tests/drivers/sx1278/test-sx1278-serial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..062eb72080c3854a538504825574b32562a88986 --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-serial.cpp @@ -0,0 +1,134 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <drivers/SX1278/SX1278.h> +#include <drivers/interrupt/external_interrupts.h> +#include <filesystem/console/console_device.h> + +#include <thread> + +#include "test-sx1278-core.h" + +using namespace Boardcore; +using namespace miosix; + +SPIBus bus(SPI3); + +GpioPin sck(GPIOC_BASE, 10); +GpioPin miso(GPIOC_BASE, 11); +GpioPin mosi(GPIOC_BASE, 12); +GpioPin cs(GPIOA_BASE, 1); +GpioPin dio(GPIOC_BASE, 15); + +SX1278* sx1278 = nullptr; + +void __attribute__((used)) EXTI15_IRQHandlerImpl() +{ + if (sx1278) + sx1278->handleDioIRQ(); +} + +/// Initialize stm32f407g board. +void initBoard() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable SPI3 + RCC->APB1ENR |= RCC_APB1ENR_SPI3EN; + RCC_SYNC(); + + // Setup SPI pins + sck.mode(miosix::Mode::ALTERNATE); + sck.alternateFunction(6); + miso.mode(miosix::Mode::ALTERNATE); + miso.alternateFunction(6); + mosi.mode(miosix::Mode::ALTERNATE); + mosi.alternateFunction(6); + + cs.mode(miosix::Mode::OUTPUT); + dio.mode(miosix::Mode::INPUT); + } + + cs.high(); + enableExternalInterrupt(dio.getPort(), dio.getNumber(), + InterruptTrigger::RISING_EDGE); +} + +void recvLoop() +{ + uint8_t msg[256]; + auto console = miosix::DefaultConsole::instance().get(); + + while (1) + { + int len = sx1278->receive(msg, sizeof(msg)); + if (len > 0) + { + console->writeBlock(msg, len, 0); + // TODO: Flushing? + } + } +} + +void sendLoop() +{ + uint8_t msg[256]; + auto console = miosix::DefaultConsole::instance().get(); + + while (1) + { + int len = console->readBlock(msg, sizeof(msg), 0); + if (len > 0) + { + sx1278->send(msg, len); + } + } +} + +int main() +{ + initBoard(); + + SX1278::Config config; + SX1278::Error err; + + sx1278 = new SX1278(bus, cs); + + printf("\n[sx1278] Configuring sx1278...\n"); + printConfig(config); + if ((err = sx1278->init(config)) != SX1278::Error::NONE) + { + printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err)); + return -1; + } + + std::thread recv([]() { recvLoop(); }); + std::thread send([]() { sendLoop(); }); + + printf("\n[sx1278] Initialization complete!\n"); + + while (1) + miosix::Thread::wait(); + + return 0; +} diff --git a/src/tests/drivers/sx1278/test-sx1278-tx.cpp b/src/tests/drivers/sx1278/test-sx1278-tx.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b332d3accf06a9d21f0c54d30e7ef66612f20db3 --- /dev/null +++ b/src/tests/drivers/sx1278/test-sx1278-tx.cpp @@ -0,0 +1,24 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#define DISABLE_RX +#include "test-sx1278-bench.cpp" diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubloxgps.cpp index 1bf036893f5cd3463252d1b6d4a06a3da582b75f..0c611daf4e7ffc2ccabcc56da70d989496e1d45f 100644 --- a/src/tests/sensors/test-ubloxgps.cpp +++ b/src/tests/sensors/test-ubloxgps.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda + * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -21,87 +21,69 @@ */ #include <drivers/timer/TimestampTimer.h> +#include <miosix.h> #include <sensors/UbloxGPS/UbloxGPS.h> #include <utils/Debug.h> -using namespace miosix; +#include <cstdio> + using namespace Boardcore; +using namespace miosix; + +#define RATE 4 int main() { - static constexpr uint8_t SAMPLE_RATE = 4; - - PrintLogger logger = Logging::getLogger("test-ubloxgps"); - -#if defined(USE_SPI) - SPIBus bus(SPI1); - GpioPin spiSck(GPIOA_BASE, 5); - GpioPin spiMiso(GPIOA_BASE, 6); - GpioPin spiMosi(GPIOA_BASE, 7); - GpioPin cs(GPIOA_BASE, 3); - - spiSck.mode(Mode::ALTERNATE); - spiSck.alternateFunction(5); - spiMiso.mode(Mode::ALTERNATE); - spiMiso.alternateFunction(5); - spiMosi.mode(miosix::Mode::ALTERNATE); - spiMosi.alternateFunction(5); - cs.mode(Mode::OUTPUT); - cs.high(); - - UbloxGPSSPI sensor{bus, cs, UbloxGPSSPI::getDefaultSPIConfig(), - SAMPLE_RATE}; -#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X) - // Keep GPS baud SAMPLE_RATE at default for easier testing - UbloxGPSSerial sensor{2, "gps", 921600, 38400, SAMPLE_RATE}; -#else - GpioPin tx(GPIOB_BASE, 6); - GpioPin rx(GPIOB_BASE, 7); + (void)TimestampTimer::getInstance(); - tx.mode(miosix::Mode::ALTERNATE); - rx.mode(miosix::Mode::ALTERNATE); + printf("Welcome to the ublox test\n"); - tx.alternateFunction(7); - rx.alternateFunction(7); + // Keep GPS baud rate at default for easier testing + UbloxGPS gps(921600, RATE, 2, "gps", 38400); + UbloxGPSData dataGPS; + printf("Gps allocated\n"); - UbloxGPSSerial sensor{91600, SAMPLE_RATE, 38400, 1, "gps"}; -#endif - - LOG_INFO(logger, "Initializing sensor...\n"); - - if (!sensor.init()) + // Init the gps + if (gps.init()) { - LOG_ERR(logger, "Initialization failed!\n"); - return -1; + printf("Successful gps initialization\n"); + } + else + { + printf("Failed gps initialization\n"); } - LOG_INFO(logger, "Performing self-test...\n"); - - if (!sensor.selfTest()) + // Perform the selftest + if (gps.selfTest()) + { + printf("Successful gps selftest\n"); + } + else { - LOG_ERR(logger, "Self-test failed! (code: %d)\n", - sensor.getLastError()); - return -1; + printf("Failed gps selftest\n"); } - // Start the sensor thread - LOG_INFO(logger, "Starting sensor...\n"); - sensor.start(); + // Start the gps thread + gps.start(); + printf("Gps started\n"); while (true) { - long long lastTick = miosix::getTick(); + // Give time to the thread + Thread::sleep(1000 / RATE); - sensor.sample(); - GPSData sample __attribute__((unused)) = sensor.getLastSample(); + // Sample + gps.sample(); + dataGPS = gps.getLastSample(); + // Print out the latest sample TRACE( - "timestamp: %4.3f, fix: %01d, lat: %f, lon: %f, height: %4.1f, " - "nsat: %2d, speed: %3.2f, velN: %3.2f, velE: %3.2f, track %3.1f\n", - (float)sample.gpsTimestamp / 1000000, sample.fix, sample.latitude, - sample.longitude, sample.height, sample.satellites, sample.speed, - sample.velocityNorth, sample.velocityEast, sample.track); - - Thread::sleepUntil(lastTick + 1000 / SAMPLE_RATE); // Sample period + "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f " + "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f " + "track %3.1f\n", + (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix, + dataGPS.latitude, dataGPS.longitude, dataGPS.height, + dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth, + dataGPS.velocityEast, dataGPS.track); } }