diff --git a/.vscode/settings.json b/.vscode/settings.json
index caa4ff15bf38b4658d34a4bf0af6745b62d54e44..68d8988bcafbddb302ab2276396aab2f81057fc8 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -118,19 +118,30 @@
"specialfunctions": "cpp"
},
"cSpell.words": [
+ "abom",
"ADCPRE",
+ "addfilter",
"ADON",
"aelf",
"airbrakes",
"Airbrakes",
+ "Alain",
"Alessandro",
+ "ALST",
"AMSL",
+ "apbclk",
"atthr",
"AVDD",
+ "awum",
"Baro",
+ "bittiming",
"boardcore",
"Boardcorev",
"boudrate",
+ "Canbus",
+ "canprotocol",
+ "Carlucci",
+ "compid",
"Corigliano",
"CORTEXM",
"cpitch",
@@ -153,6 +164,8 @@
"Fatttr",
"fedetft's",
"fiprintf",
+ "FMPIE",
+ "FOVR",
"Gatttr",
"getdetahstate",
"GNSS",
@@ -169,10 +182,15 @@
"Hatt",
"HIFCR",
"HISR",
+ "hppw",
"HSCMAND",
"HSCMRNN",
"HWMAPPING",
"IDLEIE",
+ "Impli",
+ "Implii",
+ "INAK",
+ "INRQ",
"irqn",
"irqv",
"JEOC",
@@ -182,13 +200,17 @@
"Kalman",
"Katt",
"kbps",
+ "LBKM",
"ldrex",
"leds",
"LIFCR",
"LISR",
"logdecoder",
"Luca",
+ "Mandelli",
"Matteo",
+ "Mavlink",
+ "mavlinkdriver",
"MEKF",
"microcontrollers",
"MINC",
@@ -198,6 +220,7 @@
"mosi",
"MPXHZ",
"Musso",
+ "nart",
"NATT",
"NBAR",
"NDTR",
@@ -213,13 +236,21 @@
"PINC",
"Pitot",
"Plin",
+ "prescaler",
"Qgbw",
"Qget",
"Qhandle",
"Qput",
"Qwait",
"Qwakeup",
+ "RDHR",
+ "RDLR",
+ "RDTR",
+ "rflm",
+ "RFOM",
"Riccardo",
+ "RQCP",
+ "RXIRQ",
"RXNE",
"RXNEIE",
"sats",
@@ -239,11 +270,22 @@
"syaw",
"TCIE",
"TCIF",
+ "TDHR",
+ "TDLR",
+ "TDTR",
"TEIE",
"Terraneo",
"testsuite",
+ "THID",
+ "TMEIE",
+ "tparam",
"TSCPP",
"TSVREFE",
+ "Tweakable",
+ "txfp",
+ "TXIRQ",
+ "TXOK",
+ "TXRQ",
"Ublox",
"UBXACK",
"UBXGPS",
@@ -261,4 +303,4 @@
],
"cSpell.language": "en",
"cSpell.enabled": true
-}
\ No newline at end of file
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ad794b6f0220f7347b1ac0fb5ae15c7b63e1db63..d29e49129200e02632cb2652bf9001e68da5ab70 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,7 +52,7 @@ add_executable(runcam-settings src/entrypoints/runcam-settings.cpp)
sbs_target(runcam-settings stm32f407vg_stm32f4discovery)
add_executable(sdcard-benchmark src/entrypoints/sdcard-benchmark.cpp)
-sbs_target(sdcard-benchmark stm32f205rc_skyward_ciuti)
+sbs_target(sdcard-benchmark stm32f429zi_skyward_death_stack_x)
#-----------------------------------------------------------------------------#
# Tests #
@@ -159,14 +159,18 @@ sbs_target(test-ada stm32f429zi_skyward_death_stack_v3)
#-----------------------------------------------------------------------------#
# Tests - Drivers #
#-----------------------------------------------------------------------------#
-add_executable(test-can-protocol src/tests/drivers/canbus/test-can-protocol.cpp)
-sbs_target(test-can-protocol stm32f429zi_stm32f4discovery)
-add_executable(test-canbus-loopback src/tests/drivers/canbus/test-canbus-loopback.cpp)
-sbs_target(test-canbus-loopback stm32f429zi_stm32f4discovery)
+add_executable(test-can-2way src/tests/drivers/canbus/CanDriver/test-can-2way.cpp)
+sbs_target(test-can-2way stm32f429zi_skyward_pyxis_auxiliary)
-add_executable(test-canbus-2way src/tests/drivers/canbus/test-canbus-2way.cpp)
-sbs_target(test-canbus-2way stm32f429zi_stm32f4discovery)
+add_executable(test-can-filters src/tests/drivers/canbus/CanDriver/test-can-filters.cpp)
+sbs_target(test-can-filters stm32f429zi_skyward_pyxis_auxiliary)
+
+add_executable(test-can-loopback src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp)
+sbs_target(test-can-loopback stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-can-protocol src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp)
+sbs_target(test-can-protocol stm32f429zi_skyward_death_stack_x)
add_executable(test-dma-spi src/tests/drivers/dma/test-dma-spi.cpp)
sbs_target(test-dma-spi stm32f429zi_stm32f4discovery)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 21fc12ef98d913aca0e8accc6c7f91959a210015..3453618b78c814bded59867f2d416255aad4744b 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -45,8 +45,9 @@ foreach(OPT_BOARD ${BOARDS})
# Drivers
${SBS_BASE}/src/shared/drivers/adc/InternalTemp.cpp
${SBS_BASE}/src/shared/drivers/adc/InternalADC.cpp
- ${SBS_BASE}/src/shared/drivers/canbus/Canbus.cpp
- ${SBS_BASE}/src/shared/drivers/canbus/CanInterrupt.cpp
+ ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
+ ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
+ ${SBS_BASE}/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
${SBS_BASE}/src/shared/drivers/i2c/stm32f2_f4_i2c.cpp
${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp
${SBS_BASE}/src/shared/drivers/timer/PWM.cpp
diff --git a/src/shared/drivers/canbus/BusLoadEstimation.h b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
similarity index 98%
rename from src/shared/drivers/canbus/BusLoadEstimation.h
rename to src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
index 66bf37394c23a1fc14617028d1b350e0c830a968..d635553acec07638bffa7aa20876123429fbe2de 100644
--- a/src/shared/drivers/canbus/BusLoadEstimation.h
+++ b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
@@ -27,7 +27,7 @@
#include <cstdint>
-#include "CanData.h"
+#include "CanDriverData.h"
using miosix::FastMutex;
using miosix::Lock;
diff --git a/src/shared/drivers/canbus/Canbus.cpp b/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
similarity index 94%
rename from src/shared/drivers/canbus/Canbus.cpp
rename to src/shared/drivers/canbus/CanDriver/CanDriver.cpp
index b7d70591a2b90d79f7ba675bd98c6858e89192ad..dc9469bddda098bdeac1b050a113bc04e734128c 100644
--- a/src/shared/drivers/canbus/Canbus.cpp
+++ b/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
@@ -20,7 +20,7 @@
* THE SOFTWARE.
*/
-#include "Canbus.h"
+#include "CanDriver.h"
#include <kernel/scheduler/scheduler.h>
#include <utils/ClockUtils.h>
@@ -53,26 +53,23 @@ CanbusDriver::CanbusDriver(CAN_TypeDef* can, CanbusConfig config,
ClockUtils::enablePeripheralClock(can);
// Enter init mode
- can->MCR &= (~CAN_MCR_SLEEP);
+ can->MCR &= ~CAN_MCR_SLEEP;
can->MCR |= CAN_MCR_INRQ;
while ((can->MSR & CAN_MSR_INAK) == 0)
;
+ // Automatic wakeup when a new packet is available
if (config.awum)
- {
- can->MCR |=
- CAN_MCR_AWUM; // Automatic wakeup when a new packet is available
- }
+ can->MCR |= CAN_MCR_AWUM;
+ // Automatically recover from Bus-Off mode
if (config.abom)
- {
- can->MCR |= CAN_MCR_ABOM; // Automatically recover from Bus-Off mode
- }
+ can->MCR |= CAN_MCR_ABOM;
+
+ // Disable automatic retransmission
if (config.nart)
- {
- can->MCR |= CAN_MCR_NART; // Disable automatic retransmission
- }
+ can->MCR |= CAN_MCR_NART;
// Bit timing configuration
can->BTR &= ~CAN_BTR_BRP;
@@ -86,21 +83,15 @@ CanbusDriver::CanbusDriver(CAN_TypeDef* can, CanbusConfig config,
can->BTR |= ((bitTiming.SJW - 1) & 0x3) << 24;
if (config.loopback)
- {
can->BTR |= CAN_BTR_LBKM;
- }
// Enter filter initialization mode
can->FMR |= CAN_FMR_FINIT;
if (can == CAN1)
- {
canDrivers[0] = this;
- }
else
- {
canDrivers[1] = this;
- }
// Enable interrupts
can->IER |= CAN_IER_FMPIE0 | CAN_IER_FMPIE1 | CAN_IER_TMEIE;
@@ -139,7 +130,7 @@ CanbusDriver::BitTiming CanbusDriver::calcBitTiming(AutoBitTiming autoBt)
1 << 10),
1);
- // Given N, calculate BS1 and BS2 that statusult in a sample time as
+ // Given N, calculate BS1 and BS2 that result in a sample time as
// close as possible to the target one
cfgIter.BS1 =
std::min(std::max((int)roundf(autoBt.samplePoint * N - 1), 1),
@@ -190,9 +181,7 @@ CanbusDriver::BitTiming CanbusDriver::calcBitTiming(AutoBitTiming autoBt)
void CanbusDriver::init()
{
if (isInit)
- {
return;
- }
PrintLogger ls = l.getChild("init");
@@ -203,9 +192,7 @@ void CanbusDriver::init()
LOG_DEBUG(ls, "Waiting for canbus synchronization...");
while ((can->MSR & CAN_MSR_INAK) > 0)
- {
Thread::sleep(1);
- }
LOG_INFO(ls, "Canbus synchronized! Init done!");
@@ -221,6 +208,7 @@ bool CanbusDriver::addFilter(FilterBank filter)
LOG_ERR(ls, "Cannot add filter: canbus already initialized");
return false;
}
+
if (filterIndex == NUM_FILTER_BANKS)
{
LOG_ERR(ls, "Cannot add filter: no more filter banks available");
@@ -256,6 +244,7 @@ uint32_t CanbusDriver::send(CanPacket packet)
{
miosix::FastInterruptDisableLock d;
+
// Wait until there is an empty mailbox available to use
while ((can->TSR & CAN_TSR_TME) == 0)
{
@@ -312,13 +301,9 @@ uint32_t CanbusDriver::send(CanPacket packet)
for (uint8_t i = 0; i < packet.length; ++i)
{
if (i < 4)
- {
mailbox->TDLR |= packet.data[i] << i * 8;
- }
else
- {
mailbox->TDHR |= packet.data[i] << (i - 4) * 8;
- }
}
// Finally send the packet
@@ -336,13 +321,9 @@ void CanbusDriver::handleRXInterrupt(int fifo)
mailbox = &can->sFIFOMailBox[fifo];
if (fifo == 0)
- {
RFR = &can->RF0R;
- }
else
- {
RFR = &can->RF1R;
- }
status.fifoOverrun = (*RFR & CAN_RF0R_FOVR0) > 0;
status.fifoFull = (*RFR & CAN_RF0R_FULL0) > 0;
@@ -365,25 +346,18 @@ void CanbusDriver::handleRXInterrupt(int fifo)
p.rtr = (mailbox->RIR & CAN_RI0R_RTR) > 0;
if (p.ext)
- {
p.id = (mailbox->RIR >> 3) & 0x1FFFFFFF;
- }
else
- {
p.id = (mailbox->RIR >> 21) & 0x7FF;
- }
+
p.length = mailbox->RDTR & CAN_RDT0R_DLC;
for (uint8_t i = 0; i < p.length; i++)
{
if (i < 4) // Low register
- {
p.data[i] = (mailbox->RDLR >> (i * 8)) & 0xFF;
- }
else // High register
- {
p.data[i] = (mailbox->RDHR >> ((i - 4) * 8)) & 0xFF;
- }
}
*RFR |= CAN_RF0R_RFOM0;
diff --git a/src/shared/drivers/canbus/Canbus.h b/src/shared/drivers/canbus/CanDriver/CanDriver.h
similarity index 93%
rename from src/shared/drivers/canbus/Canbus.h
rename to src/shared/drivers/canbus/CanDriver/CanDriver.h
index b841096f29ade3f3ec47fb8af18198709075e4bf..f9d9b493268bd321000865bcfa0e05692ccc51f4 100644
--- a/src/shared/drivers/canbus/Canbus.h
+++ b/src/shared/drivers/canbus/CanDriver/CanDriver.h
@@ -26,7 +26,7 @@
#include <miosix.h>
#include <utils/collections/IRQCircularBuffer.h>
-#include "CanData.h"
+#include "CanDriverData.h"
#include "Filters.h"
using miosix::Thread;
@@ -38,8 +38,8 @@ namespace Canbus
{
/**
- * @brief Low level canbus driver, with support for both peripherals (CAN1 and
- * CAN2) on stm32f4 micros.
+ * @brief Low level CanBus driver, with support for both peripherals (CAN1 and
+ * CAN2) on stm32f4 microcontrollers.
*/
class CanbusDriver
{
@@ -56,7 +56,7 @@ class CanbusDriver
public:
/**
- * @brief Configuration struct for basic canbus operation.
+ * @brief Configuration struct for basic CanBus operation.
*/
struct CanbusConfig
{
@@ -81,7 +81,7 @@ public:
{
/**
* @brief Canbus baud rate in bps (BITS PER SECOND). CANOpen standard
- * values are preferred but not mandatory: 1000 kpbs, 500 kbps, 250
+ * values are preferred but not mandatory: 1000 kbps, 500 kbps, 250
* kbps, 125 kbps, 100 kbps, 83.333 kbps, 50 kbps, 25 kbps and 10 kbps.
*/
uint32_t baudRate;
@@ -93,7 +93,7 @@ public:
};
/**
- * @brief Struct specifing exact bit timing registers values.
+ * @brief Struct specifying exact bit timing registers values.
*/
struct BitTiming
{
@@ -133,7 +133,7 @@ public:
~CanbusDriver();
/**
- * @brief Exits initialization mode and starts canbus operation.
+ * @brief Exits initialization mode and starts CanBus operation.
*/
void init();
@@ -173,7 +173,7 @@ public:
}
/**
- * @brief Returns the canbus peripheral assigned to this instance.
+ * @brief Returns the CanBus peripheral assigned to this instance.
*/
CAN_TypeDef* getCAN() { return can; }
diff --git a/src/shared/drivers/canbus/CanData.h b/src/shared/drivers/canbus/CanDriver/CanDriverData.h
similarity index 96%
rename from src/shared/drivers/canbus/CanData.h
rename to src/shared/drivers/canbus/CanDriver/CanDriverData.h
index 8123c8514b1e25e58ea88eb04d453bb742c7b509..97e583073d3f823b35b79ea7dde0df4c7e2c599d 100644
--- a/src/shared/drivers/canbus/CanData.h
+++ b/src/shared/drivers/canbus/CanDriver/CanDriverData.h
@@ -54,7 +54,7 @@ struct CanPacket
uint32_t timestamp = 0;
uint32_t id;
- bool ext = false;
+ bool ext = false; ///< Whether to use extended packet id
bool rtr = false;
diff --git a/src/shared/drivers/canbus/CanInterrupt.cpp b/src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
similarity index 99%
rename from src/shared/drivers/canbus/CanInterrupt.cpp
rename to src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
index e0312b326dc917837e84f4faca875d6f22fa62c8..4ed47a191f8c8afa1a0716b7f1b0bfc08642cf47 100644
--- a/src/shared/drivers/canbus/CanInterrupt.cpp
+++ b/src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
@@ -26,7 +26,7 @@
#include <kernel/scheduler/scheduler.h>
#include <miosix.h>
-#include "Canbus.h"
+#include "CanDriver.h"
namespace Boardcore
{
@@ -98,9 +98,7 @@ void __attribute__((used)) CAN_RXIRQHandlerImpl(int canDev, int fifo)
(void)canDev;
if (canDrivers[canDev])
- {
canDrivers[canDev]->handleRXInterrupt(fifo);
- }
}
void __attribute__((used)) CAN_TXIRQHandlerImpl(int canDev)
diff --git a/src/shared/drivers/canbus/CanInterrupt.h b/src/shared/drivers/canbus/CanDriver/CanInterrupt.h
similarity index 100%
rename from src/shared/drivers/canbus/CanInterrupt.h
rename to src/shared/drivers/canbus/CanDriver/CanInterrupt.h
diff --git a/src/shared/drivers/canbus/Filters.h b/src/shared/drivers/canbus/CanDriver/Filters.h
similarity index 100%
rename from src/shared/drivers/canbus/Filters.h
rename to src/shared/drivers/canbus/CanDriver/Filters.h
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
deleted file mode 100644
index ae2384a0bc15a1bdc64ec462a05b755b7f755c4f..0000000000000000000000000000000000000000
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ /dev/null
@@ -1,236 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Federico Mandelli
- *
- * 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 <ActiveObject.h>
-#include <utils/Debug.h>
-#include <utils/collections/IRQCircularBuffer.h>
-
-#include "Canbus.h"
-
-#define NPACKET 3 // equals the number of boards in the can system
-
-/**
- * @brief Struct that contains how the canId is composed
- */
-struct CanIDMask
-{
- uint32_t priority = 0x1E000000;
- uint32_t type = 0x1F80000;
- uint32_t source = 0x78000;
- uint32_t destination = 0x7800;
- uint32_t idType = 0x780;
- uint32_t firstPacket = 0x40;
- uint32_t leftToSend = 0x3F;
-} idMask;
-
-namespace Boardcore
-{
-namespace Canbus
-{
-
-/**
- * @brief Generic struct that contains a logical can packet
- * i.e. 1 accelerometer packet 3*4byte (acc: x,y,z)+timestamp, will be 4
- * canPacket but a single canData.
- */
-struct CanData
-{
- int32_t canId =
- -1; // the id of the can packet without the last 7 bits (sequence bit)
- uint8_t len;
- uint8_t nRec = 0;
- uint64_t payload[32];
-} data[NPACKET];
-
-/**
- * @brief Canbus protocol, given an initialized can this class takes care of
- * sending the multiple packet of CanData with the corresponding id and
- * receiving single CanPacket that are then reframed as one Candata.
- */
-class CanProtocol : public ActiveObject
-{
-private:
- miosix::FastMutex
- mutex; // todo add mutex and create get data in can protocol
- CanbusDriver* can; // the physical can
- IRQCircularBuffer<CanData, NPACKET>
- buffer; // the buffer used to send data from CanProtocol to CanHandler
-
-public:
- /**
- * @brief Construct a new CanProtocol object
- * @param can CanbusDriver pointer.
- */
- CanProtocol(CanbusDriver* can) { this->can = can; }
-
- CanData
- getPacket() // return the packet, if buffer is empty return an empty packet
- {
- CanData temp;
- miosix::Lock<miosix::FastMutex> l(mutex);
- if (!buffer.isEmpty())
- {
- temp = buffer.pop();
- }
-
- return temp;
- }
-
- bool isEmpty()
- {
- miosix::Lock<miosix::FastMutex> l(mutex);
- return buffer.isEmpty();
- }
-
- void waitEmpty() { buffer.waitUntilNotEmpty(); }
-
- uint8_t byteForInt(uint64_t number)
- {
- uint8_t i;
- for (i = 1; i <= 8; i++)
- {
- number = number >> 8;
- if (number == 0)
- return i;
- }
- return i;
- }
- /**
- * @brief Takes a canData, it splits it into single canpacket with the
- * correct sequential id
- * @param toSend = containing the id e the data of the packet to send
- * @warning requires toSend to be not empty
- */
- void sendCan(CanData toSend) //@requires toSen to not be empty
- {
- CanPacket packet;
- uint8_t tempLen = toSend.len - 1;
- uint32_t tempId = toSend.canId;
- packet.ext = true;
- packet.id = (tempId << 7) | idMask.firstPacket |
- (63 - (tempLen & idMask.leftToSend));
- packet.length = byteForInt(toSend.payload[0]);
- for (int k = 0; k < packet.length; k++)
- {
- packet.data[k] = toSend.payload[0] >> (8 * k);
- }
- tempLen--;
- can->send(packet);
- for (int i = 1; i < toSend.len; i++)
- {
- tempId = toSend.canId;
- packet.id = (tempId << 7) | !(idMask.firstPacket) |
- (63 - (tempLen & idMask.leftToSend));
- packet.length = byteForInt(toSend.payload[i]);
- for (int k = 0; k < packet.length; k++)
- {
- packet.data[k] = toSend.payload[i] >> (8 * k);
- }
- can->send(packet);
- tempLen--;
- }
- }
-
-protected:
- /**
- * @brief Keeps listening on hte canbus for packets, once received it checks
- * if they are expected (that id is already present in data), if they are
- * they are added to the list. once we receive the correct amount of packet
- * we send it to can handler.
- */
- void run() override // for now if a packet is missed/received in the wrong
- // order the whole packet will be lost once we receive
- // a new first packet without warning canhandler
- {
- uint32_t sourceId;
- CanPacket packet;
- // Infinite loop
- while (true)
- {
- can->getRXBuffer().waitUntilNotEmpty();
- if (!can->getRXBuffer().isEmpty())
- {
-
- packet = can->getRXBuffer().pop().packet;
- sourceId = (packet.id & idMask.source) >> 15;
- if (sourceId >= 0 &&
- sourceId < NPACKET) // check for maximum size
- {
- if (data[sourceId].canId == -1 ||
- (((data[sourceId].canId << 7) & idMask.source) >> 15) ==
- sourceId)
- {
- uint32_t leftToSend =
- 63 - (packet.id & idMask.leftToSend);
-
- if ((packet.id & idMask.firstPacket) >>
- 6) // it is a first
- // packet of a data;
- {
-
- data[sourceId].len = leftToSend + 1;
- data[sourceId].canId =
- packet.id >> 7; // discard the sequence number
- }
- if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
- leftToSend)
- {
-
- uint64_t tempPayload = 0;
- for (int f = 0; f < packet.length; f++)
- {
- uint64_t tempData = packet.data[f];
- tempPayload =
- tempPayload | (tempData << (f * 8));
- }
- if (data[sourceId].len - leftToSend - 1 >= 0 &&
- data[sourceId].len - leftToSend - 1 <
- 32) // check for index
- {
-
- data[sourceId].payload[data[sourceId].len -
- leftToSend - 1] =
- tempPayload;
- data[sourceId].nRec++;
- }
- }
-
- if (data[sourceId].nRec == data[sourceId].len &&
- data[sourceId].nRec != 0)
- {
- miosix::Lock<miosix::FastMutex> l(mutex);
- buffer.put(data[sourceId]);
- // empties the struct
- data[sourceId].canId = -1;
- data[sourceId].nRec = 0;
- data[sourceId].len = 0;
- }
- }
- }
- }
- }
- }
-};
-} // namespace Canbus
-} // namespace Boardcore
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f8454775b2842f3feb099b0d088397d50832149d
--- /dev/null
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -0,0 +1,283 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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 "CanProtocol.h"
+
+using namespace miosix;
+
+namespace Boardcore
+{
+
+namespace Canbus
+{
+
+CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler onReceive)
+ : can(can), onReceive(onReceive)
+{
+}
+
+bool CanProtocol::start()
+{
+ stopFlag = false;
+
+ // Start sender (joinable thread)
+ if (!sndStarted)
+ {
+ sndThread = miosix::Thread::create(
+ sndLauncher, skywardStack(4 * 1024), miosix::MAIN_PRIORITY,
+ reinterpret_cast<void*>(this), miosix::Thread::JOINABLE);
+
+ if (sndThread != nullptr)
+ sndStarted = true;
+ else
+ LOG_ERR(logger, "Could not start sender!");
+ }
+
+ // Start receiver
+ if (!rcvStarted)
+ {
+ rcvThread = miosix::Thread::create(rcvLauncher, skywardStack(4 * 1024),
+ miosix::MAIN_PRIORITY,
+ reinterpret_cast<void*>(this));
+
+ if (rcvThread != nullptr)
+ rcvStarted = true;
+ else
+ LOG_ERR(logger, "Could not start receiver!");
+ }
+
+ if (sndStarted && rcvStarted)
+ LOG_DEBUG(logger, "Sender and receiver started");
+
+ return sndStarted && rcvStarted;
+}
+
+bool CanProtocol::isStarted() { return sndStarted && rcvStarted; }
+
+void CanProtocol::stop()
+{
+ stopFlag = true;
+
+ // Wait for sender to stop
+ sndThread->join();
+}
+
+bool CanProtocol::enqueueMsg(const CanMessage& msg)
+{
+ // Append the message to the queue
+ outQueue.put(msg);
+
+ // Update stats
+ // updateQueueStats(appended);
+
+ // Return always true because the circular buffer overrides current packets
+ // and can't get full.
+ return true;
+}
+
+bool CanProtocol::addFilter(uint8_t src, uint64_t dst)
+{
+ if (src > 0xF || dst > 0xF)
+ return false;
+
+ // The filter mask will cover only the source and destination bits
+ uint32_t mask = static_cast<uint32_t>(CanProtocolIdMask::SOURCE) |
+ static_cast<uint32_t>(CanProtocolIdMask::DESTINATION);
+
+ uint32_t id =
+ src << static_cast<uint8_t>(CanProtocolShiftInformation::SOURCE) |
+ dst << static_cast<uint8_t>(CanProtocolShiftInformation::DESTINATION);
+
+ Mask32FilterBank filterBank(id, mask, 1, 1, 0, 0, 0);
+
+ return can->addFilter(filterBank);
+}
+
+void CanProtocol::sendMessage(const CanMessage& msg)
+{
+ CanPacket packet = {};
+ uint32_t leftToSend = msg.length - 1;
+
+ // Create the id for the first packet
+ packet.ext = true; // Use extended packet id
+
+ // The number of left to send packets
+ packet.id = static_cast<uint32_t>(msg.id) |
+ ((static_cast<uint32_t>(0x3F) - leftToSend) &
+ static_cast<uint32_t>(CanProtocolIdMask::LEFT_TO_SEND));
+ packet.length = byteForUint64(msg.payload[0]);
+
+ // Splits payload[0] in the right number of uint8_t
+ for (int i = 0; i < packet.length; i++)
+ packet.data[i] = msg.payload[0] >> (8 * i);
+
+ // Send the first packet
+ can->send(packet);
+ leftToSend--;
+
+ // Prepare the remaining packets
+ for (int i = 1; i < msg.length; i++)
+ {
+ packet.id =
+ static_cast<uint32_t>(msg.id) |
+ static_cast<uint32_t>(CanProtocolIdMask::FIRST_PACKET_FLAG) |
+ ((static_cast<uint32_t>(0x3F) - leftToSend) &
+ static_cast<uint32_t>(CanProtocolIdMask::LEFT_TO_SEND));
+ packet.length = byteForUint64(msg.payload[i]);
+
+ // Splits payload[i] in the right number of uint8_t
+ for (int k = 0; k < packet.length; k++)
+ packet.data[k] = msg.payload[i] >> (8 * k);
+
+ can->send(packet);
+ leftToSend--;
+ }
+}
+
+void CanProtocol::runReceiver()
+{
+ CanMessage msg;
+ uint8_t nReceived = 0;
+
+ while (!stopFlag)
+ {
+ // Wait for the next packet
+ can->getRXBuffer().waitUntilNotEmpty();
+
+ // If the buffer is not empty retrieve the packet
+ if (!can->getRXBuffer().isEmpty())
+ {
+ CanPacket pkt = can->getRXBuffer().pop().packet;
+
+ uint8_t leftToReceive =
+ static_cast<uint32_t>(0x3F) -
+ (pkt.id &
+ static_cast<uint32_t>(CanProtocolIdMask::LEFT_TO_SEND));
+
+ // Check if the packet is the first in the sequence, if this is the
+ // case then the previous message is overriden
+ if ((pkt.id & static_cast<uint32_t>(
+ CanProtocolIdMask::FIRST_PACKET_FLAG)) == 0)
+ {
+ // If it is we save the id (without the sequence number) and the
+ // message length
+ msg.id = pkt.id & static_cast<uint32_t>(
+ CanProtocolIdMask::MESSAGE_INFORMATION);
+ msg.length = leftToReceive + 1;
+
+ // Reset the number of received packets
+ nReceived = 0;
+ }
+
+ // Accept the packet only if it has the expected id
+ // clang-format off
+ if (msg.id != -1 &&
+ (pkt.id & static_cast<uint32_t>(CanProtocolIdMask::MESSAGE_INFORMATION)) ==
+ (msg.id & static_cast<uint32_t>(CanProtocolIdMask::MESSAGE_INFORMATION)))
+ // clang-format on
+ {
+ // Check if the packet is expected in the sequence. The received
+ // packet must have the expected left to send value
+
+ if (msg.length - nReceived - 1 == leftToReceive)
+ {
+ uint64_t payload = 0;
+
+ // Assemble the packet data into a uint64_t
+ for (uint8_t i = 0; i < pkt.length; i++)
+ payload |= pkt.data[i] << (i * 8);
+
+ // Add the data to the message
+ msg.payload[pkt.length - leftToReceive - 1] = payload;
+ nReceived++;
+ }
+ }
+
+ // If we have received the right number of packet call onReceive and
+ // reset the message
+ if (nReceived == msg.length && nReceived != 0)
+ {
+ onReceive(msg);
+
+ // Reset the packet
+ msg.id = -1;
+ msg.length = 0;
+ }
+ }
+ }
+}
+
+void CanProtocol::runSender()
+{
+ LOG_DEBUG(logger, "Sender is running");
+ CanMessage msg;
+
+ while (!stopFlag)
+ {
+ outQueue.waitUntilNotEmpty();
+
+ if (!outQueue.isEmpty())
+ {
+ // Get the first packet in the queue, without removing it
+ msg = outQueue.pop();
+
+ LOG_DEBUG(logger, "Sending message, length: {}", msg.length);
+
+ sendMessage(msg);
+
+ // updateSenderStats();
+ }
+ else
+ {
+ // Wait before sending something else
+ miosix::Thread::sleep(50);
+ }
+ }
+}
+
+void CanProtocol::rcvLauncher(void* arg)
+{
+ reinterpret_cast<CanProtocol*>(arg)->runReceiver();
+}
+
+void CanProtocol::sndLauncher(void* arg)
+{
+ reinterpret_cast<CanProtocol*>(arg)->runSender();
+}
+
+uint8_t CanProtocol::byteForUint64(uint64_t number)
+{
+ uint8_t i;
+
+ for (i = 1; i <= 8; i++)
+ {
+ number >>= 8;
+ if (number == 0)
+ return i;
+ }
+
+ return i;
+}
+
+} // namespace Canbus
+
+} // namespace Boardcore
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
new file mode 100644
index 0000000000000000000000000000000000000000..a8792d7c08016fbbd3f9192d2bd3927bb1a4b962
--- /dev/null
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
@@ -0,0 +1,197 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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/canbus/CanDriver/CanDriver.h>
+#include <utils/Debug.h>
+#include <utils/collections/SyncCircularBuffer.h>
+
+#include "CanProtocolData.h"
+#include "CanProtocolTypes.h"
+
+namespace Boardcore
+{
+
+namespace Canbus
+{
+
+/**
+ * @brief Canbus protocol implementation.
+ *
+ * Given a can interface this class takes care of sending messages segmented
+ * into multiple packets, and receiving single packets that are then reframed
+ * into messages.
+ *
+ * This driver has been implemented following the MavlinkDriver.
+ */
+class CanProtocol
+{
+ using MsgHandler = std::function<void(CanMessage data)>;
+
+public:
+ /**
+ * @brief Construct a new CanProtocol object.
+ *
+ * @param can Pointer to a CanbusDriver object.
+ */
+ CanProtocol(CanbusDriver* can, MsgHandler onReceive);
+
+ /**
+ * @brief Start the receiving and sending threads.
+ *
+ * @return False if at least one could not start.
+ */
+ bool start();
+
+ /**
+ * @brief Tells whether the driver was started.
+ */
+ bool isStarted();
+
+ /**
+ * @brief Stops sender and receiver threads.
+ */
+ void stop();
+
+ /**
+ * @brief Adds a filter to the can peripheral to receive only messages from
+ * the given source and targeted to the given destination.
+ *
+ * @param src Message source.
+ * @param dst Message destination.
+ * @return True if the filter was added successfully.
+ */
+ bool addFilter(uint8_t src, uint64_t dst);
+
+ /**
+ * @brief Non-blocking send function, puts the messages in a queue.
+ * Message is discarded if the queue is full.
+ *
+ * @param msg Message to send (CanMessage struct).
+ * @return True if the message could be enqueued.
+ */
+ bool enqueueMsg(const CanMessage& msg);
+
+ /**
+ * @brief Non-blocking send function for a generic data type.
+ *
+ * @warning There must be a function called with this prototype:
+ * CanMessage toCanMessage(const T& t);
+ *
+ * @param t The class to be logged.
+ */
+ template <typename T>
+ bool enqueueData(uint8_t priority, uint8_t primaryType, uint8_t source,
+ uint8_t destination, uint8_t secondaryType, const T& t);
+
+private:
+ /**
+ * @brief Blocking send function, puts the CanMessage object on the bus.
+ *
+ * Takes a CanMessage object, splits it into multiple CanPackets with the
+ * correct sequential id.
+ *
+ * @param msg Contains the id and the data of the packet to send.
+ */
+ void sendMessage(const CanMessage& msg);
+
+ /**
+ * @brief Receiver thread: waits for one packet at a time from the can
+ * driver and tries to parse a message.
+ *
+ * If the message is successfully parsed, the onReceive function is
+ * executed.
+ *
+ * For now if a packet is received in the wrong order or if a packet with a
+ * different id is received, the current (incomplete) message will be lost.
+ * Once we receive a new first packet, currently saved data are reset.
+ */
+ void runReceiver();
+
+ /**
+ * @brief Sender Thread: Periodically flushes the message queue and sends
+ * all the enqueued messages.
+ */
+ void runSender();
+
+ /**
+ * @brief Calls the run member function.
+ *
+ * @param arg The object pointer cast to void*.
+ */
+ static void rcvLauncher(void* arg);
+
+ /**
+ * @brief Calls the run member function.
+ *
+ * @param arg The object pointer cast to void*.
+ */
+ static void sndLauncher(void* arg);
+
+ /**
+ * @brief Count the number of bytes needed to encode a uint64_t number.
+ */
+ uint8_t byteForUint64(uint64_t number);
+
+ CanbusDriver* can; ///< Device used to send and receive packets.
+ MsgHandler onReceive; ///< Function executed when a message is ready.
+
+ // Threads
+ bool stopFlag = false;
+ bool sndStarted = false;
+ bool rcvStarted = false;
+
+ miosix::Thread* sndThread = nullptr;
+ miosix::Thread* rcvThread = nullptr;
+
+ SyncCircularBuffer<CanMessage, 10> outQueue;
+
+ PrintLogger logger = Logging::getLogger("canprotocol");
+};
+
+template <typename T>
+bool CanProtocol::enqueueData(uint8_t priority, uint8_t primaryType,
+ uint8_t source, uint8_t destination,
+ uint8_t secondaryType, const T& t)
+{
+ if (priority > 0xF || primaryType > 0x3F || source > 0xF ||
+ destination > 0xF || secondaryType > 0xF)
+ return false;
+
+ CanMessage msg = toCanMessage(t);
+
+ // clang-format off
+ msg.id = priority << static_cast<uint32_t>(CanProtocolShiftInformation::PRIORITY);
+ msg.id |= primaryType << static_cast<uint32_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
+ msg.id |= source << static_cast<uint32_t>(CanProtocolShiftInformation::SOURCE);
+ msg.id |= destination << static_cast<uint32_t>(CanProtocolShiftInformation::DESTINATION);
+ msg.id |= secondaryType << static_cast<uint32_t>(CanProtocolShiftInformation::SECONDARY_TYPE);
+ // clang-format off
+
+ return enqueueMsg(msg);
+}
+
+} // namespace Canbus
+
+} // namespace Boardcore
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a2b3b62e9835dddd237d74915342fc7e618ca4e
--- /dev/null
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
@@ -0,0 +1,184 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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 <stdint.h>
+
+namespace Boardcore
+{
+
+namespace Canbus
+{
+
+/**
+ * The CanProtocol allows to transmit arbitrarily sized messages over the CanBus
+ * overcoming the 8 byte limitation of each single packet.
+ *
+ * Our CanProtocol uses the extended can packet, the 29 bits id is divided such
+ * as:
+ * - Priority 4 bit \
+ * - Primary type 6 bit |
+ * - Source 4 bit | 22 bits - Message informations
+ * - Destination 4 bit |
+ * - Secondary type 4 bit /
+ * - First packet flag 1 bit \ 7 bits - Sequential informations
+ * - Remaining packets 6 bit /
+ * shiftNameOfField the number of shift needed to reach that field
+ *
+ * The id is split into 2 parts:
+ * - Message information: Common to every packet of a given message
+ * - Sequential information: Used to distinguish between packets
+ *
+ * The sender splits into multiple packets a message that is then recomposed on
+ * the receiver end. The message informations are encoded into the packets id,
+ * therefore they have an effect on packets priorities.
+ */
+/**
+ * The CanProtocol allows to transmit arbitrarily sized messages over the CanBus
+ * overcoming the 8 byte limitation of each single packet.
+ *
+ * Our CanProtocol uses the extended can packet, the 29 bits id is divided such
+ * as:
+ * - Priority 4 bit \
+ * - Primary type 6 bit |
+ * - Source 4 bit | 22 bits - Message informations
+ * - Destination 4 bit |
+ * - Secondary type 4 bit /
+ * - First packet flag 1 bit \ 7 bits - Sequential informations
+ * - Remaining packets 6 bit /
+ * shiftNameOfField the number of shift needed to reach that field
+ *
+ * The id is split into 2 parts:
+ * - Message information: Common to every packet of a given message
+ * - Sequential information: Used to distinguish between packets
+ *
+ * The sender splits into multiple packets a message that is then recomposed on
+ * the receiver end. The message informations are encoded into the packets id,
+ * therefore they have an effect on packets priorities.
+ */
+
+/**
+ * @brief Masks of the elements composing can packets ids.
+ */
+enum class CanProtocolIdMask : uint32_t
+{
+ PRIORITY = 0x1E000000,
+ PRIMARY_TYPE = 0x01F80000,
+ SOURCE = 0x00078000,
+ DESTINATION = 0x00003800,
+ SECONDARY_TYPE = 0x00000780,
+
+ MESSAGE_INFORMATION = 0x1FFFFF80,
+
+ FIRST_PACKET_FLAG = 0x00000040,
+ LEFT_TO_SEND = 0x0000003F,
+
+ SEQUENTIAL_INFORMATION = 0x0000007F
+};
+
+enum CanProtocolShiftInformation : uint8_t
+{
+ // Shift values for message informations
+ PRIORITY = 25,
+ PRIMARY_TYPE = 19,
+ SOURCE = 15,
+ DESTINATION = 11,
+ SECONDARY_TYPE = 7,
+
+ // Shift values for sequential informations
+ FIRST_PACKET_FLAG = 6,
+ LEFT_TO_SEND = 0,
+
+ // Position of the message infos relative to the entire can packet id
+ SEQUENTIAL_INFORMATION = 7
+};
+
+/**
+ * @brief Generic struct that contains a can protocol message.
+ *
+ * For example an accelerometer message could have:
+ * - 4 bytes for the timestamp
+ * - 3x4 bytes for float values
+ * This message would be divided into 2 can packets.
+ *
+ * Note that the maximum size for a message is 520 bytes since the remaining
+ * packet information is 6 bit wide.
+ */
+struct CanMessage
+{
+ int32_t id = -1; ///< Id of the message without sequential infos.
+ uint8_t length = 0; ///< Length of the message content.
+ uint64_t payload[65];
+
+ uint8_t getPriority()
+ {
+ return id >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::PRIORITY);
+ }
+
+ uint8_t getPrimaryType()
+ {
+ return (id | static_cast<uint32_t>(CanProtocolIdMask::PRIMARY_TYPE)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
+ }
+
+ uint8_t getSource()
+ {
+ return (id | static_cast<uint32_t>(CanProtocolIdMask::SOURCE)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::SOURCE);
+ }
+
+ uint8_t getDestination()
+ {
+ return (id | static_cast<uint32_t>(CanProtocolIdMask::DESTINATION)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::DESTINATION);
+ }
+
+ uint8_t getSecondaryType()
+ {
+ return (id |
+ static_cast<uint32_t>(CanProtocolIdMask::SECONDARY_TYPE)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
+ }
+};
+
+inline bool operator==(const CanMessage& lhs, const CanMessage& rhs)
+{
+ if (lhs.id != rhs.id || lhs.length != rhs.length)
+ return false;
+
+ for (int i = 0; i < lhs.length; i++)
+ if (lhs.payload[i] != rhs.payload[i])
+ return false;
+
+ return true;
+}
+
+inline bool operator!=(const CanMessage& lhs, const CanMessage& rhs)
+{
+ return !(lhs == rhs);
+}
+
+} // namespace Canbus
+
+} // namespace Boardcore
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h
new file mode 100644
index 0000000000000000000000000000000000000000..b84db80a9c1e530dc9666af6c2e9951a7e3ed7f6
--- /dev/null
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h
@@ -0,0 +1,49 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: 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
+ * 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 <sensors/analog/Pitot/PitotData.h>
+
+#include <cstring>
+
+#include "CanProtocolData.h"
+
+namespace Boardcore
+{
+
+inline Canbus::CanMessage toCanMessage(const PitotData& data)
+{
+ Canbus::CanMessage message;
+
+ uint32_t tmp;
+ memcpy(&tmp, &(data.airspeed), sizeof(tmp));
+
+ message.id = -1;
+ message.length = 1;
+ message.payload[0] = (data.timestamp & ~0x3) << 30;
+ message.payload[0] |= tmp;
+
+ return message;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/logger/Logger.h b/src/shared/logger/Logger.h
index 496b1cf311900f722a7070447cb9234f864791f4..7d404ae83eb064d15f9ef793e0f3cac9867eb11b 100644
--- a/src/shared/logger/Logger.h
+++ b/src/shared/logger/Logger.h
@@ -124,7 +124,7 @@ public:
*/
void logStats();
-public:
+private:
Logger();
static std::string getFileName(int logNumber);
diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index 1bf8c8047e51277aa25155f5221b9ac51625ffa6..81607862cfe14f10b7de94b1faf00bd2fcce23bb 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -22,15 +22,12 @@
#pragma once
-#include <diagnostic/PrintLogger.h>
-
#include <vector>
/**
* This object includes only the protocol header (`protocol.h`). To use this
* driver, you should include YOUR OWN implementation of the messages definition
- * (`mavlink.h`) before including this header. To create your implementation you
- * can use Skyward's *Mavlink Editor*.
+ * (`mavlink.h`) before including this header.
*/
#ifndef MAVLINK_H
#error \
@@ -38,6 +35,7 @@
implementation before including MavlinkDriver.h"
#endif
+#include <diagnostic/PrintLogger.h>
#include <diagnostic/SkywardStack.h>
#include <diagnostic/StackLogger.h>
#include <mavlink_lib/mavlink_types.h>
@@ -84,7 +82,8 @@ public:
/**
* @brief Start the receiving and sending threads.
- * @return false if at least one could not start.
+ *
+ * @return False if at least one could not start.
*/
bool start();
@@ -116,6 +115,17 @@ public:
*/
bool enqueueRaw(uint8_t* msg, size_t size);
+ /**
+ * @brief Synchronized status getter.
+ */
+ MavlinkStatus getStatus();
+
+ /**
+ * @brief Setter for the sleep after send value.
+ */
+ void setSleepAfterSend(uint16_t newSleepTime);
+
+private:
/**
* @brief Receiver thread: reads one char at a time from the transceiver and
* tries to parse a mavlink message.
@@ -134,21 +144,10 @@ public:
*/
void runSender();
- /**
- * @brief Synchronized status getter.
- */
- MavlinkStatus getStatus();
-
- /**
- * @brief Setter for the sleep after send value.
- */
- void setSleepAfterSend(uint16_t newSleepTime);
-
-private:
/**
* @brief Calls the run member function.
*
- * @param arg the object pointer cast to void*
+ * @param arg The object pointer cast to void*.
*/
static void rcvLauncher(void* arg)
{
@@ -158,7 +157,7 @@ private:
/**
* @brief Calls the run member function.
*
- * @param arg the object pointer cast to void*
+ * @param arg The object pointer cast to void*.
*/
static void sndLauncher(void* arg)
{
@@ -169,8 +168,8 @@ private:
void updateSenderStats(size_t msgCount, bool sent);
- Transceiver* device; ///< transceiver used to send and receive
- MavHandler onReceive; ///< function executed on message rcv
+ Transceiver* device; ///< Transceiver used to send and receive packets.
+ MavHandler onReceive; ///< Function executed when a message is ready.
// Tweakable params
uint16_t sleepAfterSend;
@@ -273,7 +272,7 @@ bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::enqueueMsg(
uint8_t msgTempBuf[MAVLINK_NUM_NON_PAYLOAD_BYTES + MavMsgLength];
int msgLen = mavlink_msg_to_send_buffer(msgTempBuf, &msg);
- // Append message to the queue
+ // Append the message to the queue
bool appended = outQueue.put(msgTempBuf, msgLen);
// Update stats
diff --git a/src/tests/drivers/canbus/SimpleCanManager.h b/src/tests/drivers/canbus/CanDriver/SimpleCanManager.h
similarity index 95%
rename from src/tests/drivers/canbus/SimpleCanManager.h
rename to src/tests/drivers/canbus/CanDriver/SimpleCanManager.h
index f7ff5a59d5c65a83500948b128f2f0714c344701..141c5a60e2fe12c72607cd171f35231313874559 100644
--- a/src/tests/drivers/canbus/SimpleCanManager.h
+++ b/src/tests/drivers/canbus/CanDriver/SimpleCanManager.h
@@ -21,16 +21,16 @@
*/
#pragma once
+
+#include <ActiveObject.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
#include <miosix.h>
+#include <utils/collections/SyncCircularBuffer.h>
#include <cstdlib>
#include <functional>
-#include "ActiveObject.h"
-#include "drivers/canbus/BusLoadEstimation.h"
-#include "drivers/canbus/Canbus.h"
-#include "utils/collections/SyncCircularBuffer.h"
-
using std::function;
class SimpleCanManager
diff --git a/src/tests/drivers/canbus/test-canbus-2way.cpp b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
similarity index 96%
rename from src/tests/drivers/canbus/test-canbus-2way.cpp
rename to src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
index 91b69f5c2b9589fc67bfe5c843f0ed3eadad35b9..1a22bdfc874f306cfa4222838a19cd6b7ac02628 100644
--- a/src/tests/drivers/canbus/test-canbus-2way.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
@@ -25,17 +25,17 @@
// considered lost. It may also receive requests from other canbus devices, to
// which it will respond
+#include <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
#include <utils/Debug.h>
#include <utils/Stats/Stats.h>
+#include <utils/collections/CircularBuffer.h>
#include <string>
-#include "ActiveObject.h"
#include "SimpleCanManager.h"
-#include "diagnostic/PrintLogger.h"
-#include "drivers/canbus/BusLoadEstimation.h"
-#include "drivers/canbus/Canbus.h"
-#include "utils/collections/CircularBuffer.h"
constexpr uint32_t BAUD_RATE = 500 * 1000;
constexpr float SAMPLE_POINT = 87.5f / 100.0f;
diff --git a/src/tests/drivers/canbus/test-canbus-filters.cpp b/src/tests/drivers/canbus/CanDriver/test-can-filters.cpp
similarity index 97%
rename from src/tests/drivers/canbus/test-canbus-filters.cpp
rename to src/tests/drivers/canbus/CanDriver/test-can-filters.cpp
index 64c8868840454afd28abcb7506592713ceb08095..b6c3f6b667ffcac3ff1021d99634f5430ec08b78 100644
--- a/src/tests/drivers/canbus/test-canbus-filters.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-filters.cpp
@@ -22,8 +22,8 @@
#include <ActiveObject.h>
#include <diagnostic/PrintLogger.h>
-#include <drivers/canbus/BusLoadEstimation.h>
-#include <drivers/canbus/Canbus.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
#include <string>
diff --git a/src/tests/drivers/canbus/test-canbus-loopback.cpp b/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
similarity index 96%
rename from src/tests/drivers/canbus/test-canbus-loopback.cpp
rename to src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
index d01ac89a1eee286837069dde8cbd8c8eef46e28e..06559020e6eaee2e933d2ce66696ec2a1c5baf44 100644
--- a/src/tests/drivers/canbus/test-canbus-loopback.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
@@ -20,12 +20,12 @@
* THE SOFTWARE.
*/
-#include <string>
+#include <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
-#include "ActiveObject.h"
-#include "diagnostic/PrintLogger.h"
-#include "drivers/canbus/BusLoadEstimation.h"
-#include "drivers/canbus/Canbus.h"
+#include <string>
using std::string;
using namespace Boardcore;
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2fcc424502a10db5c3936d2924a264120219e2cd
--- /dev/null
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
@@ -0,0 +1,88 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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/canbus/CanProtocol/CanProtocol.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <scheduler/TaskScheduler.h>
+
+using namespace std;
+using namespace miosix;
+using namespace Boardcore;
+using namespace Canbus;
+
+void print(CanMessage data)
+{
+ printf("Received packet:\n");
+ printf("\tpriority: %d\n", data.getPriority());
+ printf("\tprimary type: %d\n", data.getPrimaryType());
+ printf("\tsource: %d\n", data.getSource());
+ printf("\tdestination: %d\n", data.getDestination());
+ printf("\tsecondary type: %d\n", data.getSecondaryType());
+ printf("\n");
+}
+
+int main()
+{
+ // Prepare the cab driver
+ CanbusDriver::CanbusConfig config;
+ config.loopback = true;
+ CanbusDriver::AutoBitTiming bitTiming;
+ bitTiming.baudRate = 500 * 1000;
+ bitTiming.samplePoint = 87.5f / 100.0f;
+ CanbusDriver* driver = new CanbusDriver(CAN1, config, bitTiming);
+
+ // Prepare the can driver
+ CanProtocol protocol(driver, print);
+
+ // Add a filter to allow every message
+ Mask32FilterBank f2(0, 0, 1, 1, 0, 0, 0);
+ driver->addFilter(f2);
+ driver->init();
+
+ // Start the protocol
+ protocol.start();
+
+ CanMessage msg1;
+ msg1.id = 0x200;
+ msg1.length = 2;
+ msg1.payload[0] = 0xffffffffffffffff;
+ msg1.payload[1] = 0x0123456789ABCDEF;
+
+ TaskScheduler scheduler;
+
+ scheduler.addTask([&]() { protocol.enqueueMsg(msg1); }, 1000);
+ scheduler.addTask(
+ [&]()
+ {
+ PitotData data{TimestampTimer::getTimestamp(), 23};
+
+ protocol.enqueueData(0xF, 0xA, 0x1, 0x2, 0xB, data);
+ },
+ 2000);
+
+ scheduler.start();
+
+ printf("Started\n");
+
+ while (true)
+ Thread::sleep(1000);
+}
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
deleted file mode 100644
index 9f8eef1ed68be96fccad479a789df19a701d2ba5..0000000000000000000000000000000000000000
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-
-
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Federico Mandelli
- *
- * 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/canbus/CanProtocol.h>
-
-#include <thread>
-
-#include "drivers/canbus/BusLoadEstimation.h"
-#include "drivers/canbus/Canbus.h"
-#include "utils/collections/CircularBuffer.h"
-
-constexpr uint32_t BAUD_RATE = 500 * 1000;
-constexpr float SAMPLE_POINT = 87.5f / 100.0f;
-constexpr uint32_t MSG_DEADLINE = 100; // ms
-constexpr uint32_t MSG_LOST_DEADLINE = 400; // ms
-
-using std::string;
-using namespace Boardcore;
-using namespace Boardcore::Canbus;
-using namespace miosix;
-
-#ifdef _ARCH_CORTEXM3_STM32
-using CanRX = Gpio<GPIOA_BASE, 11>;
-using CanTX = Gpio<GPIOA_BASE, 12>;
-#else
-using CanRX = Gpio<GPIOA_BASE, 11>;
-using CanTX = Gpio<GPIOA_BASE, 12>;
-#endif
-
-#define SLP 100
-miosix::FastMutex mutex;
-CanData toSend1;
-CanData toSend2;
-void sendData(CanProtocol* protocol, CanData* toSend)
-{
- while (true)
- {
-
- TRACE("send\n");
- {
- miosix::Lock<miosix::FastMutex> l(mutex);
- (*protocol).sendCan(*toSend);
- }
- Thread::sleep(SLP);
- }
-}
-bool equal(CanData* first, CanData* second)
-{
- if ((*first).canId != (*second).canId || (*first).len != (*second).len)
- {
- return false;
- }
- for (int i = 0; i < (*first).len; i++)
- {
- if ((*first).payload[i] != (*second).payload[i])
- return false;
- }
- return true;
-}
-
-int main()
-{
- {
- miosix::FastInterruptDisableLock dLock;
-
-#ifdef _ARCH_CORTEXM3_STM32
- CanRX::mode(Mode::ALTERNATE);
- CanTX::mode(Mode::ALTERNATE);
-#else
- CanRX::mode(Mode::ALTERNATE);
- CanTX::mode(Mode::ALTERNATE);
-
- CanRX::alternateFunction(9);
- CanTX::alternateFunction(9);
-#endif
- }
- CanbusDriver::CanbusConfig cfg{};
- CanbusDriver::AutoBitTiming bt;
- bt.baudRate = BAUD_RATE;
- bt.samplePoint = SAMPLE_POINT;
- CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
- CanProtocol protocol(c);
- // Allow every message
- Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
-
- c->addFilter(f2);
- c->init();
- protocol.start();
-
- toSend1.canId = 0x200;
- toSend1.len = 8;
- toSend1.payload[0] = 0xffffffffffffffff;
- toSend1.payload[1] = 2;
- toSend1.payload[2] = 78022;
- toSend1.payload[3] = 0xfffffffffffff;
- toSend1.payload[4] = 23;
- toSend1.payload[5] = 3234;
- toSend1.payload[6] = 12;
- toSend1.payload[7] = 0;
- std::thread firstSend(sendData, &protocol, &toSend1);
-
- Thread::sleep(10);
- toSend2.canId = 0x100;
- toSend2.len = 4;
- toSend2.payload[0] = 0xffffff;
- toSend2.payload[1] = 2;
- toSend2.payload[2] = 0x123ff;
- toSend2.payload[3] = 1;
- std::thread secondSend(sendData, &protocol, &toSend2);
- TRACE("start \n");
- for (;;)
- {
-
- protocol.waitEmpty();
- CanData temp = protocol.getPacket();
- TRACE("received packet \n");
- if ((!equal(&temp, &toSend1) && !equal(&temp, &toSend2)))
- {
- TRACE("Error\n");
- TRACE("Received %lu\n", temp.canId);
- TRACE("Received %d\n", temp.len);
- for (int i = 0; i < temp.len; i++)
- {
- TRACE("Received payload %d: %llu,\n", i, temp.payload[i]);
- }
- }
- else
- {
- TRACE("OK :) id %lu\n", temp.canId);
- }
- }
-}