diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c07b46355e37135c849ff6faa1c81f512fb09d7..53273bb8481f7b3ac5c2385fd4505fd29a0f90fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -274,7 +274,7 @@ 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) +sbs_target(test-sx1278-serial stm32f429zi_stm32f4discovery) #-----------------------------------------------------------------------------# # Tests - Sensors # diff --git a/src/shared/drivers/timer/TimestampTimer.h b/src/shared/drivers/timer/TimestampTimer.h index 3f54fb1ad6959892353230fce24cc75a5e813f5c..8b2bc6f2f445bc0a21f3fd32bd754be102759e25 100644 --- a/src/shared/drivers/timer/TimestampTimer.h +++ b/src/shared/drivers/timer/TimestampTimer.h @@ -101,7 +101,7 @@ private: inline uint64_t TimestampTimer::getTimestamp() { #ifdef COMPILE_FOR_HOST - return 0; + return time(NULL); #else // With a timer frequency of 250KHz, the conversion from timer ticks to // microseconds only take a 2 byte shift (x4) diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h index fee9e9b5d0df1b9bb3372b01a2c419b4aa34cd46..79a63e0cc9e6b940ebac1a3acd1d74c14338efa8 100644 --- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h +++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h @@ -250,7 +250,8 @@ bool MavlinkDriver<PktLength, OutQueueSize>::enqueueMsg( const mavlink_message_t& msg) { // Convert mavlink message to char array - uint8_t msgtempBuf[PktLength]; + // Use fixed buffer size to avoid overflows + uint8_t msgtempBuf[256]; int msgLen = mavlink_msg_to_send_buffer(msgtempBuf, &msg); // Append message to the queue @@ -347,11 +348,12 @@ void MavlinkDriver<PktLength, OutQueueSize>::runSender() // Get the first packet in the queue, without removing it pkt = outQueue.get(); - uint64_t age = (uint64_t)miosix::getTick() - pkt.timestamp(); // If the packet is ready or too old, send it + uint64_t age = TimestampTimer::getInstance().getTimestamp() - + pkt.getTimestamp(); if (pkt.isReady() || age >= outBufferMaxAge) { - outQueue.pop(); // remove from queue + outQueue.pop(); // Remove the packet from queue LOG_DEBUG(logger, "Sending packet. Size: {} (age: {})", pkt.size(), age); diff --git a/src/shared/radio/SX1278/SX1278.cpp b/src/shared/radio/SX1278/SX1278.cpp index 52a91f3ba06520aa4b18974468319a22256b6774..bed1ae549d6daf57f3b77e8f6f0787c3efa9b5d9 100644 --- a/src/shared/radio/SX1278/SX1278.cpp +++ b/src/shared/radio/SX1278/SX1278.cpp @@ -31,6 +31,8 @@ namespace Boardcore using namespace SX1278Defs; +long long now() { return miosix::getTick() * 1000 / miosix::TICK_FREQ; } + // Default values for registers constexpr uint8_t REG_OP_MODE_DEFAULT = RegOpMode::LONG_RANGE_MODE_FSK | RegOpMode::MODULATION_TYPE_FSK | @@ -247,10 +249,14 @@ ssize_t SX1278::receive(uint8_t *pkt, size_t max_len) bool SX1278::send(uint8_t *pkt, size_t len) { - // Packets longer than 255 are not supported - if (len > 255) + // Packets longer than FIFO_LEN (-1 for the len byte) are not supported + if (len > SX1278Defs::FIFO_LEN - 1) return false; + // This shouldn't be needed, but for some reason the device "lies" about + // being ready, so lock up if we are going too fast + rateLimitTx(); + bus_mgr.lock(SX1278BusManager::Mode::MODE_TX); // Wait for TX ready @@ -267,11 +273,24 @@ bool SX1278::send(uint8_t *pkt, size_t len) // Wait for packet sent bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT); bus_mgr.unlock(); + + last_tx = now(); return true; } void SX1278::handleDioIRQ() { bus_mgr.handleDioIRQ(); } +void SX1278::rateLimitTx() +{ + const long long RATE_LIMIT = 2; + + long long delta = now() - last_tx; + if (delta <= RATE_LIMIT) + { + miosix::Thread::sleep(RATE_LIMIT - delta); + } +} + uint8_t SX1278::getVersion() { SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); diff --git a/src/shared/radio/SX1278/SX1278.h b/src/shared/radio/SX1278/SX1278.h index 6fb4c355d36f81331e7299e6a291e58c737e4156..3d89a9f9494a95ddcae4871f84a8c21132571066 100644 --- a/src/shared/radio/SX1278/SX1278.h +++ b/src/shared/radio/SX1278/SX1278.h @@ -228,6 +228,8 @@ public: public: using Mode = SX1278BusManager::Mode; + void rateLimitTx(); + void setBitrate(int bitrate); void setFreqDev(int freq_dev); void setFreqRF(int freq_rf); @@ -240,6 +242,7 @@ public: void waitForIrq(uint16_t mask); + long long last_tx = 0; SX1278BusManager bus_mgr; PrintLogger logger = Logging::getLogger("sx1278"); }; diff --git a/src/shared/radio/SX1278/SX1278Defs.h b/src/shared/radio/SX1278/SX1278Defs.h index cd50d09a8c3294659f1c1186f2ad1fd0512937c6..06954099644e8a7b6d6b6566e0ad7d2ef1118f3b 100644 --- a/src/shared/radio/SX1278/SX1278Defs.h +++ b/src/shared/radio/SX1278/SX1278Defs.h @@ -33,6 +33,11 @@ namespace Boardcore namespace SX1278Defs { +/** + * @brief Length of the internal FIFO + */ +constexpr int FIFO_LEN = 64; + /** * @brief Main oscillator frequency (Hz) */ diff --git a/src/shared/sensors/BME280/BME280.cpp b/src/shared/sensors/BME280/BME280.cpp index 53f2b2160f01eaee81a6151f4d4fd931fd4ef362..c8fd6e5400f5209f54a43a2897d990a0505d585a 100644 --- a/src/shared/sensors/BME280/BME280.cpp +++ b/src/shared/sensors/BME280/BME280.cpp @@ -66,21 +66,47 @@ bool BME280::init() loadCompensationParameters(); - // Read once the temperature to compute fineTemperature - setConfiguration(BME280_CONFIG_TEMP_SINGLE); - miosix::Thread::sleep( - calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE)); - readTemperature(); + // Set the configuration 10 times to be sure + for (int i = 0; i < 10; i++) + { + // Read once the temperature to compute fineTemperature + setConfiguration(BME280_CONFIG_TEMP_SINGLE); + miosix::Thread::sleep( + calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE)); + readTemperature(); - setConfiguration(); + setConfiguration(); + } + + // Set a sleep time to allow the sensor to change internally the data + miosix::Thread::sleep(100); - BME280Config readBackConfig = readConfiguration(); + // I create the config state which represents the logic or of all the + // 5 readConfiguration controls (We perform 5 checks to avoid that the + // sensor is busy implicating in wrong responses) + bool readConfigResult = false; + BME280Config readBackConfig; + + for (int i = 0; i < 10; i++) + { + readBackConfig = readConfiguration(); + // Check if the configration on the device matches ours + if (config.bytes.ctrlHumidity == readBackConfig.bytes.ctrlHumidity && + config.bytes.ctrlPressureAndTemperature == + readBackConfig.bytes.ctrlPressureAndTemperature && + config.bytes.config == readBackConfig.bytes.config) + { + readConfigResult = true; + break; + } + + // After the check i sleep 100 milliseconds + miosix::Thread::sleep(20); + } - // Check if the configration on the device matches ours - if (config.bytes.ctrlHumidity != readBackConfig.bytes.ctrlHumidity || - config.bytes.ctrlPressureAndTemperature != - readBackConfig.bytes.ctrlPressureAndTemperature || - config.bytes.config != readBackConfig.bytes.config) + // If after the 5 iterations the sensor didn't report the configuration + // set I can report the init error + if (!readConfigResult) { LOG_ERR(logger, "Device configuration incorrect, setup failed"); diff --git a/src/shared/sensors/SensorManager.cpp b/src/shared/sensors/SensorManager.cpp index c877dc04a378f68c6479fb8e9f53520a1a515de6..53ac4adc512f65aea68ca0e313ee6b51b3984870 100644 --- a/src/shared/sensors/SensorManager.cpp +++ b/src/shared/sensors/SensorManager.cpp @@ -50,7 +50,12 @@ SensorManager::~SensorManager() delete scheduler; } -bool SensorManager::start() { return initResult ? scheduler->start() : false; } +bool SensorManager::start() +{ + if (customScheduler) + scheduler->start(); + return initResult; +} void SensorManager::stop() { scheduler->stop(); } diff --git a/src/shared/utils/collections/CircularBuffer.h b/src/shared/utils/collections/CircularBuffer.h index fa6a2e58286996793ee9c7b949b26da61e4cb1db..6182bdcc5b7967af9b90570dada7d62c635438ce 100644 --- a/src/shared/utils/collections/CircularBuffer.h +++ b/src/shared/utils/collections/CircularBuffer.h @@ -32,7 +32,7 @@ namespace Boardcore { /** - * Implementation of an non-synchronized circular buffer + * Implementation of an non-synchronized circular buffer. */ template <typename T, unsigned int Size> class CircularBuffer @@ -41,21 +41,25 @@ class CircularBuffer public: CircularBuffer() {} + virtual ~CircularBuffer() {} /** - * Puts a copy of the element in the buffer - * @param elem element + * @brief Puts a copy of the element in the buffer. + * + * @param elem Element to be added to the queue. + * @return The element added. */ virtual T& put(const T& elem) { buffer[writePtr] = elem; T& added = buffer[writePtr]; + // Advance the read pointer if the two pointers match if (!empty && writePtr == readPtr) - { readPtr = (readPtr + 1) % Size; - } + + // Advance the write pointer writePtr = (writePtr + 1) % Size; empty = false; @@ -64,20 +68,19 @@ public: } /** - * Gets an element from the buffer, without removing it - * Index starts from the oldest element in the buffer: get(0) returns the - * same element as get() + * @brief Gets an element from the buffer, without removing it. * - * @warning Remember to catch the exception! - * @throw range_error if index >= count() + * Index starts from the oldest element in the buffer. + * get() returns the first element. * - * @param i Index of the elemnt to get, starting from the oldest - * - * @return the element + * @warning Remember to catch the exception! + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - virtual T& get(unsigned int i) + virtual T& get(unsigned int i = 0) { - if (i < CircularBuffer<T, Size>::count()) + if (i < count()) { int ptr = (readPtr + i) % Size; return buffer[ptr]; @@ -89,33 +92,18 @@ public: /** * @brief Returns the last element added in the buffer. * - * @throw range_error if buffer is empty * @warning Remember to catch the exception! - * @return the element + * @throw range_error if buffer is empty. + * @return The element. */ virtual T& last() { return get(count() - 1); } /** - * Gets the first element from the buffer, without removing it - * @throw range_error if buffer is empty - * @warning Remember to catch the exception! - * @return the element - */ - virtual T& get() - { - if (!empty) - { - return buffer[readPtr]; - } - else - throw range_error("CircularBuffer is empty!"); - } - - /** - * Pops the first element in the buffer. - * @throw range_error if buffer is empty + * @brief Pops the first element in the buffer. + * * @warning Remember to catch the exception! - * @return the element that has been popped + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ virtual const T& pop() { @@ -133,8 +121,9 @@ public: } /** - * Counts the elements in the buffer - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @return Number of elements in the buffer. */ virtual size_t count() const { @@ -157,16 +146,19 @@ public: return CircularBuffer<T, Size>::count() == Size; } /** - * Returns the maximum number of elements that can be stored in the buffer - * @return buffer size + * @brief Returns the maximum number of elements that can be stored in the + * buffer. + * + * @return Buffer size. */ size_t getSize() const { return Size; } protected: T buffer[Size]; - size_t writePtr = 0, readPtr = 0; - bool empty = true; + size_t writePtr = 0; + size_t readPtr = 0; + bool empty = true; }; } // namespace Boardcore diff --git a/src/shared/utils/collections/IRQCircularBuffer.h b/src/shared/utils/collections/IRQCircularBuffer.h index a6a02e63664400c7b5efc1fc9af1287613862598..e27d15e2a37652df6e822d29fd8d3ff0cd3af104 100644 --- a/src/shared/utils/collections/IRQCircularBuffer.h +++ b/src/shared/utils/collections/IRQCircularBuffer.h @@ -34,7 +34,8 @@ namespace Boardcore { /** - * Implementation of a synchronized circular buffer + * Implementation of a synchronized circular buffer that can be used inside + * interrupt service routines. */ template <typename T, unsigned int Size> class IRQCircularBuffer : public CircularBuffer<T, Size> @@ -44,8 +45,7 @@ class IRQCircularBuffer : public CircularBuffer<T, Size> public: /** - * Puts a copy of the element in the buffer - * @param elem element + * @brief Puts a copy of the element in the buffer. */ T& put(const T& elem) override { @@ -55,37 +55,28 @@ public: } /** - * Gets the first element from the buffer, without removing it - * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty - */ - T& get() override - { - FastInterruptDisableLock d; - return Super::get(); - } - - /** - * Gets an element from the buffer, without removing it - * Index starts at the element returned by get() or pop(): get(0) is - * the same as get() + * @brief Gets an element from the buffer, without removing it. + * + * Index starts from the oldest element in the buffer. + * get() returns the first element. * * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - T& get(unsigned int i) override + T& get(unsigned int i = 0) override { FastInterruptDisableLock d; return Super::get(i); } /** - * Pops the first element in the buffer. + * @brief Pops the first element in the buffer. + * * @warning Remember to catch the exception! - * @return the element that has been popped - * @throws range_error if buffer is empty + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ const T& pop() override { @@ -94,8 +85,9 @@ public: } /** - * Counts the elements in the buffer - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @return Number of elements in the buffer. */ size_t count() const override { @@ -116,9 +108,9 @@ public: } /** - * Puts a copy of the element in the buffer - * Only to be called inside an ISR or with interrupts disabled - * @param elem element + * @brief Puts a copy of the element in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. */ T& IRQput(const T& elem) { @@ -127,8 +119,10 @@ public: } /** - * Puts a copy of the element in the buffer - * Only to be called inside an ISR or with interrupts disabled + * @brief Puts a copy of the element in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * * @param elem element * @param hppw Set to true if the woken thread is higher priority than the * current one, unchanged otherwise @@ -137,68 +131,59 @@ public: { if (waiting && (waiting->IRQgetPriority() > Thread::IRQgetCurrentThread()->IRQgetPriority())) - { hppw = true; - } IRQwakeWaitingThread(); return Super::put(elem); } /** - * Gets the first element from the buffer, without removing it - * Only to be called inside an ISR or with interrupts disabled - * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty - */ - T& IRQget() { return Super::get(); } - - /** - * Gets an element from the buffer, without removing it - * Index starts at the element returned by get() or pop(): get(0) is - * the same as get() - * @warning Only to be called inside an ISR or with interrupts disabled + * @brief Gets an element from the buffer, without removing it. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * + * Index starts from the oldest element in the buffer. + * get() returns the first element. + * * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - T& IRQget(unsigned int i) { return Super::get(i); } + T& IRQget(unsigned int i = 0) { return Super::get(i); } /** - * Pops the first element in the buffer. - * @warning Only to be called inside an ISR or with interrupts disabled + * @brief Pops the first element in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * * @warning Remember to catch the exception! - * @return the element that has been popped - * @throws range_error if buffer is empty + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ const T& IRQpop() { return Super::pop(); } /** - * Counts the elements in the buffer - * @warning Only to be called inside an ISR or with interrupts disabled - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * + * @return Number of elements in the buffer. */ size_t IRQcount() const { return Super::count(); } /** - * @brief Returns true if the buffer is empty - * - * @warning Only to be called inside an ISR or with interrupts disabled - * @return empty or not + * @warning Only to be called inside an ISR or with interrupts disabled. */ bool IRQisEmpty() const { return Super::isEmpty(); } /** - * @brief Returns true if the buffer is full - * - * @warning Only to be called inside an ISR or with interrupts disabled - * @return buffer full or not + * @warning Only to be called inside an ISR or with interrupts disabled. */ bool IRQisFull() const { return Super::isFull(); } /** - * @brief Waits until the buffer contains at least one element + * @brief Waits until the buffer contains at least one element. */ void waitUntilNotEmpty() { @@ -228,4 +213,5 @@ private: Thread* waiting = nullptr; }; -} // namespace Boardcore \ No newline at end of file + +} // namespace Boardcore diff --git a/src/shared/utils/collections/SyncCircularBuffer.h b/src/shared/utils/collections/SyncCircularBuffer.h index 9e05227df79823f100b0ff915530c451fd0862b8..cab438fbb629c00aa98122ed9b3512deca329e57 100644 --- a/src/shared/utils/collections/SyncCircularBuffer.h +++ b/src/shared/utils/collections/SyncCircularBuffer.h @@ -55,37 +55,28 @@ public: } /** - * Gets the first element from the buffer, without removing it - * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty - */ - T& get() override - { - Lock<FastMutex> l(mutex); - return Super::get(); - } - - /** - * Gets an element from the buffer, without removing it - * Index starts at the element returned by get() or pop(): get(0) is - * the same as get() + * @brief Gets an element from the buffer, without removing it. + * + * Index starts from the oldest element in the buffer. + * get() returns the first element. * * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - T& get(unsigned int i) override + T& get(unsigned int i = 0) override { Lock<FastMutex> l(mutex); return Super::get(i); } /** - * Pops the first element in the buffer. + * @brief Pops the first element in the buffer. + * * @warning Remember to catch the exception! - * @return the element that has been popped - * @throws range_error if buffer is empty + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ const T& pop() override { @@ -94,8 +85,9 @@ public: } /** - * Counts the elements in the buffer - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @return Number of elements in the buffer. */ size_t count() const override { @@ -116,7 +108,7 @@ public: } /** - * @brief Waits until the buffer contains at least one element + * @brief Waits until the buffer contains at least one element. */ void waitUntilNotEmpty() { diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h index ff5e7b360a3f612cf73feae05e0283267d96108f..3d646d04da08bd600d663c951871e0960ee9b85d 100644 --- a/src/shared/utils/collections/SyncPacketQueue.h +++ b/src/shared/utils/collections/SyncPacketQueue.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2019 Skyward Experimental Rocketry - * Author: Alvise de'Faveri Tron +/* Copyright (c) 2019-2022 Skyward Experimental Rocketry + * Author: Alvise de'Faveri Tron, 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 @@ -22,6 +22,7 @@ #pragma once +#include <drivers/timer/TimestampTimer.h> #include <miosix.h> #include <utils/Debug.h> @@ -46,10 +47,9 @@ namespace Boardcore * @brief The Packet class is used for packing together messages with variable * lengths into a fixed size packet. Useful for telemetry. * - * The buffer can only be appended, read or flushed. The caller can also mark - * the packet as ready to be sent. + * Data can only be appended to the payload. The packet can be marked ready. * - * @tparam len Maximum length for the packet. + * @tparam len Packet's payload length. */ template <unsigned int len> class Packet @@ -59,7 +59,7 @@ public: /** * @brief Reserves a fixed length for the packet. */ - Packet() : msgCounter(0), ts(0), ready(false) { content.reserve(len); }; + Packet() { content.reserve(len); }; /** * @brief Clears the buffer. @@ -67,24 +67,25 @@ public: ~Packet() { content.clear(); }; /** - * @brief Try to append a given message to the packet. + * @brief Append a given message to the packet. * - * If it's the first message, also set the timestamp. + * If the message can't fit inside the remaining space only the first bytes + * are copied. * * @param msg The message to be appended. - * @param msgLen Length of msg. - * @return true if the message was appended correctly. - * @return false if there isn't enough space for the message. + * @param msgLen Length of the message. + * @return How many bytes were actually appended. */ - bool tryAppend(const uint8_t* msg, const size_t msgLen); + size_t append(const uint8_t* msg, size_t msgLen); /** - * @brief Mark the packet as ready to be sent. + * @brief Mark the packet as ready. */ inline void markAsReady() { ready = true; } /** - * @brief Copies the content of the buffer at a given address. + * @brief Copies the content of the payload at the given address. + * * @param buf Where to copy the content. * @return How many bytes where copied, i.e. the size of the packet. */ @@ -111,12 +112,12 @@ public: inline bool isReady() const { return ready; } /** - * @return The timestamp of the first successful call to tryAppend(). + * @return The timestamp of the first successful call to append(). */ - inline uint64_t timestamp() const { return ts; } + inline uint64_t getTimestamp() const { return timestamp; } /** - * @return The occupied portion of the buffer (bytes). + * @return The occupied portion of the buffer [bytes]. */ inline size_t size() const { return content.size(); } @@ -126,7 +127,7 @@ public: inline unsigned int maxSize() const { return len; } /** - * @return How many times the tryAppend() function has been called + * @return How many times the append() function has been called * successfully. */ inline unsigned int getMsgCount() const { return msgCounter; } @@ -134,7 +135,7 @@ public: /** * @brief Print information about this object. * - * @param os For example, std::cout + * @param os For example, std::cout. */ void print(std::ostream& os) const; @@ -142,32 +143,29 @@ public: std::vector<uint8_t> content; private: - unsigned int msgCounter; - uint64_t ts; - bool ready; + unsigned int msgCounter = 0; + uint64_t timestamp = 0; + bool ready = false; }; template <unsigned int len> -bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen) +size_t Packet<len>::append(const uint8_t* msg, size_t msgLen) { - if (msgLen == 0 || content.size() + msgLen > len) - { - return false; - } - else + size_t remaining = len - content.size(); + msgLen = std::min(remaining, msgLen); + + if (msgLen != 0) { // Set the packet's timestamp when the first message is inserted if (content.size() == 0) - { - ts = miosix::getTick(); - } + timestamp = TimestampTimer::getInstance().getTimestamp(); // Append the message to the packet content.insert(content.end(), msg, msg + msgLen); msgCounter++; - - return true; } + + return msgLen; } template <unsigned int len> @@ -175,7 +173,7 @@ void Packet<len>::clear() { content.clear(); msgCounter = 0; - ts = 0; + timestamp = 0; ready = false; } @@ -189,194 +187,167 @@ size_t Packet<len>::dump(uint8_t* buf) template <unsigned int len> void Packet<len>::print(std::ostream& os) const { - os << "timestamp=" << ts << ", ready=" << ready + os << "timestamp=" << timestamp << ", ready=" << ready << ", size=" << content.size() << ", msgCounter=" << msgCounter << ", content= "; for (auto const& i : content) - { os << i; - } os << '\n'; } -/****************************************************************************** - * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets. The difference - * is that you pop() Packets but you append() bytes. The bytes will be appended - * to the first available packet. This class is suitable for synchronization - * between two threads. +/** + * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets. * - * @tparam pktLen Maximum length of each packet. (bytes) - * @tparam pktNum Total number of packets. - ******************************************************************************/ + * The difference is that you pop() Packets but you append() bytes. The bytes + * will be appended to the first available packet and the next ones. + * This class is suitable for synchronization between two threads. + * + * @tparam pktLen Maximum length of each packet [bytes]. + * @tparam pktNum Total number of packets. + */ template <unsigned int pktLen, unsigned int pktNum> class SyncPacketQueue { - using Pkt = Packet<pktLen>; - public: /** - * @brief Try to append a given message to the last packet. If there isn't - * enough space, the packet is marked as ready and the message is appended - * to the next packet. If there are no more available packets, the oldest - * one is overwritten. + * @brief Try to append a given message to the packets queue. * - * @param msg the message to be appended - * @param msgLen length of msg - * @return true if the message was appended correctly - * @return false if there isn't enough space for the message + * The message is appended to the last packet and if the space isn't enough, + * it is divided into successive packets. If there are no more available + * packets, the oldest one is overwritten. + * + * The message isn't added to the queue if there is no space considering all + * the queue packets. + * + * @param msg The message to be appended. + * @param msgLen Length of the message [bytes]. + * @return True if the message was appended. */ - int put(uint8_t* msg, size_t msgLen) + bool put(uint8_t* msg, size_t msgLen) { - int dropped = 0; + // Check if the message is empty + if (msgLen == 0) + return false; - if (msgLen == 0 || msgLen > pktLen) - { - return -1; - } + // Check if the queue can hold the packet + if (msgLen > pktLen * pktNum) + return false; { + // Lock the mutex on the buffer Lock<FastMutex> l(mutex); + // Add an element if there isn't any if (buffer.count() == 0) - { - buffer.put(Pkt{}); - } + buffer.put({}); - Pkt& last = buffer.last(); + // Write all the packet + while (msgLen > 0) + { + // If the last packet is ready append a new one + if (buffer.last().isReady()) + buffer.put({}); - bool added = false; + // Append what data is possible to the last packet + size_t appendedLength = buffer.last().append(msg, msgLen); - // Add to the current packet only if it isn't already ready - if (!last.isReady()) - { - added = last.tryAppend(msg, msgLen); - } + // If the packet is full mark it as ready + if (buffer.last().isFull()) + buffer.last().markAsReady(); - // If already ready or cannot fit the new data, add to a new packet - if (!added) - { - // Mark the packet as ready (in the case it wasn't already) - last.markAsReady(); - - if (buffer.isFull()) - { - // We have dropped a packet - ++dropped; - } - // Add a new packet and fill that instead - Pkt& newpkt = buffer.put(Pkt{}); - - if (!newpkt.tryAppend(msg, msgLen)) - { - TRACE("Packet is too big!\n"); - return -1; - } + // Go forward in the data + msgLen -= appendedLength; + msg += appendedLength; } + } - // Mark as ready if the packet is full - if (buffer.last().isFull()) - { - buffer.last().markAsReady(); - } + // Wake all waiting threads + condVerNotEmpty.broadcast(); - cvNotempty.broadcast(); - return dropped; - } + return true; } /** - * @return a copy of the oldest packet, without removing it from the queue. + * @return The oldest packet, without removing it from the queue. */ - const Pkt& get() + const Packet<pktLen>& get() { Lock<FastMutex> l(mutex); return buffer.get(); } /** - * @return the oldest packet, removing it from the queue. + * @return The oldest packet, removing it from the queue. */ - const Pkt& pop() + const Packet<pktLen>& pop() { Lock<FastMutex> l(mutex); return buffer.pop(); } /** - * @return true if all the packets have been marked as ready. + * @return True if all the packets have been marked as ready. */ bool isFull() { Lock<FastMutex> l(mutex); if (buffer.count() > 0) - { return buffer.isFull() && buffer.last().isReady(); - } else - { return false; - } } /** - * @return true if all the packets are completely empty. + * @return True if all the packets are completely empty. */ bool isEmpty() { Lock<FastMutex> l(mutex); - return buffer.isEmpty(); } /** * @brief Blocks the calling thread until the queue is not empty. + * * Returns immediately if already not empty. */ void waitUntilNotEmpty() { Lock<FastMutex> l(mutex); if (buffer.isEmpty()) - { - cvNotempty.wait(mutex); - } + condVerNotEmpty.wait(mutex); } /** - * @return the number of packets that are ready to be sent. + * @return The number of packets that are ready to be sent. */ size_t countReady() { Lock<FastMutex> l(mutex); if (!buffer.isEmpty()) - { return buffer.last().isReady() ? buffer.count() : buffer.count() - 1; - } else - { return 0; - } } /** - * @return the number of packets in use, that are either fully or partially + * @return The number of packets in use, that are either fully or partially * filled. */ size_t countNotEmpty() { Lock<FastMutex> l(mutex); - return buffer.count(); } private: FastMutex mutex; - ConditionVariable cvNotempty; - - CircularBuffer<Pkt, pktNum> buffer; + ConditionVariable condVerNotEmpty; + CircularBuffer<Packet<pktLen>, pktNum> buffer; }; } // namespace Boardcore diff --git a/src/tests/catch/test-packetqueue.cpp b/src/tests/catch/test-packetqueue.cpp index aa2beb55466235a36a0f3336034b1c3bf8bf5262..e0cf2865ac8da3f9dbc691dbcd583c2aaa79ec03 100644 --- a/src/tests/catch/test-packetqueue.cpp +++ b/src/tests/catch/test-packetqueue.cpp @@ -82,10 +82,12 @@ TEST_CASE("Packet tests") SECTION("Adding stuff to packet") { + // Add 5 bytes + REQUIRE(p.append(messageBase, 5)); + uint64_t ts = p.getTimestamp(); - REQUIRE(p.tryAppend(messageBase, 5)); - uint64_t ts = p.timestamp(); - REQUIRE(miosix::getTick() - ts < 5); + REQUIRE(Boardcore::TimestampTimer::getInstance().getTimestamp() - ts < + 5); REQUIRE(p.dump(buf) == 5); COMPARE(buf, BUF_LEN, "01234"); @@ -93,29 +95,21 @@ TEST_CASE("Packet tests") REQUIRE(p.size() == 5); REQUIRE(p.getMsgCount() == 1); - REQUIRE(p.tryAppend(messageBase + 5, 3)); + // Add 3 bytes + REQUIRE(p.append(messageBase + 5, 3)); REQUIRE(p.dump(buf) == 8); COMPARE(buf, BUF_LEN, "01234567"); REQUIRE(p.isEmpty() == false); REQUIRE(p.size() == 8); - REQUIRE_FALSE(p.tryAppend(messageBase + 8, 3)); - REQUIRE(p.dump(buf) == 8); - COMPARE(buf, BUF_LEN, "01234567"); - REQUIRE(p.isEmpty() == false); - REQUIRE(p.size() == 8); - REQUIRE(p.getMsgCount() == 2); - - REQUIRE(p.tryAppend(messageBase + 8, 2)); - REQUIRE(p.dump(buf) == 10); + // Trying to add 3 more bytes, only 2 should be written + REQUIRE(p.append(messageBase + 8, 3) == 2); + REQUIRE(p.dump(buf) == PKT_LEN); COMPARE(buf, BUF_LEN, "0123456789"); REQUIRE(p.isEmpty() == false); - REQUIRE(p.isFull()); - REQUIRE(p.size() == 10); + REQUIRE(p.size() == PKT_LEN); REQUIRE(p.getMsgCount() == 3); - REQUIRE(p.timestamp() == ts); - p.clear(); REQUIRE(p.isEmpty()); REQUIRE(p.isFull() == false); @@ -130,7 +124,7 @@ TEST_CASE("Packet tests") SECTION("Edge cases") { INFO("Adding empty msg"); - REQUIRE_FALSE(p.tryAppend(messageBase, 0)); + REQUIRE_FALSE(p.append(messageBase, 0)); REQUIRE(p.isEmpty()); REQUIRE(p.isFull() == false); REQUIRE(p.isReady() == false); @@ -141,29 +135,16 @@ TEST_CASE("Packet tests") REQUIRE(p.dump(buf) == 0); INFO("Adding too big msg"); - REQUIRE_FALSE(p.tryAppend(messageBase, PKT_LEN + 1)); - - REQUIRE(p.isEmpty()); - REQUIRE(p.isFull() == false); - REQUIRE(p.isReady() == false); - REQUIRE(p.size() == 0); - REQUIRE(p.maxSize() == PKT_LEN); + REQUIRE(p.append(messageBase, PKT_LEN + 1) == PKT_LEN); - REQUIRE(p.getMsgCount() == 0); - REQUIRE(p.dump(buf) == 0); - - INFO("Adding something to full packet"); - REQUIRE(p.tryAppend(messageBase, PKT_LEN)); - REQUIRE_FALSE(p.tryAppend(messageBase, 1)); - - REQUIRE(p.isEmpty() == false); + REQUIRE_FALSE(p.isEmpty()); REQUIRE(p.isFull()); - REQUIRE(p.isReady() == false); + REQUIRE_FALSE(p.isReady()); REQUIRE(p.size() == PKT_LEN); REQUIRE(p.maxSize() == PKT_LEN); REQUIRE(p.getMsgCount() == 1); - REQUIRE(p.dump(buf) == 10); + REQUIRE(p.dump(buf) == PKT_LEN); COMPARE(buf, 10, "0123456789"); } } @@ -182,100 +163,142 @@ TEST_CASE("PacketQueue tests") SECTION("Normal operation") { INFO("Adding two elements to first packet"); - REQUIRE(pq.put(messageBase, 4) == 0); - REQUIRE(pq.put(messageBase, 4) == 0); + REQUIRE(pq.put(messageBase, 4)); + REQUIRE(pq.put(messageBase, 4)); + // No packet should be ready REQUIRE(pq.countReady() == 0); REQUIRE(pq.countNotEmpty() == 1); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); INFO("Adding third element and filling first packet"); - REQUIRE(pq.put(messageBase, 2) == 0); + REQUIRE(pq.put(messageBase, 2)); + // Now one single packet should be filled and ready REQUIRE(pq.countReady() == 1); REQUIRE(pq.countNotEmpty() == 1); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); + // Check the packet content Packet<PKT_LEN> p = pq.get(); REQUIRE(p.getMsgCount() == 3); REQUIRE(p.isFull()); REQUIRE(p.isReady()); COMPARE(p, "0123012301"); - INFO("Adding element to second packet"); - REQUIRE(pq.put(messageBase + 10, 4) == 0); + INFO("Adding more data to create a second packet"); + REQUIRE(pq.put(messageBase + 10, 4)); + // The second packet should not be ready REQUIRE(pq.countReady() == 1); REQUIRE(pq.countNotEmpty() == 2); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); REQUIRE_FALSE(pq.buffer.get(1).isReady()); - COMPARE(pq.buffer.get(1), "abcd"); p = pq.get(); // Should still return first packet REQUIRE(p.getMsgCount() == 3); - INFO( - "Adding element not fitting the second packet, added to the third"); - REQUIRE(pq.put(messageBase + 10, 7) == 0); + INFO("Adding more data to create a third packet"); + REQUIRE(pq.put(messageBase + 10, 7)); + p = pq.get(); // Should still return first packet REQUIRE(p.getMsgCount() == 3); - REQUIRE(pq.countReady() == 2); - REQUIRE(pq.countNotEmpty() == 3); - REQUIRE_FALSE(pq.isEmpty()); - REQUIRE_FALSE(pq.isFull()); - + // Check all the packages REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "0123012301"); REQUIRE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == PKT_LEN); + COMPARE(pq.buffer.get(1), "abcdabcdef"); REQUIRE_FALSE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == 1); + COMPARE(pq.buffer.get(2), "g"); - COMPARE(pq.buffer.get(0), "0123012301"); - COMPARE(pq.buffer.get(1), "abcd"); - COMPARE(pq.buffer.get(2), "abcdefg"); + // Check the queue stats + REQUIRE(pq.countReady() == 2); + REQUIRE(pq.countNotEmpty() == 3); + REQUIRE_FALSE(pq.isEmpty()); + REQUIRE_FALSE(pq.isFull()); INFO("Popping first element"); p = pq.pop(); // Should still return first packet REQUIRE(p.getMsgCount() == 3); COMPARE(p, "0123012301"); - // Should now return what was the second element - COMPARE(pq.get(), "abcd"); - + // The packets should now be shifted REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "abcdabcdef"); REQUIRE_FALSE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == 1); + COMPARE(pq.buffer.get(1), "g"); REQUIRE(pq.countReady() == 1); REQUIRE(pq.countNotEmpty() == 2); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); - INFO("Adding a msg back to the first packet and filling it"); - REQUIRE(pq.put(messageBase, 10) == 0); - REQUIRE(pq.countReady() == 3); + INFO("Adding more data to fill the last packet"); + REQUIRE(pq.put(messageBase, 10)); + REQUIRE(pq.countReady() == 2); REQUIRE(pq.countNotEmpty() == 3); REQUIRE_FALSE(pq.isEmpty()); - REQUIRE(pq.isFull()); + REQUIRE_FALSE(pq.isFull()); - COMPARE(pq.buffer.get(0), "abcd"); - COMPARE(pq.buffer.get(1), "abcdefg"); - COMPARE(pq.buffer.get(2), "0123456789"); + // We should now have three packets + REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "abcdabcdef"); + REQUIRE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == PKT_LEN); + COMPARE(pq.buffer.get(1), "g012345678"); + REQUIRE_FALSE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == 1); + COMPARE(pq.buffer.get(2), "9"); + + // If we now add another 10 bytes the last packet, the last byte which + // does not fit should be put in a new packet at the start of the queue + REQUIRE(pq.put(messageBase, 10)); + REQUIRE(pq.countReady() == 2); + REQUIRE(pq.countNotEmpty() == 3); + REQUIRE_FALSE(pq.isEmpty()); + REQUIRE_FALSE(pq.isFull()); + REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "g012345678"); + REQUIRE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == PKT_LEN); + COMPARE(pq.buffer.get(1), "9012345678"); + REQUIRE_FALSE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == 1); + COMPARE(pq.buffer.get(2), "9"); + + // And now by adding the last 9 bytes the queue should be marked ready + REQUIRE(pq.put(messageBase + 10, 9)); + REQUIRE(pq.countReady() == 3); + REQUIRE(pq.countNotEmpty() == 3); + REQUIRE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == PKT_LEN); + COMPARE(pq.buffer.get(2), "9abcdefghi"); INFO("Popping everything"); p = pq.pop(); - COMPARE(p, "abcd"); REQUIRE(p.isReady()); + REQUIRE(p.size() == PKT_LEN); + COMPARE(p, "g012345678"); p = pq.pop(); - COMPARE(p, "abcdefg"); + COMPARE(p, "9012345678"); REQUIRE(p.isReady()); p = pq.pop(); - COMPARE(p, "0123456789"); + COMPARE(p, "9abcdefghi"); REQUIRE(p.isReady()); REQUIRE_FALSE(pq.isFull()); @@ -289,19 +312,19 @@ TEST_CASE("PacketQueue tests") SECTION("Edge cases") { INFO("Adding too big msg"); - REQUIRE(pq.put(messageBase, PKT_LEN + 1) == -1); + REQUIRE_FALSE(pq.put(messageBase, PKT_LEN * QUEUE_LEN + 1)); REQUIRE_FALSE(pq.isFull()); REQUIRE(pq.isEmpty()); REQUIRE(pq.countNotEmpty() == 0); REQUIRE(pq.countReady() == 0); INFO("Adding empty message"); - REQUIRE(pq.put(messageBase, 0) == -1); + REQUIRE_FALSE(pq.put(messageBase, 0)); INFO("Adding something to full queue"); - REQUIRE(pq.put(messageBase, PKT_LEN) == 0); - REQUIRE(pq.put(messageBase + 5, PKT_LEN) == 0); - REQUIRE(pq.put(messageBase + 10, PKT_LEN) == 0); + REQUIRE(pq.put(messageBase, PKT_LEN)); + REQUIRE(pq.put(messageBase + 5, PKT_LEN)); + REQUIRE(pq.put(messageBase + 10, PKT_LEN)); REQUIRE(pq.buffer.count() == 3); for (int i = 0; i < 3; i++) diff --git a/src/tests/drivers/sx1278/test-sx1278-bench.cpp b/src/tests/drivers/sx1278/test-sx1278-bench.cpp index d484d39cab98c3703d89d27da0c176900cca7690..31db75f4f5c47aa0a847be004e403718df858116 100644 --- a/src/tests/drivers/sx1278/test-sx1278-bench.cpp +++ b/src/tests/drivers/sx1278/test-sx1278-bench.cpp @@ -93,9 +93,6 @@ struct Stats } stats; -/// Interval between transmissions. -const int TX_INTERVAL = 1; - SX1278 *sx1278[2] = {nullptr, nullptr}; struct Msg @@ -115,12 +112,10 @@ void recvLoop(int idx) while (1) { Msg msg; - msg.idx = 0; - msg.dummy_1 = 0; - msg.dummy_2 = 0; - msg.dummy_3 = 0; + memset(&msg, 0, sizeof(msg)); int len = sx1278[idx]->receive((uint8_t *)&msg, sizeof(msg)); + if (len != sizeof(msg)) { stats.recv_errors++; @@ -143,8 +138,6 @@ void sendLoop(int idx) { while (1) { - miosix::Thread::sleep(TX_INTERVAL); - int next_idx = stats.send_count + 1; Msg msg; @@ -269,6 +262,7 @@ int main() std::thread recv([]() { recvLoop(0); }); #endif #ifndef DISABLE_TX + miosix::Thread::sleep(500); std::thread send([]() { sendLoop(1); }); #endif diff --git a/src/tests/drivers/sx1278/test-sx1278-serial.cpp b/src/tests/drivers/sx1278/test-sx1278-serial.cpp index 0078a16b949f927ca05b82cd7dd6f67b2023b40f..22df3fe51535b7a62aefe3bdce39aae66208ec5f 100644 --- a/src/tests/drivers/sx1278/test-sx1278-serial.cpp +++ b/src/tests/drivers/sx1278/test-sx1278-serial.cpp @@ -31,17 +31,17 @@ using namespace Boardcore; using namespace miosix; -SPIBus bus(SPI3); +SPIBus bus(SPI4); -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); +GpioPin sck(GPIOE_BASE, 2); +GpioPin miso(GPIOE_BASE, 5); +GpioPin mosi(GPIOE_BASE, 6); +GpioPin cs(GPIOC_BASE, 1); +GpioPin dio(GPIOF_BASE, 10); SX1278* sx1278 = nullptr; -void __attribute__((used)) EXTI15_IRQHandlerImpl() +void __attribute__((used)) EXTI10_IRQHandlerImpl() { if (sx1278) sx1278->handleDioIRQ(); @@ -54,16 +54,16 @@ void initBoard() miosix::FastInterruptDisableLock dLock; // Enable SPI3 - RCC->APB1ENR |= RCC_APB1ENR_SPI3EN; + RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; // Enable SPI4 bus RCC_SYNC(); // Setup SPI pins sck.mode(miosix::Mode::ALTERNATE); - sck.alternateFunction(6); + sck.alternateFunction(5); miso.mode(miosix::Mode::ALTERNATE); - miso.alternateFunction(6); + miso.alternateFunction(5); mosi.mode(miosix::Mode::ALTERNATE); - mosi.alternateFunction(6); + mosi.alternateFunction(5); cs.mode(miosix::Mode::OUTPUT); dio.mode(miosix::Mode::INPUT); @@ -77,30 +77,35 @@ void initBoard() 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? + auto serial = miosix::DefaultConsole::instance().get(); + serial->writeBlock(msg, len, 0); } } } void sendLoop() { - uint8_t msg[256]; - auto console = miosix::DefaultConsole::instance().get(); + // I create a GPIO with the onboard led to tell the user that + // a package is being sent + miosix::GpioPin led(GPIOG_BASE, 13); + led.mode(miosix::Mode::OUTPUT); + led.low(); + uint8_t msg[63]; while (1) { - int len = console->readBlock(msg, sizeof(msg), 0); + auto serial = miosix::DefaultConsole::instance().get(); + int len = serial->readBlock(msg, sizeof(msg), 0); if (len > 0) { + led.high(); sx1278->send(msg, len); + led.low(); } } } @@ -122,13 +127,20 @@ int main() return -1; } + printf("\n[sx1278] Initialization complete!\n"); + std::thread recv([]() { recvLoop(); }); std::thread send([]() { sendLoop(); }); - printf("\n[sx1278] Initialization complete!\n"); + for (;;) + { + miosix::Thread::sleep(100); + } - while (1) - miosix::Thread::wait(); + // God please forgive me + // FIXME(davide.mor): ABSOLUTELY fix this + // miosix::Thread::sleep(20000); + // miosix::reboot(); return 0; }