diff --git a/CMakeLists.txt b/CMakeLists.txt
index 83d05228c5f8d06d1276fc95447bd2514fd84375..09bec0a5f3a8d1e6bc1aed9a421b271a121b96f2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -136,6 +136,7 @@ add_executable(catch-tests-boardcore
src/tests/catch/test-eventbroker.cpp
src/tests/catch/test-gptimer.cpp
src/tests/catch/test-kalman.cpp
+ src/tests/catch/test-numeric.cpp
src/tests/catch/test-packetqueue.cpp
src/tests/catch/test-sensormanager-catch.cpp
src/tests/catch/xbee/test-xbee-parser.cpp
diff --git a/src/shared/drivers/usart/USART.cpp b/src/shared/drivers/usart/USART.cpp
index 5efb72ea91d3c933a092f2da2b888b5b79edfc09..ea076f486972f1e2523f3e03a814e7a37b224b86 100644
--- a/src/shared/drivers/usart/USART.cpp
+++ b/src/shared/drivers/usart/USART.cpp
@@ -25,6 +25,7 @@
#include <fcntl.h>
#include <stdio.h>
#include <utils/Debug.h>
+#include <utils/Numeric.h>
#include <string>
@@ -366,15 +367,15 @@ void USART::IRQhandleInterrupt()
if (error || idle || (rxQueue.size() >= rxQueue.capacity() / 2))
{
// Enough data in buffer or idle line, awake thread
- if (rxWaiting)
+ if (rxWaiter)
{
- rxWaiting->IRQwakeup();
- if (rxWaiting->IRQgetPriority() >
+ rxWaiter->IRQwakeup();
+ if (rxWaiter->IRQgetPriority() >
miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
{
miosix::Scheduler::IRQfindNextThread();
}
- rxWaiting = nullptr;
+ rxWaiter = nullptr;
}
}
}
@@ -500,13 +501,16 @@ void USART::setBaudrate(int baudrate)
}
bool USART::readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
- const bool blocking)
+ const bool blocking, std::chrono::nanoseconds timeout)
{
miosix::Lock<miosix::FastMutex> l(rxMutex);
char *buf = reinterpret_cast<char *>(buffer);
size_t result = 0;
error = false;
+ // Whether we timed out while waiting
+ bool timedOut = false;
+
miosix::FastInterruptDisableLock dLock;
for (;;)
{
@@ -523,23 +527,39 @@ bool USART::readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
// If blocking, we are waiting for at least one byte of data before
// returning. If not blocking, in the case the bus is idle we return
// anyway
- if ((result == nBytes) || (idle && (!blocking || (result > 0))))
+ if ((result == nBytes) || (idle && (!blocking || (result > 0))) ||
+ timedOut)
break;
// Wait for data in the queue
do
{
- rxWaiting = miosix::Thread::IRQgetCurrentThread();
- miosix::Thread::IRQwait();
+ rxWaiter = miosix::Thread::IRQgetCurrentThread();
+
+ if (timeout == std::chrono::nanoseconds::zero())
{
- miosix::FastInterruptEnableLock eLock(dLock);
- miosix::Thread::yield();
+ miosix::Thread::IRQenableIrqAndWait(dLock);
}
- } while (rxWaiting);
+ else
+ {
+ int64_t wakeup = add_sat(miosix::IRQgetTime(), timeout.count());
+ auto waitResult =
+ miosix::Thread::IRQenableIrqAndTimedWait(dLock, wakeup);
+
+ if (waitResult == miosix::TimedWaitResult::Timeout)
+ {
+ // De-register from wakeup by the IRQ
+ rxWaiter = nullptr;
+ // Make the outer for-loop quit after reading data from the
+ // rxQueue, or we'd end up waiting again
+ timedOut = true;
+ }
+ }
+ } while (rxWaiter);
}
nBytesRead = result;
- return (result > 0);
+ return (result > 0) && !timedOut;
}
void USART::write(const void *buffer, size_t nBytes)
@@ -686,7 +706,8 @@ bool STM32SerialWrapper::serialCommSetup()
}
bool STM32SerialWrapper::readImpl(void *buffer, size_t nBytes,
- size_t &nBytesRead, const bool blocking)
+ size_t &nBytesRead, const bool blocking,
+ std::chrono::nanoseconds timeout)
{
// non-blocking read not supported in STM32SerialWrapper
if (!blocking)
@@ -697,6 +718,16 @@ bool STM32SerialWrapper::readImpl(void *buffer, size_t nBytes,
"STM32SerialWrapper::read doesn't support non-blocking read"));
}
+ // Timeout is not supported on STM32SerialWrapper, timeout is always set to
+ // std::chrono::nanoseconds::zero() unless specified by the user
+ if (timeout != std::chrono::nanoseconds::zero())
+ {
+ LOG_ERR(logger,
+ "STM32SerialWrapper::read doesn't support timeout on read");
+ D(assert(false &&
+ "STM32SerialWrapper::read doesn't support timeout on read"));
+ }
+
size_t n = ::read(fd, buffer, nBytes);
nBytesRead = n;
diff --git a/src/shared/drivers/usart/USART.h b/src/shared/drivers/usart/USART.h
index 0e45b41a40753aa0fd0e97f41bd47dd2b04f065e..d2d08f486396c7bff1b7e96425335b5d7cb03039 100644
--- a/src/shared/drivers/usart/USART.h
+++ b/src/shared/drivers/usart/USART.h
@@ -27,6 +27,7 @@
#include <miosix.h>
#include <utils/ClockUtils.h>
+#include <chrono>
#include <string>
#include "arch/common/drivers/serial.h"
@@ -79,30 +80,37 @@ public:
virtual ~USARTInterface() = 0;
/**
- * @brief Blocking read operation to read nBytes or till the data transfer
- * is complete.
+ * @brief Blocking read operation to read nBytes until the data transfer
+ * is complete or the timeout is reached.
* @param buffer Buffer that will contain the received data.
* @param nBytes Maximum size of the buffer.
- * @return If operation succeeded.
+ * @param timeout The maximum time that will be waited, defaults to waiting
+ * forever.
+ * @return Whether bytes were read and no timeout occurred.
*/
- [[nodiscard]] virtual bool readBlocking(void *buffer, size_t nBytes)
+ [[nodiscard]] virtual bool readBlocking(
+ void *buffer, size_t nBytes,
+ std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero())
{
size_t temp;
- return readImpl(buffer, nBytes, temp, true);
+ return readImpl(buffer, nBytes, temp, true, timeout);
};
/**
- * @brief Blocking read operation to read nBytes or till the data transfer
- * is complete.
+ * @brief Blocking read operation to read nBytes until the data transfer
+ * is complete or the timeout is reached.
* @param buffer Buffer that will contain the received data.
* @param nBytes Maximum size of the buffer.
* @param nBytesRead Number of bytes read in the transaction.
- * @return If operation succeeded.
+ * @param timeout The maximum time that will be waited, defaults to waiting
+ * forever.
+ * @return Whether bytes were read and no timeout occurred.
*/
- [[nodiscard]] virtual bool readBlocking(void *buffer, size_t nBytes,
- size_t &nBytesRead)
+ [[nodiscard]] virtual bool readBlocking(
+ void *buffer, size_t nBytes, size_t &nBytesRead,
+ std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero())
{
- return readImpl(buffer, nBytes, nBytesRead, true);
+ return readImpl(buffer, nBytes, nBytesRead, true, timeout);
};
/**
@@ -132,10 +140,13 @@ protected:
* @param nBytesRead Number of bytes read.
* @param blocking Whether the read should block or not; in case it isn't
* blocking the read could return also 0 bytes.
- * @return If operation succeeded.
+ * @param timeout The maximum time that will be waited when in blocking
+ * mode, 0 to disable the timeout and wait forever.
+ * @return Whether bytes were read and no timeout occurred.
*/
virtual bool readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
- const bool blocking) = 0;
+ const bool blocking,
+ std::chrono::nanoseconds timeout) = 0;
USARTType *usart;
int id = -1; ///< Can be from 1 to 8, -1 is invalid.
@@ -218,7 +229,8 @@ public:
[[nodiscard]] bool read(void *buffer, size_t nBytes)
{
size_t temp;
- return readImpl(buffer, nBytes, temp, false);
+ auto timeout = std::chrono::nanoseconds::zero();
+ return readImpl(buffer, nBytes, temp, false, timeout);
}
/**
@@ -233,7 +245,8 @@ public:
*/
[[nodiscard]] bool read(void *buffer, size_t nBytes, size_t &nBytesRead)
{
- return readImpl(buffer, nBytes, nBytesRead, false);
+ auto timeout = std::chrono::nanoseconds::zero();
+ return readImpl(buffer, nBytes, nBytesRead, false, timeout);
};
/**
@@ -301,16 +314,19 @@ private:
* @param nBytesRead Number of bytes read.
* @param blocking Whether the read should block or not; in case it isn't
* blocking the read could return also 0 bytes.
- * @return If operation succeeded.
+ * @param timeout The maximum time that will be waited when in blocking
+ * mode, 0 to disable the timeout and wait forever.
+ * @return Whether bytes were read and no timeout occurred.
*/
[[nodiscard]] bool readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
- const bool blocking) override;
+ const bool blocking,
+ std::chrono::nanoseconds timeout) override;
miosix::FastMutex rxMutex; ///< mutex for receiving on serial
miosix::FastMutex txMutex; ///< mutex for transmitting on serial
- ///< Pointer to the waiting on receive thread
- miosix::Thread *rxWaiting = 0;
+ miosix::Thread *rxWaiter =
+ nullptr; ///< The thread that is waiting to receive data
miosix::DynUnsyncQueue<char> rxQueue; ///< Receiving queue
bool idle = true; ///< Receiver idle
@@ -390,10 +406,13 @@ private:
* @param nBytesRead Number of bytes read.
* @param blocking Whether the read should block or not; in case it isn't
* blocking the read could return also 0 bytes.
- * @return If operation succeeded.
+ * @param timeout The maximum time that will be waited when in blocking
+ * mode.
+ * @return Whether bytes were read and no timeout occurred.
*/
[[nodiscard]] bool readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
- const bool blocking) override;
+ const bool blocking,
+ std::chrono::nanoseconds timeout) override;
/**
* @brief Creates a device that represents the serial port, adds it to the
diff --git a/src/shared/utils/Numeric.h b/src/shared/utils/Numeric.h
new file mode 100644
index 0000000000000000000000000000000000000000..e87cf933bb617c3450626cbcb78a9b817c5f161e
--- /dev/null
+++ b/src/shared/utils/Numeric.h
@@ -0,0 +1,65 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Damiano Procaccia
+ *
+ * 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.
+ */
+
+/**
+ * @file Numeric.h contains utility functions for numeric types, possibly
+ * from newer C++ standards.
+ */
+
+#pragma once
+
+#include <limits>
+#include <type_traits>
+
+namespace Boardcore
+{
+
+/**
+ * @brief Computes the saturating addition x + y for integral types.
+ * @returns x + y, if the result is representable as a value of type T.
+ * Otherwise, the largest or smallest value of type T, whichever is closer to
+ * the result.
+ */
+template <class T>
+constexpr auto add_sat(T x, T y) noexcept ->
+ typename std::enable_if_t<std::is_integral<T>::value, T>
+{
+ auto result = T{};
+ // The built-in function returns true if the addition overflows
+ bool overflow = __builtin_add_overflow(x, y, &result);
+
+ if (!overflow)
+ {
+ return result;
+ }
+
+ if (x < 0)
+ {
+ return std::numeric_limits<T>::min();
+ }
+ else
+ {
+ return std::numeric_limits<T>::max();
+ }
+}
+
+} // namespace Boardcore
diff --git a/src/tests/catch/test-numeric.cpp b/src/tests/catch/test-numeric.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5eae76bf09e3b6ae7ded34ca5f7b2d782d6285ec
--- /dev/null
+++ b/src/tests/catch/test-numeric.cpp
@@ -0,0 +1,58 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * 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 <utils/Numeric.h>
+
+#include <catch2/catch.hpp>
+#include <climits>
+
+using namespace Boardcore;
+
+TEST_CASE("Numeric - Saturating Add")
+{
+ constexpr int a = add_sat(3, 4); // no saturation occurs, T = int
+ REQUIRE(a == 7);
+
+ constexpr unsigned char b =
+ add_sat<unsigned char>(UCHAR_MAX, 4); // saturated
+ REQUIRE(b == UCHAR_MAX);
+
+ constexpr unsigned char c = static_cast<unsigned char>(
+ add_sat(UCHAR_MAX, 4)); // not saturated, T = int
+ // add_sat(int, int) returns int tmp == 259,
+ // then assignment truncates 259 % 256 == 3
+ REQUIRE(c == 3);
+
+ constexpr unsigned char e = add_sat<unsigned char>(251, a); // saturated
+ REQUIRE(e == UCHAR_MAX);
+ // 251 is of type T = unsigned char, `a` is converted to unsigned char
+ // value; might yield an int -> unsigned char conversion warning for `a`
+
+ constexpr signed char f = add_sat<signed char>(-123, -3); // not saturated
+ REQUIRE(f == -126);
+
+ constexpr signed char g = add_sat<signed char>(-123, -13); // saturated
+ REQUIRE(g == std::numeric_limits<signed char>::min()); // g == -128
+
+ constexpr signed char h = add_sat<signed char>(126, CHAR_MAX); // saturated
+ REQUIRE(h == std::numeric_limits<signed char>::max()); // h == 127
+}
diff --git a/src/tests/drivers/usart/test-usart.cpp b/src/tests/drivers/usart/test-usart.cpp
index 06502eb2e900eba82ea9a0069287c95c38221ce6..f13bbf1a229358352aa84c0207e9cc07cf25bcc8 100644
--- a/src/tests/drivers/usart/test-usart.cpp
+++ b/src/tests/drivers/usart/test-usart.cpp
@@ -23,6 +23,7 @@
#include <fmt/format.h>
#include <cassert>
+#include <chrono>
#include "drivers/usart/USART.h"
#include "miosix.h"
@@ -206,7 +207,7 @@ bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst)
bool testClearQueue(USART *src, USART *dst)
{
char buf[128];
- size_t nReads{0};
+ unsigned int nReads{0};
src->writeString("Transmitting useless stuff!");
// miosix::delayUs(1000);
dst->clearQueue();
@@ -214,7 +215,7 @@ bool testClearQueue(USART *src, USART *dst)
// Can be commented to test without read
if (dst->read(buf, 128, nReads))
{
- printf("### read something after the clearQueue: %s (%zu bytes)\n", buf,
+ printf("### read something after the clearQueue: %s (%u bytes)\n", buf,
nReads);
// Shouldn't read anything
return false;
@@ -227,7 +228,7 @@ bool testClearQueue(USART *src, USART *dst)
if (strcmp(buf, "Now transmitting the juicy stuff :P") != 0)
{
printf(
- "### read something different than the things sent: %s (%zu "
+ "### read something different than the things sent: %s (%u "
"bytes)\n",
buf, nReads);
return false;
@@ -237,6 +238,85 @@ bool testClearQueue(USART *src, USART *dst)
return true;
}
+bool testReadTimeout(USART *src, USART *dst)
+{
+ using namespace std::chrono;
+
+ constexpr auto testString = "This is truly a string :-D";
+
+ constexpr auto timeout = 100ms;
+ char buf[64] = {0};
+ unsigned int bytesRead = 0;
+
+ /************** Read with timeout without sending anything ***************/
+
+ printf("Reading with timeout %lldms\n", timeout.count());
+ printf("\t%d--> sent: \t'' (nothing being sent, force timeout)\n",
+ src->getId());
+
+ auto start = steady_clock::now();
+ bool result = dst->readBlocking(buf, sizeof(buf), bytesRead, timeout);
+ auto end = steady_clock::now();
+
+ auto measuredTime = duration_cast<milliseconds>(end - start);
+
+ printf("\t%d<-- received: \t'%s'\n", dst->getId(), buf);
+ printf("\tTimed out after %lldms\n", measuredTime.count());
+
+ // Timeout should return false, if it returned true then the test failed
+ if (result)
+ {
+ printf("### readBlocking returned success on timeout: %s (%u bytes)\n",
+ buf, bytesRead);
+ return false;
+ }
+
+ // Check if the timeout is correct
+ if (measuredTime < timeout)
+ {
+ printf(
+ "### readBlocking returned before the timeout: expected: %lld ms, "
+ "measured: %lld ms (%u bytes)\n",
+ timeout.count(), measuredTime.count(), bytesRead);
+ return false;
+ }
+
+ src->clearQueue();
+ dst->clearQueue();
+
+ /************** Read with timeout after sending something ***************/
+
+ printf("Reading with timeout %lldms\n", timeout.count());
+ printf("\t%d--> sent: \t'%s'\n", src->getId(), testString);
+ src->writeString(testString);
+
+ start = steady_clock::now();
+ result = dst->readBlocking(buf, sizeof(buf), bytesRead, timeout);
+ end = steady_clock::now();
+
+ measuredTime = duration_cast<milliseconds>(end - start);
+
+ printf("\t%d<-- received: \t'%s'\n", dst->getId(), buf);
+ if (!result)
+ {
+ printf("\tTimed out after %lldms\n", measuredTime.count());
+ }
+ else
+ {
+ printf("\tNo timeout\n");
+ }
+
+ if (strcmp(buf, testString) != 0)
+ {
+ printf("### readBlocking returned different string: %s (%u bytes)\n",
+ buf, bytesRead);
+ return false;
+ }
+
+ printf("*** ReadTimeout test passed\n");
+ return true;
+}
+
/* Available default pins:
* - USART1: tx=PA9 rx=PA10
* - USART2: tx=PA2 rx=PA3
@@ -255,57 +335,61 @@ int main()
u6tx1::getPin().mode(miosix::Mode::ALTERNATE);
u6tx1::getPin().alternateFunction(8);
- u4rx2::getPin().mode(miosix::Mode::ALTERNATE);
- u4rx2::getPin().alternateFunction(8);
- u4tx2::getPin().mode(miosix::Mode::ALTERNATE);
- u4tx2::getPin().alternateFunction(8);
+ u4rx1::getPin().mode(miosix::Mode::ALTERNATE);
+ u4rx1::getPin().alternateFunction(8);
+ u4tx1::getPin().mode(miosix::Mode::ALTERNATE);
+ u4tx1::getPin().alternateFunction(8);
+
+ bool testPassed = true;
+ printf("*** SERIAL 3 WORKING!\n");
- for (;;)
+ for (int baudrate : baudrates)
{
- bool testPassed = true;
- printf("*** SERIAL 3 WORKING!\n");
-
- for (int baudrate : baudrates)
- {
- printf("\n\n########################### %d\n", baudrate);
- // declaring the usart peripherals
- USART usartx(USART6, baudrate);
- // usartx.setBaudrate(baudrate);
- // usartx.setOversampling(false);
- // usartx.setStopBits(1);
- // usartx.setWordLength(USART::WordLength::BIT8);
- // usartx.setParity(USART::ParityBit::NO_PARITY);
-
- USART usarty(UART4, baudrate);
- // STM32SerialWrapper usarty(UART4, baudrate, u4rx2::getPin(),
- // u4tx2::getPin());
-
- // testing transmission (both char and binary) "serial 1 <- serial
- // 2"
- testPassed &= testCommunicationSequential(&usartx, &usarty);
- testPassed &= testClearQueue(&usartx, &usarty);
-
- // testing transmission (both char and binary) "serial 1 -> serial
- // 2"
- testPassed &= testCommunicationSequential(&usarty, &usartx);
- testPassed &= testClearQueue(&usarty, &usartx);
- }
-
- if (testPassed)
- {
- printf(
- "********************************\n"
- "*** TEST PASSED ***\n"
- "********************************\n");
- }
- else
- {
- printf(
- "################################\n"
- "### TEST FAILED ###\n"
- "################################\n");
- }
- Thread::sleep(5000);
+ printf("\n\n########################### %d\n", baudrate);
+ // declaring the usart peripherals
+ USART usartx(USART6, baudrate);
+ // usartx.setBaudrate(baudrate);
+ // usartx.setOversampling(false);
+ // usartx.setStopBits(1);
+ // usartx.setWordLength(USART::WordLength::BIT8);
+ // usartx.setParity(USART::ParityBit::NO_PARITY);
+
+ USART usarty(UART4, baudrate);
+ // STM32SerialWrapper usarty(UART4, baudrate, u4rx2::getPin(),
+ // u4tx2::getPin());
+
+ // testing transmission (both char and binary) "serial 1 <- serial
+ // 2"
+ testPassed &= testCommunicationSequential(&usartx, &usarty);
+ testPassed &= testClearQueue(&usartx, &usarty);
+
+ // testing transmission (both char and binary) "serial 1 -> serial
+ // 2"
+ testPassed &= testCommunicationSequential(&usarty, &usartx);
+ testPassed &= testClearQueue(&usarty, &usartx);
+
+ testPassed &= testReadTimeout(&usartx, &usarty);
}
+
+ if (testPassed)
+ {
+ printf(
+ "********************************\n"
+ "*** TEST PASSED ***\n"
+ "********************************\n");
+ }
+ else
+ {
+ printf(
+ "################################\n"
+ "### TEST FAILED ###\n"
+ "################################\n");
+ }
+
+ while (true)
+ {
+ Thread::wait();
+ }
+
return 0;
}