diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp index 01b66facabd8da5306204ad9a49d6621cb92d0bf..2483af3be6c0307d6e86d036b94c807af82968ea 100644 --- a/src/boards/RIGv2/Actuators/Actuators.cpp +++ b/src/boards/RIGv2/Actuators/Actuators.cpp @@ -25,6 +25,7 @@ #include <RIGv2/Actuators/ActuatorsData.h> #include <RIGv2/Configs/ActuatorsConfig.h> #include <common/Events.h> +#include <events/EventBroker.h> // TODO(davide.mor): Remove TimestampTimer #include <drivers/timer/TimestampTimer.h> @@ -40,14 +41,20 @@ void Actuators::ServoInfo::openServo(uint64_t time) openedTs = currentTime; closeTs = currentTime + (time * Constants::NS_IN_MS); - // TODO(davide.mor): Dispatch the open event + if (openingEvent != 0) + { + EventBroker::getInstance().post(openingEvent, TOPIC_MOTOR); + } } void Actuators::ServoInfo::closeServo() { closeTs = 0; - // TODO(davide.mor): Dispatch the close event + if (closingEvent != 0) + { + EventBroker::getInstance().post(closingEvent, TOPIC_MOTOR); + } } void Actuators::ServoInfo::unsafeSetServoPosition(float position) @@ -193,7 +200,7 @@ bool Actuators::start() bool Actuators::wiggleServo(ServosList servo) { // Wiggle means open the servo for 1s - return openServoAtomic(servo, 1000); + return openServoWithTime(servo, 1000); } bool Actuators::toggleServo(ServosList servo) @@ -219,7 +226,7 @@ bool Actuators::toggleServo(ServosList servo) return true; } -bool Actuators::closeServo(ServosList servo) +bool Actuators::openServo(ServosList servo) { Lock<FastMutex> lock(infosMutex); ServoInfo *info = getServo(servo); @@ -228,11 +235,11 @@ bool Actuators::closeServo(ServosList servo) return false; } - info->closeServo(); + info->openServo(info->openingTime); return true; } -bool Actuators::openServoAtomic(ServosList servo, uint64_t time) +bool Actuators::openServoWithTime(ServosList servo, uint64_t time) { Lock<FastMutex> lock(infosMutex); ServoInfo *info = getServo(servo); @@ -245,6 +252,28 @@ bool Actuators::openServoAtomic(ServosList servo, uint64_t time) return true; } +bool Actuators::closeServo(ServosList servo) +{ + Lock<FastMutex> lock(infosMutex); + ServoInfo *info = getServo(servo); + if (info == nullptr) + { + return false; + } + + info->closeServo(); + return true; +} + +void Actuators::closeAllServos() +{ + Lock<FastMutex> lock(infosMutex); + for (uint8_t idx = 0; idx < 10; idx++) + { + infos[idx].closeServo(); + } +} + bool Actuators::setMaxAperture(ServosList servo, float aperture) { Lock<FastMutex> lock(infosMutex); @@ -351,8 +380,11 @@ void Actuators::updatePositionsTask() { // The valve JUST closed, notify everybody infos[idx].closeTs = 0; - - // TODO(davide.mor): Dispatch the close event + if (infos[idx].closingEvent) + { + EventBroker::getInstance().post(infos[idx].closingEvent, + TOPIC_MOTOR); + } } if (currentTime < diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h index b588f2be54c7b7e428fd0891c275cf3832fb8bea..75aa4920e32a70c62dbd18b8d2a2de320baa588b 100644 --- a/src/boards/RIGv2/Actuators/Actuators.h +++ b/src/boards/RIGv2/Actuators/Actuators.h @@ -69,8 +69,10 @@ public: bool wiggleServo(ServosList servo); bool toggleServo(ServosList servo); + bool openServo(ServosList servo); + bool openServoWithTime(ServosList servo, uint64_t time); bool closeServo(ServosList servo); - bool openServoAtomic(ServosList servo, uint64_t time); + void closeAllServos(); bool setMaxAperture(ServosList servo, float aperture); bool setOpeningTime(ServosList servo, uint64_t time); bool isServoOpen(ServosList servo); diff --git a/src/boards/RIGv2/Buses.h b/src/boards/RIGv2/Buses.h index 78ec1fa779d049da76f39da2296d7e0744a92316..59ecb527269d597802409a858171867a91256e81 100644 --- a/src/boards/RIGv2/Buses.h +++ b/src/boards/RIGv2/Buses.h @@ -33,12 +33,13 @@ class Buses : public Boardcore::Module { private: Boardcore::SPIBus spi1; + Boardcore::SPIBus spi2; Boardcore::SPIBus spi3; Boardcore::SPIBus spi4; Boardcore::SPIBus spi6; public: - Buses() : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6) {} + Buses() : spi1(SPI1), spi2(SPI2), spi3(SPI3), spi4(SPI4), spi6(SPI6) {} Boardcore::SPIBus &getH3LIS331DL() { return spi4; } Boardcore::SPIBus &getLPS22DF() { return spi4; } diff --git a/src/boards/RIGv2/Configs/IgnitionConfig.h b/src/boards/RIGv2/Configs/IgnitionConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..cf32e420d7968e1ed3fa047236e54101a05cef59 --- /dev/null +++ b/src/boards/RIGv2/Configs/IgnitionConfig.h @@ -0,0 +1,36 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 <cstdint> + +namespace RIGv2 +{ +namespace Config +{ +namespace Ignition +{ +static constexpr uint32_t DEFAULT_IGNITION_WAITING_TIME = 5451; // [ms] +} +} // 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 index b6d5863a71a76ef9e6fb3e123a144012a97ddc59..6b6846dd1453951fd59ce7fd33182d1fc3881021 100644 --- a/src/boards/RIGv2/Radio/Radio.cpp +++ b/src/boards/RIGv2/Radio/Radio.cpp @@ -200,9 +200,12 @@ void Radio::handleMessage(const mavlink_message_t& msg) if (modules.get<GroundModeManager>()->isDisarmed()) { - if(modules.get<Actuators>()->wiggleServo(servo)) { + if (modules.get<Actuators>()->wiggleServo(servo)) + { sendAck(msg); - } else { + } + else + { sendNack(msg); } } @@ -252,8 +255,10 @@ void Radio::handleMessage(const mavlink_message_t& msg) case MAVLINK_MSG_ID_SET_IGNITION_TIME_TC: { - // TODO(davide.mor): Implement set ignition time - sendNack(msg); + uint32_t timing = mavlink_msg_set_ignition_time_tc_get_timing(&msg); + modules.get<GroundModeManager>()->setIgnitionTime(timing); + + sendAck(msg); break; } @@ -412,6 +417,9 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg) tm.ignition_state = modules.get<GroundModeManager>()->isIgniting(); // TODO(davide.mor): Add the rest of these + tm.battery_voltage = sensors->getADC1LastSample().voltage[0]; + tm.current_consumption = sensors->getADC1LastSample().voltage[1]; + mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID, Config::Radio::MAV_COMPONENT_ID, &msg, &tm); @@ -431,6 +439,9 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg) tm.floating_level = 69.0f; // Lol // TODO(davide.mor): Add the rest of these + tm.battery_voltage = sensors->getADC1LastSample().voltage[2]; + tm.current_consumption = sensors->getADC1LastSample().voltage[3]; + mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID, Config::Radio::MAV_COMPONENT_ID, &msg, &tm); @@ -483,7 +494,9 @@ void Radio::handleConrigState(const mavlink_message_t& msg) if (oldConrigState.ignition_btn == 0 && state.ignition_btn == 1) { // The ignition switch was pressed - // TODO(davide.mor): Perform ignition + // TODO(davide.mor): Notify everybody of a manual actuation + + EventBroker::getInstance().post(MOTOR_IGNITION, TOPIC_MOTOR); lastManualActuation = currentTime; } diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp index d419254438e8a5f1d7c4e26c1119be52f8a86c50..a48feec0a3e41e328e1732dc5ee6b1b646a3f1d6 100644 --- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp +++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp @@ -22,6 +22,7 @@ #include "GroundModeManager.h" +#include <RIGv2/Actuators/Actuators.h> #include <RIGv2/Sensors/Sensors.h> #include <common/Events.h> #include <events/EventBroker.h> @@ -34,13 +35,13 @@ using namespace miosix; using namespace Common; using namespace RIGv2; -void armLightOn() { - relays::armLight::low(); -} +void armLightOn() { relays::armLight::low(); } -void armLightOff() { - relays::armLight::high(); -} +void armLightOff() { relays::armLight::high(); } + +void igniterOn() { relays::ignition::low(); } + +void igniterOff() { relays::ignition::high(); } GroundModeManager::GroundModeManager() : FSM(&GroundModeManager::state_idle) { @@ -60,6 +61,8 @@ bool GroundModeManager::isIgniting() return testState(&GroundModeManager::state_igniting); } +void GroundModeManager::setIgnitionTime(uint32_t time) { ignitionTime = time; } + void GroundModeManager::state_idle(const Boardcore::Event &event) { switch (event) @@ -132,11 +135,13 @@ void GroundModeManager::state_disarmed(const Boardcore::Event &event) void GroundModeManager::state_armed(const Boardcore::Event &event) { + ModuleManager &modules = ModuleManager::getInstance(); switch (event) { case EV_ENTRY: { armLightOn(); + modules.get<Actuators>()->closeAllServos(); logStatus(GroundModeManagerState::STATE_ARMED); break; } @@ -146,12 +151,57 @@ void GroundModeManager::state_armed(const Boardcore::Event &event) transition(&GroundModeManager::state_disarmed); break; } + + case MOTOR_IGNITION: + { + transition(&GroundModeManager::state_igniting); + break; + } } } void GroundModeManager::state_igniting(const Boardcore::Event &event) { - // TODO(davide.mor): Not yet implemented + ModuleManager &modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + // Start ignition + igniterOn(); + + // Send event to open the oxidant + openOxidantDelayEventId = EventBroker::getInstance().postDelayed( + MOTOR_OPEN_OXIDANT, TOPIC_MOTOR, ignitionTime); + + logStatus(GroundModeManagerState::STATE_IGNITING); + break; + } + + case MOTOR_OPEN_OXIDANT: + { + // Stop ignition + igniterOff(); + modules.get<Actuators>()->openServo(ServosList::MAIN_VALVE); + break; + } + + case TMTC_DISARM: // Abort signal + case MOTOR_CLOSE_FEED_VALVE: + { + // Stop ignition + igniterOff(); + + // Close all of the valves + modules.get<Actuators>()->closeAllServos(); + + // Disable MOTOR_OPEN_OXIDANT event + EventBroker::getInstance().removeDelayed(openOxidantDelayEventId); + + transition(&GroundModeManager::state_disarmed); + break; + } + } } void GroundModeManager::logStatus(GroundModeManagerState newState) diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h index 68ecb4855898109747d36399e1d67246b6336b47..79699bcac1c0fc92fa3a16c8214a4063348e4bee 100644 --- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h +++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h @@ -22,12 +22,13 @@ #pragma once +#include <RIGv2/Configs/IgnitionConfig.h> #include <RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h> #include <diagnostic/PrintLogger.h> #include <events/FSM.h> #include <logger/Logger.h> -#include <atomic> +#include <atomic> #include <utils/ModuleManager/ModuleManager.hpp> namespace RIGv2 @@ -43,6 +44,8 @@ public: bool isDisarmed(); bool isIgniting(); + void setIgnitionTime(uint32_t time); + private: void state_idle(const Boardcore::Event &event); void state_init_err(const Boardcore::Event &event); @@ -54,6 +57,10 @@ private: Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors"); + + uint16_t openOxidantDelayEventId = -1; + std::atomic<uint32_t> ignitionTime{ + Config::Ignition::DEFAULT_IGNITION_WAITING_TIME}; }; } // namespace RIGv2 \ 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 d8f94a7d798d192441c1f840ea328f91a3fedf3a..b390de8f69afec81369dedbce783969b8b47783e 100644 --- a/src/entrypoints/RIGv2/rig-v2-entry.cpp +++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp @@ -113,12 +113,6 @@ int main() LOG_ERR(logger, "Failed to start EventBroker"); } - if (!sensors->start()) - { - initResult = false; - LOG_ERR(logger, "Error failed to start Sensors module"); - } - if (!actuators->start()) { initResult = false; @@ -131,6 +125,12 @@ int main() LOG_ERR(logger, "Error failed to start Radio module"); } + if (!sensors->start()) + { + initResult = false; + LOG_ERR(logger, "Error failed to start Sensors module"); + } + if (!gmm->start()) { initResult = false;