diff --git a/CMakeLists.txt b/CMakeLists.txt index 1902a11e287e5a8d2e1ebe2c2e0f5b8eae44a0c1..6485fa3c453d12f232e4636b24a64fd75fb09e2f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,7 +70,7 @@ sbs_target(motor-entry stm32f767zi_lyra_motor) add_executable(rig-v2-entry src/RIGv2/rig-v2-entry.cpp ${RIG_V2_COMPUTER}) target_include_directories(rig-v2-entry PRIVATE ${OBSW_INCLUDE_DIRS}) -sbs_target(rig-v2-entry stm32f767zi_rig_v2) +sbs_target(rig-v2-entry stm32f767zi_lyra_gs) add_executable(rig-v2-adc-test src/RIGv2/rig-v2-adc-test.cpp ${RIG_V2_COMPUTER}) target_include_directories(rig-v2-adc-test PRIVATE ${OBSW_INCLUDE_DIRS}) diff --git a/src/RIGv2/Actuators/Actuators.cpp b/src/RIGv2/Actuators/Actuators.cpp index 9593345c8e977e927dfc183062c97bb009a7a93e..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 100644 --- a/src/RIGv2/Actuators/Actuators.cpp +++ b/src/RIGv2/Actuators/Actuators.cpp @@ -1,484 +0,0 @@ -/* Copyright (c) 2025 Skyward Experimental Rocketry - * Authors: Davide Mor, 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 "Actuators.h" - -#include <RIGv2/Actuators/ActuatorsData.h> -#include <RIGv2/Configs/ActuatorsConfig.h> -#include <common/Events.h> -#include <drivers/timer/TimestampTimer.h> -#include <events/EventBroker.h> -#include <interfaces-impl/hwmapping.h> - -#include "ActuatorsMacros.h" - -using namespace Boardcore; -using namespace miosix; -using namespace Common; -using namespace RIGv2; - -void Actuators::ServoInfo::openServoWithTime(uint32_t time) -{ - long long currentTime = getTime(); - - closeTs = currentTime + (time * Constants::NS_IN_MS); - lastActionTs = currentTime; - - if (config.openingEvent != 0) - EventBroker::getInstance().post(config.openingEvent, TOPIC_MOTOR); -} - -void Actuators::ServoInfo::closeServo() -{ - closeTs = 0; - lastActionTs = getTime(); - - if (config.closingEvent != 0) - EventBroker::getInstance().post(config.closingEvent, TOPIC_MOTOR); -} - -void Actuators::ServoInfo::unsafeSetServoPosition(float position) -{ - // Check that the servo is actually there, just to be safe - if (!servo) - return; - - position *= config.limit; - if (config.flipped) - position = 1.0f - position; - - servo->setPosition(position); -} - -float Actuators::ServoInfo::getServoPosition() -{ - // Check that the servo is actually there, just to be safe - if (!servo) - return 0.0f; - - float position = servo->getPosition(); - if (config.flipped) - position = 1.0f - position; - - position /= config.limit; - return position; -} - -float Actuators::ServoInfo::getMaxAperture() -{ - return getModule<Registry>()->getOrSetDefaultUnsafe( - config.maxApertureRegKey, config.defaultMaxAperture); -} - -uint32_t Actuators::ServoInfo::getOpeningTime() -{ - return getModule<Registry>()->getOrSetDefaultUnsafe( - config.openingTimeRegKey, config.defaultOpeningTime); -} - -bool Actuators::ServoInfo::setMaxAperture(float aperture) -{ - if (aperture >= 0.0 && aperture <= 1.0) - { - getModule<Registry>()->setUnsafe(config.maxApertureRegKey, aperture); - return true; - } - else - { - // What? Who would ever set this to above 100%? - return false; - } -} - -bool Actuators::ServoInfo::setOpeningTime(uint32_t time) -{ - getModule<Registry>()->setUnsafe(config.openingTimeRegKey, time); - return true; -} - -Actuators::Actuators() - : infos{MAKE_SERVO(OX_FIL), MAKE_SERVO(OX_REL), MAKE_DETACH_SERVO(OX_DET), - MAKE_SERVO(N2_FIL), MAKE_SERVO(N2_REL), MAKE_DETACH_SERVO(N2_DET), - MAKE_SERVO(NITR), MAKE_SERVO(OX_VEN), MAKE_SERVO(N2_QUE), - MAKE_SERVO(MAIN)}, - n2_3wayValveInfo(MAKE_SIMPLE_SERVO(N2_3W)) -{ - for (auto& servo : infos) - servo.unsafeSetServoPosition(0.0f); - - n2_3wayValveInfo.unsafeSetServoPosition(0.0f); -} - -bool Actuators::isStarted() { return started; } - -bool Actuators::start() -{ - TaskScheduler& scheduler = - getModule<BoardScheduler>()->getActuatorsScheduler(); - - // Enable all servos - for (ServoInfo& info : infos) - info.servo->enable(); - - n2_3wayValveInfo.servo->enable(); - - uint8_t result = - scheduler.addTask([this]() { updatePositionsTask(); }, - Config::Servos::SERVO_TIMINGS_CHECK_PERIOD); - - if (result == 0) - { - LOG_ERR(logger, "Failed to add updatePositionsTask"); - return false; - } - - started = true; - return true; -} - -void Actuators::armLightOn() { relays::armLight::low(); } -void Actuators::armLightOff() { relays::armLight::high(); } - -void Actuators::igniterOn() { relays::ignition::low(); } -void Actuators::igniterOff() { relays::ignition::high(); } - -bool Actuators::wiggleServo(ServosList servo) -{ - // Special handling for the 3-way valve - if (servo == N2_3WAY_VALVE) - { - // Toggle the valve to the current opposite state - auto state = get3wayValveState(); - - set3wayValveState(!state); - Thread::sleep(1000); - set3wayValveState(state); - - return true; - } - - // Wiggle means open the servo for 1s - return openServoWithTime(servo, 1000); -} - -bool Actuators::toggleServo(ServosList servo) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return false; - - if (info->closeTs == 0) - { - uint32_t time = info->getOpeningTime(); - - // The servo is closed, open it - getModule<CanHandler>()->sendServoOpenCommand(servo, time); - info->openServoWithTime(time); - } - else - { - // The servo is open, close it - getModule<CanHandler>()->sendServoCloseCommand(servo); - info->closeServo(); - } - - return true; -} - -bool Actuators::openServo(ServosList servo) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return false; - - uint32_t time = info->getOpeningTime(); - getModule<CanHandler>()->sendServoOpenCommand(servo, time); - info->openServoWithTime(time); - - return true; -} - -bool Actuators::openServoWithTime(ServosList servo, uint32_t time) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return false; - - getModule<CanHandler>()->sendServoOpenCommand(servo, time); - info->openServoWithTime(time); - return true; -} - -bool Actuators::closeServo(ServosList servo) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return false; - - getModule<CanHandler>()->sendServoCloseCommand(servo); - info->closeServo(); - return true; -} - -void Actuators::closeAllServos() -{ - Lock<FastMutex> lock(infosMutex); - for (uint8_t idx = 0; idx < 10; idx++) - infos[idx].closeServo(); - - getModule<CanHandler>()->sendServoCloseCommand(ServosList::MAIN_VALVE); - getModule<CanHandler>()->sendServoCloseCommand( - ServosList::OX_VENTING_VALVE); -} - -bool Actuators::setMaxAperture(ServosList servo, float aperture) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return false; - - return info->setMaxAperture(aperture); -} - -bool Actuators::setOpeningTime(ServosList servo, uint32_t time) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return false; - - return info->setOpeningTime(time); -} - -bool Actuators::isServoOpen(ServosList servo) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return false; - - return info->closeTs != 0; -} - -bool Actuators::isCanServoOpen(ServosList servo) -{ - Lock<FastMutex> lock(infosMutex); - - if (servo == ServosList::MAIN_VALVE) - return canMainOpen; - else if (servo == ServosList::NITROGEN_VALVE) - return canNitrogenOpen; - else if (servo == ServosList::OX_VENTING_VALVE) - return canOxVentingOpen; - else if (servo == ServosList::N2_QUENCHING_VALVE) - return canN2QuenchingOpen; - else - return false; -} - -void Actuators::set3wayValveState(bool state) { n2_3wayValveState = state; } - -bool Actuators::get3wayValveState() { return n2_3wayValveState; } - -void Actuators::openChamberWithTime(uint32_t time) -{ - Lock<FastMutex> lock(infosMutex); - long long currentTime = getTime(); - - chamberCloseTs = currentTime + (time * Constants::NS_IN_MS); - chamberLastActionTs = currentTime; -} - -void Actuators::closeChamber() -{ - Lock<FastMutex> lock(infosMutex); - chamberCloseTs = 0; -} - -bool Actuators::isChamberOpen() -{ - Lock<FastMutex> lock(infosMutex); - return chamberCloseTs != 0; -} - -uint32_t Actuators::getServoOpeningTime(ServosList servo) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return 0; - - return info->getOpeningTime(); -} - -float Actuators::getServoMaxAperture(ServosList servo) -{ - Lock<FastMutex> lock(infosMutex); - ServoInfo* info = getServo(servo); - if (info == nullptr) - return 0; - - return info->getMaxAperture(); -} - -void Actuators::setCanServoOpen(ServosList servo, bool open) -{ - Lock<FastMutex> lock(infosMutex); - if (servo == ServosList::MAIN_VALVE) - canMainOpen = open; - else if (servo == ServosList::NITROGEN_VALVE) - canNitrogenOpen = open; - else if (servo == ServosList::OX_VENTING_VALVE) - canOxVentingOpen = open; - else if (servo == ServosList::N2_QUENCHING_VALVE) - canN2QuenchingOpen = open; -} - -void Actuators::inject(DependencyInjector& injector) -{ - Super::inject(injector); - for (ServoInfo& info : infos) - info.inject(injector); -} - -Actuators::ServoInfo* Actuators::getServo(ServosList servo) -{ - switch (servo) - { - case OX_FILLING_VALVE: // OX_FIL - return &infos[0]; - case OX_RELEASE_VALVE: // OX_REL - return &infos[1]; - case OX_DETACH_SERVO: // OX_DET - return &infos[2]; - case N2_FILLING_VALVE: // N2_FIL - return &infos[3]; - case N2_RELEASE_VALVE: // N2_REL - return &infos[4]; - case N2_DETACH_SERVO: // N2_DET - return &infos[5]; - case NITROGEN_VALVE: // NITR - return &infos[6]; - case OX_VENTING_VALVE: // OX_VEN - return &infos[7]; - case N2_QUENCHING_VALVE: // N2_QUE - return &infos[8]; - case MAIN_VALVE: // MAIN - return &infos[9]; - - default: - // Oh FUCK - LOG_ERR(logger, "Invalid servo requested"); - return nullptr; - } -} - -void Actuators::unsafeSetServoPosition(uint8_t idx, float position) -{ - infos[idx].unsafeSetServoPosition(position); - - // Log the update - ActuatorsData data; - data.timestamp = TimestampTimer::getTimestamp(); - data.servoIdx = idx; - data.position = position; - sdLogger.log(data); -} - -void Actuators::unsafeOpenChamber() { relays::nitrogen::low(); } - -void Actuators::unsafeCloseChamber() { relays::nitrogen::high(); } - -void Actuators::updatePositionsTask() -{ - Lock<FastMutex> lock(infosMutex); - - long long currentTime = getTime(); - - // Iterate over all servos - for (uint8_t idx = 0; idx < infos.size(); idx++) - { - if (currentTime < infos[idx].closeTs) - { - // The valve should be open - if (currentTime < infos[idx].lastActionTs + - (Config::Servos::SERVO_CONFIDENCE_TIME * - Constants::NS_IN_MS)) - { - // We should open the valve all the way - unsafeSetServoPosition(idx, infos[idx].getMaxAperture()); - } - else - { - // Time to wiggle the valve a little - unsafeSetServoPosition( - idx, infos[idx].getMaxAperture() * - (1.0 - Config::Servos::SERVO_CONFIDENCE)); - } - } - else - { - // Ok the valve should be closed - if (infos[idx].closeTs != 0) - { - // Perform the servo closing - infos[idx].closeServo(); - } - - if (currentTime < infos[idx].lastActionTs + - (Config::Servos::SERVO_CONFIDENCE_TIME * - Constants::NS_IN_MS)) - { - // We should close the valve all the way - unsafeSetServoPosition(idx, 0.0); - } - else - { - // Time to wiggle the valve a little - unsafeSetServoPosition(idx, - infos[idx].getMaxAperture() * - Config::Servos::SERVO_CONFIDENCE); - } - } - } - - // Handle nitrogen logic - if (currentTime < chamberCloseTs) - { - unsafeOpenChamber(); - } - else - { - chamberCloseTs = 0; - - unsafeCloseChamber(); - } - - // Handle the 3-way valve - auto position = n2_3wayValveState ? 1.0f : 0.0f; - n2_3wayValveInfo.unsafeSetServoPosition(position); -} diff --git a/src/RIGv2/Actuators/Actuators.h b/src/RIGv2/Actuators/Actuators.h index b6cb422d7fd882adbb001e0faf14e596dfd95c62..66f09207c6ca87bd2e2c6615150fd20db8ca9de7 100644 --- a/src/RIGv2/Actuators/Actuators.h +++ b/src/RIGv2/Actuators/Actuators.h @@ -81,72 +81,42 @@ private: }; public: - Actuators(); + [[nodiscard]] bool start() { return true; } - [[nodiscard]] bool start(); + bool isStarted() { return true; } - bool isStarted(); - - bool wiggleServo(ServosList servo); - bool toggleServo(ServosList servo); - bool openServo(ServosList servo); - bool openServoWithTime(ServosList servo, uint32_t time); - bool closeServo(ServosList servo); - void closeAllServos(); - bool setMaxAperture(ServosList servo, float aperture); - bool setOpeningTime(ServosList servo, uint32_t time); - bool isServoOpen(ServosList servo); - bool isCanServoOpen(ServosList servo); + bool wiggleServo(ServosList servo) { return true; } + bool toggleServo(ServosList servo) { return true; } + bool openServo(ServosList servo) { return true; } + bool openServoWithTime(ServosList servo, uint32_t time) { return true; } + bool closeServo(ServosList servo) { return true; } + void closeAllServos() {} + bool setMaxAperture(ServosList servo, float aperture) { return true; } + bool setOpeningTime(ServosList servo, uint32_t time) { return true; } + bool isServoOpen(ServosList servo) { return false; } + bool isCanServoOpen(ServosList servo) { return false; } // N2 3-way valve control - void set3wayValveState(bool state); - bool get3wayValveState(); + void set3wayValveState(bool state) {} + bool get3wayValveState() { return false; } // Chamber valve control - void openChamberWithTime(uint32_t time); - void closeChamber(); - bool isChamberOpen(); - - uint32_t getServoOpeningTime(ServosList servo); - float getServoMaxAperture(ServosList servo); - - void armLightOn(); - void armLightOff(); - - void igniterOn(); - void igniterOff(); - - void setCanServoOpen(ServosList servo, bool open); - - void inject(Boardcore::DependencyInjector& injector) override; - -private: - ServoInfo* getServo(ServosList servo); - - void unsafeSetServoPosition(uint8_t idx, float position); - void unsafeOpenChamber(); - void unsafeCloseChamber(); - - void updatePositionsTask(); + void openChamberWithTime(uint32_t time) {} + void closeChamber() {} + bool isChamberOpen() { return false; } - std::atomic<bool> started{false}; + uint32_t getServoOpeningTime(ServosList servo) { return 0; } + float getServoMaxAperture(ServosList servo) { return 0.f; } - miosix::FastMutex infosMutex; - std::array<ServoInfo, 10> infos; - ServoInfo n2_3wayValveInfo; - std::atomic<bool> n2_3wayValveState{false}; + void armLightOn() {} + void armLightOff() {} - long long chamberCloseTs = - 0; ///< Timestamp to close the chamber (0 if closed) - long long chamberLastActionTs = 0; ///< Timestamp of last chamber action + void igniterOn() {} + void igniterOff() {} - bool canMainOpen = false; - bool canNitrogenOpen = false; - bool canOxVentingOpen = false; - bool canN2QuenchingOpen = false; + void setCanServoOpen(ServosList servo, bool open) {} - Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); - Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("actuators"); + void inject(Boardcore::DependencyInjector& injector) override {} }; } // namespace RIGv2 diff --git a/src/RIGv2/Buses.h b/src/RIGv2/Buses.h index a777b13e6aac9420ad4ca291b9fe294a4601a1bf..45e5ce93eb1a7e7db0ee4cb1235b7f9c61c734c5 100644 --- a/src/RIGv2/Buses.h +++ b/src/RIGv2/Buses.h @@ -41,7 +41,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; } + Boardcore::SPIBus& getRadio() { return spi1; } private: Boardcore::SPIBus spi1; diff --git a/src/RIGv2/Radio/Radio.cpp b/src/RIGv2/Radio/Radio.cpp index df959b0327045a3e3c6c1dbfff6e5b2d63d2fc7f..a56f2d2000b9b859d1741f959f8c18066b3f85fa 100644 --- a/src/RIGv2/Radio/Radio.cpp +++ b/src/RIGv2/Radio/Radio.cpp @@ -53,9 +53,9 @@ void setIRQRadio(SX1278Lora* radio) gRadio = radio; } -void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); } -void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); } -void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO1_DIO0_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO1_DIO1_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO1_DIO3_IRQ() { handleDioIRQ(); } bool Radio::isStarted() { return started; } @@ -63,8 +63,8 @@ bool Radio::start() { // Setup the frontend std::unique_ptr<SX1278::ISX1278Frontend> frontend = - std::make_unique<EbyteFrontend>(radio::txEn::getPin(), - radio::rxEn::getPin()); + std::make_unique<EbyteFrontend>(radio::txen::getPin(), + radio::rxen::getPin()); // Setup transceiver radio = std::make_unique<SX1278Lora>( diff --git a/src/RIGv2/Sensors/Sensors.cpp b/src/RIGv2/Sensors/Sensors.cpp index 89e22b6491d570854891dfbacd5a8ee3d65707e0..4e3155fd9b6bc9bc3b5c58d25a5deb235cfb4839 100644 --- a/src/RIGv2/Sensors/Sensors.cpp +++ b/src/RIGv2/Sensors/Sensors.cpp @@ -37,9 +37,12 @@ bool Sensors::isStarted() { return started; } bool Sensors::start() { + started = true; + return true; + if (Config::Sensors::InternalADC::ENABLED) - internalAdcInit(); - + internalAdcInit(); + if (Config::Sensors::ADC_1::ENABLED) { adc1Init(); @@ -52,23 +55,23 @@ bool Sensors::start() rocketWeightInit(); oxTankWeightInit(); } - + if (Config::Sensors::ADC_2::ENABLED) { adc2Init(); oxTankBottomPressureInit(); n2TankPressureInit(); } - + if (Config::Sensors::MAX31856::ENABLED) - tc1Init(); - + tc1Init(); + if (!sensorManagerInit()) { LOG_ERR(logger, "Failed to init SensorManager"); return false; } - + started = true; return true; } @@ -428,9 +431,6 @@ void Sensors::adc1Init() .offset = 0, .gain = 1.0}; - adc1 = std::make_unique<ADS131M08>(getModule<Buses>()->getADS131M08_1(), - sensors::ADS131_1::cs::getPin(), - spiConfig, config); } void Sensors::adc1Callback() { sdLogger.log(ADC1Data{getADC1LastSample()}); } @@ -464,9 +464,6 @@ void Sensors::adc2Init() .offset = 0, .gain = 1.0}; - adc2 = std::make_unique<ADS131M08>(getModule<Buses>()->getADS131M08_2(), - sensors::ADS131_2::cs::getPin(), - spiConfig, config); } void Sensors::adc2Callback() { sdLogger.log(ADC2Data{getADC2LastSample()}); } @@ -476,9 +473,6 @@ void Sensors::tc1Init() SPIBusConfig spiConfig = MAX31856::getDefaultSPIConfig(); spiConfig.clockDivider = SPI::ClockDivider::DIV_32; - tc1 = std::make_unique<MAX31856>( - getModule<Buses>()->getMAX31856_1(), sensors::MAX31856_1::cs::getPin(), - spiConfig, MAX31856::ThermocoupleType::K_TYPE); } void Sensors::tc1Callback() { sdLogger.log(TC1Data{getTc1LastSample()}); } diff --git a/src/RIGv2/rig-v2-entry.cpp b/src/RIGv2/rig-v2-entry.cpp index 6548c191f8c6863add690115be2c3471a51d389e..324e0d6a5d5cc92f188a4c5659e48286a622dac1 100644 --- a/src/RIGv2/rig-v2-entry.cpp +++ b/src/RIGv2/rig-v2-entry.cpp @@ -147,16 +147,7 @@ int main() led2On(); } - std::cout << "Starting CanHandler" << std::endl; - if (!canHandler->start()) - { - initResult = false; - std::cout << "*** Failed to start CanHandler ***" << std::endl; - } - else - { - led3On(); - } + std::cout << "Skipping CanHandler" << std::endl; std::cout << "Starting GroundModeManager" << std::endl; if (!gmm->start()) @@ -202,7 +193,7 @@ int main() } else { - broker.post(FMM_INIT_ERROR, TOPIC_MOTOR); + broker.post(FMM_INIT_OK, TOPIC_MOTOR); std::cout << "*** Init failure ***" << std::endl; }