diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 60f998772c0c5f0ef8c2031dbe4488f4e8df4c2b..57a4927e6f94dd4a159dbf61a5dd597bca8fc853 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -69,6 +69,7 @@ set(RIG_COMPUTER ) set(RIG_V2_COMPUTER + src/boards/RIGv2/Radio/Radio.cpp src/boards/RIGv2/Sensors/Sensors.cpp ) diff --git a/src/boards/RIGv2/Buses.h b/src/boards/RIGv2/Buses.h index d4fbd3463c000b86123b49c34729b866e9dc2511..78ec1fa779d049da76f39da2296d7e0744a92316 100644 --- a/src/boards/RIGv2/Buses.h +++ b/src/boards/RIGv2/Buses.h @@ -48,6 +48,7 @@ public: Boardcore::SPIBus &getADS131M08_2() { return spi3; } Boardcore::SPIBus &getMAX31856_1() { return spi3; } Boardcore::SPIBus &getMAX31856_2() { return spi1; } + Boardcore::SPIBus &getRadio() { return spi6; } }; } // namespace RIGv2 \ No newline at end of file diff --git a/src/boards/RIGv2/Configs/RadioConfig.h b/src/boards/RIGv2/Configs/RadioConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..23801aae1e71fa0e1e3dee35f5c2814170347087 --- /dev/null +++ b/src/boards/RIGv2/Configs/RadioConfig.h @@ -0,0 +1,51 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <common/Mavlink.h> + +namespace RIGv2 +{ + +namespace Config +{ + +namespace Radio +{ + +static constexpr unsigned int MAV_OUT_QUEUE_SIZE = 20; +static constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; + +static constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0; +static constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10; + +static constexpr unsigned int CIRCULAR_BUFFER_SIZE = 8; + +static constexpr uint8_t MAV_SYSTEM_ID = 171; +static constexpr uint8_t MAV_COMPONENT_ID = 96; + +} // namespace Radio + +} // namespace Config + +} // namespace RIGv2 \ No newline at end of file diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3d649ecc4da0c698bc384c7b48593a11fa4cd18c --- /dev/null +++ b/src/boards/RIGv2/Radio/Radio.cpp @@ -0,0 +1,256 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "Radio.h" + +#include <RIGv2/Buses.h> +#include <common/Radio.h> +#include <radio/SX1278/SX1278Frontends.h> + +#include <atomic> + +using namespace Boardcore; +using namespace miosix; +using namespace Common; +using namespace RIGv2; + +Boardcore::SX1278Lora* gRadio{nullptr}; + +void handleDioIRQ() +{ + Boardcore::SX1278Lora* instance = gRadio; + if (instance) + { + instance->handleDioIRQ(); + } +} + +void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); } + +bool Radio::start() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Setup the frontend + std::unique_ptr<SX1278::ISX1278Frontend> frontend = + std::make_unique<EbyteFrontend>(radio::txEn::getPin(), + radio::rxEn::getPin()); + + // Setup transceiver + radio = std::make_unique<SX1278Lora>( + modules.get<Buses>()->getRadio(), radio::cs::getPin(), + radio::dio0::getPin(), radio::dio1::getPin(), radio::dio3::getPin(), + SPI::ClockDivider::DIV_64, std::move(frontend)); + + // Store the global radio instance + { + miosix::FastDisableInterrupt di; + gRadio = radio.get(); + } + + // Initialize radio + auto result = radio->init(RIG_RADIO_CONFIG); + if (result != SX1278Lora::Error::NONE) + { + LOG_ERR(logger, "Failed to initialize RIG radio"); + return false; + } + + // Initialize mavdriver + mavDriver = std::make_unique<MavDriver>( + radio.get(), + [this](MavDriver*, const mavlink_message_t& msg) + { handleMessage(msg); }, + Config::Radio::MAV_SLEEP_AFTER_SEND, + Config::Radio::MAV_OUT_BUFFER_MAX_AGE); + + if (!mavDriver->start()) + { + LOG_ERR(logger, "Failed to initialize RIG mav driver"); + return false; + } + + return true; +} + +void Radio::stop() +{ + // Remove global radio instance + { + miosix::FastDisableInterrupt di; + gRadio = nullptr; + } + mavDriver->stop(); +} + +void Radio::queuePacket(const mavlink_message_t& msg) +{ + queuedPackets.put(msg); +} + +void Radio::flushPackets() +{ + // Flush all packets of the queue + size_t count = queuedPackets.count(); + for (size_t i = 0; i < count; i++) + { + try + { + mavDriver->enqueueMsg(queuedPackets.pop()); + } + catch (...) + { + // This shouldn't happen, but still try to prevent it + } + } +} + +void Radio::sendAck(const mavlink_message_t& msg) +{ + mavlink_message_t ackMsg; + mavlink_msg_ack_tm_pack(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &ackMsg, msg.msgid, + msg.seq); + queuePacket(ackMsg); +} + +void Radio::sendNack(const mavlink_message_t& msg) +{ + mavlink_message_t nackMsg; + mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &nackMsg, + msg.msgid, msg.seq); + queuePacket(nackMsg); +} + +Boardcore::MavlinkStatus Radio::getMavStatus() +{ + return mavDriver->getStatus(); +} + +void Radio::handleMessage(const mavlink_message_t& msg) +{ + switch (msg.msgid) + { + case MAVLINK_MSG_ID_PING_TC: + sendAck(msg); + break; + + case MAVLINK_MSG_ID_COMMAND_TC: + handleCommand(msg); + break; + + case MAVLINK_MSG_ID_CONRIG_STATE_TC: + handleConrigState(msg); + break; + + case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC: + // TODO: Implement SystemTM + sendNack(msg); + break; + + case MAVLINK_MSG_ID_WIGGLE_SERVO_TC: + // TODO: Implement servo wiggle + sendNack(msg); + break; + + case MAVLINK_MSG_ID_SET_ATOMIC_VALVE_TIMING_TC: + // TODO: Implement set atomic valve timing + sendNack(msg); + break; + + case MAVLINK_MSG_ID_SET_VALVE_MAXIMUM_APERTURE_TC: + // TODO: Implement set valve maximum aperture + sendNack(msg); + break; + + case MAVLINK_MSG_ID_SET_IGNITION_TIME_TC: + // TODO: Implement set ignition time + sendNack(msg); + break; + + default: + // Unrecognized packet + sendNack(msg); + break; + } +} + +void Radio::handleCommand(const mavlink_message_t& msg) +{ + uint8_t cmd = mavlink_msg_command_tc_get_command_id(&msg); + switch (cmd) + { + case MAV_CMD_START_LOGGING: + if (!Logger::getInstance().start()) + { + sendNack(msg); + } + else + { + sendAck(msg); + } + break; + + case MAV_CMD_STOP_LOGGING: + Logger::getInstance().stop(); + sendAck(msg); + break; + + case MAV_CMD_CALIBRATE: + // TODO: Implement calibrate + sendNack(msg); + break; + + default: + // Unrecognized command + sendNack(msg); + break; + } +} + +void Radio::handleConrigState(const mavlink_message_t& msg) +{ + sendAck(msg); + + // TODO: Send actual data + sendFakeGseTm(); + + flushPackets(); + + // TODO: Handle buttons +} + +void Radio::sendFakeGseTm() +{ + mavlink_gse_tm_t tm = {0}; + mavlink_message_t msg; + + tm.timestamp = 69; + + mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, &tm); + + queuePacket(msg); +} diff --git a/src/boards/RIGv2/Radio/Radio.h b/src/boards/RIGv2/Radio/Radio.h new file mode 100644 index 0000000000000000000000000000000000000000..7880b6a4863148c8ce3c8671f14ff0fd000244a9 --- /dev/null +++ b/src/boards/RIGv2/Radio/Radio.h @@ -0,0 +1,75 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <RIGv2/Configs/RadioConfig.h> +#include <common/Mavlink.h> +#include <interfaces-impl/hwmapping.h> +#include <radio/MavlinkDriver/MavlinkDriver.h> +#include <radio/SX1278/SX1278Lora.h> +#include <utils/collections/CircularBuffer.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIGv2 +{ +using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Lora::MTU, + Config::Radio::MAV_OUT_QUEUE_SIZE, + Config::Radio::MAV_MAX_LENGTH>; + +class Radio : public Boardcore::Module +{ +public: + Radio() {} + + [[nodiscard]] bool start(); + + void stop(); + + Boardcore::MavlinkStatus getMavStatus(); + +private: + void sendAck(const mavlink_message_t& msg); + void sendNack(const mavlink_message_t& msg); + + void queuePacket(const mavlink_message_t &msg); + void flushPackets(); + + void handleMessage(const mavlink_message_t& msg); + void handleCommand(const mavlink_message_t& msg); + void handleConrigState(const mavlink_message_t& msg); + + void sendFakeGseTm(); + + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("radio"); + + Boardcore::CircularBuffer<mavlink_message_t, + Config::Radio::CIRCULAR_BUFFER_SIZE> + queuedPackets; + + std::unique_ptr<Boardcore::SX1278Lora> radio; + std::unique_ptr<MavDriver> mavDriver; +}; + +} // namespace RIGv2 \ No newline at end of file diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp index 07afb8e04f36fc8fc4097fea23dc29706bf8d87c..0415649cdfc09d309a328d7f48b99df274f41e9a 100644 --- a/src/boards/RIGv2/Sensors/Sensors.cpp +++ b/src/boards/RIGv2/Sensors/Sensors.cpp @@ -36,13 +36,11 @@ bool Sensors::start() SensorManager::SensorMap_t map; adc1Init(map); - manager.reset(new SensorManager(map, &scheduler)); + manager = std::make_unique<SensorManager>(map, &scheduler); return manager->start(); } -void Sensors::stop() { - manager->stop(); -} +void Sensors::stop() { manager->stop(); } ADS131M08Data Sensors::getADC1LastSample() { @@ -62,9 +60,9 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map) config.oversamplingRatio = ADS131M08Defs::OversamplingRatio::OSR_8192; config.globalChopModeEnabled = true; - adc1.reset(new ADS131M08(modules.get<Buses>()->getADS131M08_1(), - sensors::ADS131_1::cs::getPin(), spiConfig, - config)); + adc1 = std::make_unique<ADS131M08>(modules.get<Buses>()->getADS131M08_1(), + sensors::ADS131_1::cs::getPin(), + spiConfig, config); SensorInfo info("ADS131M08_1", Config::Sensors::ADC_SAMPLE_PERIOD, [this]() { adc1Callback(); }); @@ -81,6 +79,7 @@ void Sensors::adc1Callback() sample.voltage[4], sample.voltage[5], sample.voltage[6], sample.voltage[7]}; + // For Flavio, fuck Flavio LOG_INFO(logger, "{}\t{}", sample.voltage[6], sample.voltage[7]); sdLogger.log(data); diff --git a/src/boards/common/Radio.h b/src/boards/common/Radio.h index efdfa4b891240c09733cb2fd0fef96239c898d35..7040e918f5dd417ff9a3d4f5791d0edd34184d79 100644 --- a/src/boards/common/Radio.h +++ b/src/boards/common/Radio.h @@ -23,11 +23,12 @@ #pragma once #include <radio/SX1278/SX1278Fsk.h> +#include <radio/SX1278/SX1278Lora.h> namespace Common { -static const Boardcore::SX1278Fsk::Config MAIN_RADIO_CONFIG = { +static constexpr Boardcore::SX1278Fsk::Config MAIN_RADIO_CONFIG = { .freq_rf = 419000000, .freq_dev = 50000, .bitrate = 48000, @@ -39,7 +40,7 @@ static const Boardcore::SX1278Fsk::Config MAIN_RADIO_CONFIG = { .dc_free = Boardcore::SX1278Fsk::Config::DcFree::WHITENING, .enable_crc = false}; -static const Boardcore::SX1278Fsk::Config PAYLOAD_RADIO_CONFIG = { +static constexpr Boardcore::SX1278Fsk::Config PAYLOAD_RADIO_CONFIG = { .freq_rf = 868000000, .freq_dev = 50000, .bitrate = 48000, @@ -51,4 +52,14 @@ static const Boardcore::SX1278Fsk::Config PAYLOAD_RADIO_CONFIG = { .dc_free = Boardcore::SX1278Fsk::Config::DcFree::WHITENING, .enable_crc = false}; +static constexpr Boardcore::SX1278Lora::Config RIG_RADIO_CONFIG = { + .bandwidth = Boardcore::SX1278Lora::Config::Bw::HZ_125000, + .coding_rate = Boardcore::SX1278Lora::Config::Cr::CR_1, + .spreading_factor = Boardcore::SX1278Lora::Config::Sf::SF_7, + .low_data_rate_optimize = false, + .freq_rf = 434000000, + .ocp = 120, + .power = 2, + .enable_crc = false}; + } // namespace Common \ No newline at end of file diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp index 08a75fcfd01f2b6d626d1e583190aaefe25a4f5b..c8066ce51f830d4fbd51fd9a8da56ce6a66fea70 100644 --- a/src/entrypoints/RIGv2/rig-v2-entry.cpp +++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp @@ -20,8 +20,9 @@ * THE SOFTWARE. */ -#include <RIGv2/Sensors/Sensors.h> #include <RIGv2/Buses.h> +#include <RIGv2/Radio/Radio.h> +#include <RIGv2/Sensors/Sensors.h> #include <diagnostic/CpuMeter/CpuMeter.h> #include <diagnostic/StackLogger.h> @@ -29,52 +30,75 @@ using namespace Boardcore; using namespace RIGv2; using namespace miosix; -int main() { +int main() +{ ModuleManager &modules = ModuleManager::getInstance(); - PrintLogger logger = Logging::getLogger("main"); + PrintLogger logger = Logging::getLogger("main"); // TODO: Move this to a dedicated board scheduler TaskScheduler *scheduler = new TaskScheduler(3); - Buses *buses = new Buses(); + Buses *buses = new Buses(); Sensors *sensors = new Sensors(*scheduler); + Radio *radio = new Radio(); Logger &sdLogger = Logger::getInstance(); bool initResult = true; // Insert modules - if(!modules.insert<Buses>(buses)) { + if (!modules.insert<Buses>(buses)) + { initResult = false; LOG_ERR(logger, "Error failed to insert Buses"); } - if(!modules.insert<Sensors>(sensors)) { + if (!modules.insert<Sensors>(sensors)) + { + initResult = false; + LOG_ERR(logger, "Error failed to insert Sensors"); + } + + if (!modules.insert<Radio>(radio)) + { initResult = false; - LOG_ERR(logger, "Error failed to insert Sensors"); + LOG_ERR(logger, "Error failed to insert Radio"); } // Start modules - if(!sensors->start()) { + if (!sensors->start()) + { initResult = false; - LOG_ERR(logger, "Error failed to start sensors module"); + LOG_ERR(logger, "Error failed to start Sensors module"); } - if(!scheduler->start()) { + if (!radio->start()) + { + initResult = false; + LOG_ERR(logger, "Error failed to start Radio module"); + } + + if (!scheduler->start()) + { initResult = false; LOG_ERR(logger, "Error failed to start scheduler"); } - LOG_INFO(logger, "All good!"); + if (!initResult) + { + LOG_INFO(logger, "All good!"); + } // TEMPORARY CODE // Start logger module sdLogger.start(); // Periodic statistics - while(true) { + while (true) + { Thread::sleep(1000); + sdLogger.log(modules.get<Radio>()->getMavStatus()); sdLogger.log(CpuMeter::getCpuStats()); CpuMeter::resetCpuStats(); // TODO: What the fuck is this?