diff --git a/CMakeLists.txt b/CMakeLists.txt index 14369b6886f36b9b733b8f5cce41cce13e871c30..e50a959a0d54b57a185d1331bb1f1b49add26e68 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,6 +56,11 @@ add_executable(motor-entry src/entrypoints/Motor/motor-entry.cpp ${MOTOR_SOURCES target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS}) sbs_target(motor-entry stm32f767zi_gemini_motor) +add_executable(rig-entry src/entrypoints/RIG/rig-entry.cpp ${RIG_COMPUTER}) +target_include_directories(rig-entry PRIVATE ${OBSW_INCLUDE_DIRS}) +sbs_target(rig-entry stm32f429zi_skyward_rig) + #-----------------------------------------------------------------------------# # Test entrypoints # -#-----------------------------------------------------------------------------# \ No newline at end of file +#-----------------------------------------------------------------------------# + diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 098b277a6b2fc13b067e467dbf74fcd6c571a31d..65ae83089d526ae7967dc9be87e3a8175b094772 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -54,4 +54,16 @@ set(MOTOR_SOURCES src/boards/Motor/Sensors/Sensors.cpp src/boards/Motor/BoardScheduler.cpp src/boards/Motor/CanHandler/CanHandler.cpp +) + +set(RIG_COMPUTER + src/boards/RIG/BoardScheduler.cpp + src/boards/RIG/Sensors/Sensors.cpp + src/boards/RIG/Actuators/Actuators.cpp + src/boards/RIG/Radio/Radio.cpp + src/boards/RIG/TMRepository/TMRepository.cpp + src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp + src/boards/RIG/StateMachines/TARS1/TARS1.cpp + src/boards/RIG/CanHandler/CanHandler.cpp + src/boards/RIG/StatesMonitor/StatesMonitor.cpp ) diff --git a/src/boards/RIG/Actuators/Actuators.cpp b/src/boards/RIG/Actuators/Actuators.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0273849a869ffe503fa76eda11a5d61b6ac51adf --- /dev/null +++ b/src/boards/RIG/Actuators/Actuators.cpp @@ -0,0 +1,413 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Actuators/Actuators.h> +#include <RIG/Actuators/ActuatorsData.h> +#include <RIG/CanHandler/CanHandler.h> +#include <RIG/Configs/ActuatorsConfig.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> +#include <miosix.h> + +using namespace miosix; +using namespace Boardcore; + +namespace RIG +{ +Actuators::Actuators(TaskScheduler* sched) : scheduler(sched) +{ + servo1 = + new Servo(Config::Servos::SERVO1_TIMER, Config::Servos::SERVO1_PWM_CH, + Config::Servos::MAX_PULSE, Config::Servos::MIN_PULSE); + servo2 = + new Servo(Config::Servos::SERVO2_TIMER, Config::Servos::SERVO2_PWM_CH, + Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE); + servo3 = + new Servo(Config::Servos::SERVO3_TIMER, Config::Servos::SERVO3_PWM_CH, + Config::Servos::MAX_PULSE, Config::Servos::MIN_PULSE); + servo4 = + new Servo(Config::Servos::SERVO4_TIMER, Config::Servos::SERVO4_PWM_CH, + Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE); + servo5 = + new Servo(Config::Servos::SERVO5_TIMER, Config::Servos::SERVO5_PWM_CH, + Config::Servos::MAX_PULSE, Config::Servos::MIN_PULSE); + + // Set the default openings + openings[ServosList::FILLING_VALVE] = + Config::Servos::DEFAULT_FILLING_MAXIMUM_APERTURE; + openings[ServosList::MAIN_VALVE] = + Config::Servos::DEFAULT_MAIN_MAXIMUM_APERTURE; + openings[ServosList::RELEASE_VALVE] = + Config::Servos::DEFAULT_RELEASE_MAXIMUM_APERTURE; + openings[ServosList::VENTING_VALVE] = + Config::Servos::DEFAULT_VENTING_MAXIMUM_APERTURE; + openings[ServosList::DISCONNECT_SERVO] = + Config::Servos::DEFAULT_DISCONNECT_MAXIMUM_APERTURE; + + // Set the default opening times + openingTimes[ServosList::FILLING_VALVE] = + Config::Servos::DEFAULT_FILLING_OPENING_TIME; + openingTimes[ServosList::MAIN_VALVE] = + Config::Servos::DEFAULT_MAIN_OPENING_TIME; + openingTimes[ServosList::RELEASE_VALVE] = + Config::Servos::DEFAULT_RELEASE_OPENING_TIME; + openingTimes[ServosList::VENTING_VALVE] = + Config::Servos::DEFAULT_VENTING_OPENING_TIME; + openingTimes[ServosList::DISCONNECT_SERVO] = + Config::Servos::DEFAULT_DISCONNECT_OPENING_TIME; + + // Set the opening / closing events + openingEvents[ServosList::FILLING_VALVE] = + Common::Events::MOTOR_OPEN_FILLING_VALVE; + openingEvents[ServosList::MAIN_VALVE] = + Common::Events::MOTOR_OPEN_FEED_VALVE; + openingEvents[ServosList::RELEASE_VALVE] = + Common::Events::MOTOR_OPEN_RELEASE_VALVE; + openingEvents[ServosList::VENTING_VALVE] = + Common::Events::MOTOR_OPEN_VENTING_VALVE; + openingEvents[ServosList::DISCONNECT_SERVO] = + Common::Events::MOTOR_DISCONNECT; + + closingEvents[ServosList::FILLING_VALVE] = + Common::Events::MOTOR_CLOSE_FILLING_VALVE; + closingEvents[ServosList::MAIN_VALVE] = + Common::Events::MOTOR_CLOSE_FEED_VALVE; + closingEvents[ServosList::RELEASE_VALVE] = + Common::Events::MOTOR_CLOSE_RELEASE_VALVE; + closingEvents[ServosList::VENTING_VALVE] = + Common::Events::MOTOR_CLOSE_VENTING_VALVE; +} + +bool Actuators::start() +{ + servo1->enable(); + servo2->enable(); + servo3->enable(); + servo4->enable(); + servo5->enable(); + + uint8_t result = scheduler->addTask( + [=]() { checkTimings(); }, Config::Servos::SERVO_TIMINGS_CHECK_PERIOD); + return result != 0; +} + +bool Actuators::wiggleServo(ServosList servo) +{ + PauseKernelLock lock; + // Get the choosen servo and nullptr in case not present + Servo* requestedServo = getServo(servo); + if (requestedServo != nullptr) + { + // Close all the servo + closeAllServo(); + + // Store the previous aperture time + uint64_t time = openingTimes[servo]; + + // Set a new one + openingTimes[servo] = 1000; // 1s + + // Toggle the servo + toggleServo(servo); + + // Set the previous opening time + openingTimes[servo] = time; + + return true; + } + + return false; +} + +Servo* Actuators::getServo(ServosList servo) +{ + switch (servo) + { + case MAIN_VALVE: + return servo4; + case VENTING_VALVE: + return servo5; + case RELEASE_VALVE: + return servo2; + case FILLING_VALVE: + return servo1; + case DISCONNECT_SERVO: + return servo3; + default: + return nullptr; + } +} + +float Actuators::getServoPosition(ServosList servo) +{ + // Get the choosen servo and nullptr in case not present + Servo* requestedServo = getServo(servo); + + if (requestedServo == nullptr) + { + return -1; + } + + return requestedServo->getPosition(); +} + +void Actuators::setServoPosition(ServosList servo, float position) +{ + // To lock the resources use a kernel lock + PauseKernelLock lock; + + // Get the choosen servo and nullptr in case not present + Servo* requestedServo = getServo(servo); + + if (!(requestedServo == nullptr)) + { + requestedServo->setPosition(position); + + // Log the position + ActuatorsData data; + data.timestamp = TimestampTimer::getTimestamp(); + data.servoId = servo; + data.position = position; + Logger::getInstance().log(data); + } +} + +void Actuators::checkTimings() +{ + uint64_t currentTick = getTick(); + + // Enter in protected zone where the timings should be checked and changed + // and the servo should be positioned atomically over all the threads. A + // kernel lock is adopted only due to it's performance but a mutex could be + // easily adopted. + { + PauseKernelLock lock; + + for (uint8_t i = 0; i < ServosList::ServosList_ENUM_END; i++) + { + if (timings[i] > currentTick) + { + if (currentTick > + setFlag[i] + Config::Servos::SERVO_CONFIDENCE_TIME) + { + setServoPosition( + static_cast<ServosList>(i), + openings[i] - + openings[i] * + Config::Servos:: + SERVO_CONFIDENCE); // 2% less than the + // actual aperture + } + else + { + // Open the corresponding valve wrt the maximum opening + setServoPosition(static_cast<ServosList>(i), openings[i]); + } + } + else + { + if (timings[i] != 0) + { + timings[i] = 0; + setFlag[i] = currentTick; + + { + RestartKernelLock l(lock); + + // Publish the closing event + EventBroker::getInstance().post( + closingEvents[i], Common::Topics::TOPIC_MOTOR); + } + } + if (currentTick > + setFlag[i] + Config::Servos::SERVO_CONFIDENCE_TIME) + { + setServoPosition( + static_cast<ServosList>(i), + openings[i] * + Config::Servos::SERVO_CONFIDENCE); // 2% open + } + else + { + setServoPosition(static_cast<ServosList>(i), 0); + } + } + } + } +} + +void Actuators::toggleServo(ServosList servo) +{ + PauseKernelLock lock; + + if (getServo(servo) != nullptr) + { + // If the valve is already open + if (timings[servo] > 0) + { + timings[servo] = 0; + setFlag[servo] = getTick(); + + { + RestartKernelLock l(lock); + + // Publish the closing event + EventBroker::getInstance().post(closingEvents[servo], + Common::Topics::TOPIC_MOTOR); + + // Publish the command also into the CAN bus + ModuleManager::getInstance() + .get<CanHandler>() + ->sendCanServoCommand(servo, 0, 0); + } + } + else + { + timings[servo] = getTick() + openingTimes[servo]; + setFlag[servo] = getTick(); + + { + RestartKernelLock l(lock); + + // Publish the opening event + EventBroker::getInstance().post(openingEvents[servo], + Common::Topics::TOPIC_MOTOR); + + // Publish the command also into the CAN bus + ModuleManager::getInstance() + .get<CanHandler>() + ->sendCanServoCommand(servo, 1, openingTimes[servo]); + } + } + } +} + +void Actuators::openServoAtomic(ServosList servo, uint32_t time) +{ + PauseKernelLock lock; + + if (getServo(servo) != nullptr) + { + // If the valve is already open + if (timings[servo] > 0) + { + timings[servo] = 0; + setFlag[servo] = getTick(); + + { + RestartKernelLock l(lock); + + // Publish the closing event + EventBroker::getInstance().post(closingEvents[servo], + Common::Topics::TOPIC_MOTOR); + + // Publish the command also into the CAN bus + ModuleManager::getInstance() + .get<CanHandler>() + ->sendCanServoCommand(servo, 0, 0); + } + } + else + { + timings[servo] = getTick() + time; + setFlag[servo] = getTick(); + + { + RestartKernelLock l(lock); + + // Publish the opening event + EventBroker::getInstance().post(openingEvents[servo], + Common::Topics::TOPIC_MOTOR); + + // Publish the command also into the CAN bus + ModuleManager::getInstance() + .get<CanHandler>() + ->sendCanServoCommand(servo, 1, time); + } + } + } +} + +void Actuators::closeAllServo() +{ + PauseKernelLock lock; + + for (uint8_t i = 0; i < ServosList::ServosList_ENUM_END; i++) + { + // Once the disconnect servo is open it shall never close + if (timings[i] != 0 && i != ServosList::DISCONNECT_SERVO) + { + // Make the timings expire + timings[i] = 0; + setFlag[i] = getTick(); + + // Publish the command also into the CAN bus + ModuleManager::getInstance().get<CanHandler>()->sendCanServoCommand( + static_cast<ServosList>(i), 0, 0); + } + } +} + +void Actuators::setMaximumAperture(ServosList servo, float aperture) +{ + PauseKernelLock lock; + + // Check if the servo exists + if (getServo(servo) != nullptr) + { + // Check aperture + if (aperture < 0) + { + aperture = 0; + } + else if (aperture > 1) + { + aperture = 1; + } + + openings[servo] = aperture; + } +} + +void Actuators::setTiming(ServosList servo, uint32_t time) +{ + PauseKernelLock lock; + + // Check if the servo exists + if (getServo(servo) != nullptr) + { + openingTimes[servo] = time; + } +} + +uint32_t Actuators::getTiming(ServosList servo) +{ + PauseKernelLock lock; + + if (getServo(servo) != nullptr) + { + return openingTimes[servo]; + } + return 0; +} + +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Actuators/Actuators.h b/src/boards/RIG/Actuators/Actuators.h new file mode 100644 index 0000000000000000000000000000000000000000..b6e2118eedaf08af26518087343f709920934948 --- /dev/null +++ b/src/boards/RIG/Actuators/Actuators.h @@ -0,0 +1,158 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <actuators/Servo/Servo.h> +#include <common/Events.h> +#include <common/Mavlink.h> +#include <events/EventBroker.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +class Actuators : public Boardcore::Module +{ +public: + /** + * @brief Construct a new Actuators object + * + * @param sched The scheduler to respect the atomic timings in case of TARS0 + * engaged + */ + Actuators(Boardcore::TaskScheduler* sched); + + /** + * @brief Enables all the servos PWMs and adds to the scheduler a periodic + * method to check whether the single servos time expired + */ + [[nodiscard]] bool start(); + + /** + * @brief Wiggles the requested servo for 1 second + * + * @param servo The Mavlink requested servo + * @returns False if the servo does not appear in the list + */ + bool wiggleServo(ServosList servo); + + /** + * @brief Get the specified Servo's Position + * + * @param servo The Mavlink requested servo + * @return float The servo position in normalized notation [0-1] + */ + float getServoPosition(ServosList servo); + + /** + * @brief Toggles the servo valve passed via parameter. It sets the timings + * such that the valve opens for a certain amount of time, otherwise, if the + * valve is already open it closes it. + * + * @param servo The servo valve to toggle + */ + void toggleServo(ServosList servo); + + /** + * @brief Opens the servo valve passed via parameter for a certain amount of + * time. If the valve is already open it closes it. + * + * @param servo The servo valve to open + * @param time The time in [ms] + */ + void openServoAtomic(ServosList servo, uint32_t time); + + /** + * @brief Closes all the servo valves + */ + void closeAllServo(); + + /** + * @brief Sets a servo maximum aperture in normalized form. [0,1] + * + * @param servo The servo to which set the aperture + * @param aperture The aperture in [0,1] + */ + void setMaximumAperture(ServosList servo, float aperture); + + /** + * @brief Set the timing of a certain valve. + * @note THE TIMING IS IN [ms] + * + * @param servo The servo to which set the timing + * @param time The timing in [ms] + */ + void setTiming(ServosList servo, uint32_t time); + + /** + * @brief Get the Timing of the passed servo + * + * @param servo The servo whose timing is needed + * @return uint32_t The timing in [ms] + */ + uint32_t getTiming(ServosList servo); + +private: + /** + * @brief Set the Servo's position to the parameter one + * + * @param servo The servo to move + * @param position A normalized position [0, 1] + */ + void setServoPosition(ServosList servo, float position); + + Boardcore::Servo* getServo(ServosList servo); + + /** + * @brief Checks that the atomic timings for the servos didn't expire and + * sets the positioning according to them. + * @note Utilizes also the variable setFlag to understand when the position + * was set the last time. If greater than CONSTANT seconds ago, the function + * sets a little offset to the servo to avoid power angriness. + */ + void checkTimings(); + + // Create the list of timings for every servo + uint64_t timings[ServosList::ServosList_ENUM_END] = {0}; + + // This set of flags helps the controller to know when the servo have been + // set, in order to change slightly their angle after CONSTANT time, to + // avoid over consumption + uint64_t setFlag[ServosList::ServosList_ENUM_END] = {0}; + float openings[ServosList::ServosList_ENUM_END] = {1}; + uint64_t openingTimes[ServosList::ServosList_ENUM_END] = { + 100000}; // Default 100s + + // This set represents the events to throw at opening/closing of valves + uint8_t openingEvents[ServosList::ServosList_ENUM_END] = {0}; + uint8_t closingEvents[ServosList::ServosList_ENUM_END] = {0}; + + Boardcore::TaskScheduler* scheduler = nullptr; + + Boardcore::Servo* servo1; + Boardcore::Servo* servo2; + Boardcore::Servo* servo3; + Boardcore::Servo* servo4; + Boardcore::Servo* servo5; +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Actuators/ActuatorsData.h b/src/boards/RIG/Actuators/ActuatorsData.h new file mode 100644 index 0000000000000000000000000000000000000000..a233b16336e9356cecdfca9660462869414b268c --- /dev/null +++ b/src/boards/RIG/Actuators/ActuatorsData.h @@ -0,0 +1,57 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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> + +#include <ostream> + +namespace RIG +{ +struct ActuatorsData +{ + uint64_t timestamp; + uint8_t servoId; + float position; + + ActuatorsData() + { + timestamp = 0; + servoId = 0; + position = 0; + } + + ActuatorsData(uint64_t time, uint8_t servo, float pos) + : timestamp(time), servoId(servo), position(pos) + { + } + + static std::string header() { return "timestamp,servoId,position\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)servoId << "," << position << "\n"; + } +}; + +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/BoardScheduler.cpp b/src/boards/RIG/BoardScheduler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8f8eff0cd7d8a403db8603b003260f917d71ff91 --- /dev/null +++ b/src/boards/RIG/BoardScheduler.cpp @@ -0,0 +1,65 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 "BoardScheduler.h" + +using namespace Boardcore; + +namespace RIG +{ +BoardScheduler::BoardScheduler() +{ + scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4); + scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3); + scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2); + scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1); +} + +TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority) +{ + switch (priority.get()) + { + case miosix::PRIORITY_MAX: + return scheduler4; + case miosix::PRIORITY_MAX - 1: + return scheduler3; + case miosix::PRIORITY_MAX - 2: + return scheduler2; + case miosix::MAIN_PRIORITY: + return scheduler1; + default: + return scheduler1; + } +} + +bool BoardScheduler::start() +{ + return scheduler1->start() && scheduler2->start() && scheduler3->start() && + scheduler4->start(); +} + +bool BoardScheduler::isStarted() +{ + return scheduler1->isRunning() && scheduler2->isRunning() && + scheduler3->isRunning() && scheduler4->isRunning(); +} +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/BoardScheduler.h b/src/boards/RIG/BoardScheduler.h new file mode 100644 index 0000000000000000000000000000000000000000..f2d878112be17a233b7c6a1ab83bf255f95b5591 --- /dev/null +++ b/src/boards/RIG/BoardScheduler.h @@ -0,0 +1,62 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +/** + * @brief Class that wraps the 4 main task schedulers of the entire OBSW. + * There is a task scheduler for every miosix priority + */ +class BoardScheduler : public Boardcore::Module +{ +public: + BoardScheduler(); + + /** + * @brief Get the Scheduler object relative to the requested priority + * + * @param priority The task scheduler priority + * @return Boardcore::TaskScheduler& Reference to the requested task + * scheduler. + * @note Min priority scheduler is returned in case of non valid priority. + */ + Boardcore::TaskScheduler* getScheduler(miosix::Priority priority); + + [[nodiscard]] bool start(); + + /** + * @brief Returns if all the schedulers are up and running + */ + bool isStarted(); + +private: + Boardcore::TaskScheduler* scheduler1; + Boardcore::TaskScheduler* scheduler2; + Boardcore::TaskScheduler* scheduler3; + Boardcore::TaskScheduler* scheduler4; +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Buses.h b/src/boards/RIG/Buses.h new file mode 100644 index 0000000000000000000000000000000000000000..27a4fd08616256dc28ebfad401f39cd44d365b52 --- /dev/null +++ b/src/boards/RIG/Buses.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <drivers/spi/SPIBus.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +class Buses : public Boardcore::Module +{ +public: + Boardcore::SPIBus spi1; + Boardcore::SPIBus spi2; + Boardcore::SPIBus spi3; + Boardcore::SPIBus spi4; + Boardcore::SPIBus spi5; + Boardcore::SPIBus spi6; + + Buses() + : spi1(SPI1), spi2(SPI2), spi3(SPI3), spi4(SPI4), spi5(SPI5), spi6(SPI6) + { + } +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/CanHandler/CanHandler.cpp b/src/boards/RIG/CanHandler/CanHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..913bbe3016fadeadcdff32227022107c46833532 --- /dev/null +++ b/src/boards/RIG/CanHandler/CanHandler.cpp @@ -0,0 +1,314 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Alberto Nidasio, Matteo Pignataro + * + * 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 "CanHandler.h" + +#include <RIG/BoardScheduler.h> +#include <RIG/Buses.h> +#include <RIG/CanHandler/CanHandlerData.h> +#include <RIG/Configs/CanHandlerConfig.h> +#include <RIG/Sensors/Sensors.h> +#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h> +#include <RIG/StatesMonitor/StatesMonitor.h> +#include <common/CanConfig.h> +#include <common/Events.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> + +#include <functional> + +using namespace std; +using namespace placeholders; +using namespace Boardcore; +using namespace Canbus; +using namespace Common; +using namespace CanConfig; +using namespace RIG::CanHandlerConfig; + +namespace RIG +{ +CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched) +{ + // Configure the CAN driver + CanbusDriver::AutoBitTiming bitTiming; + bitTiming.baudRate = BAUD_RATE; + bitTiming.samplePoint = SAMPLE_POINT; + + CanbusDriver::CanbusConfig config; + + // NOTE configure the peripheral CAN1 due to shared configs + driver = new CanbusDriver(CAN1, config, bitTiming); + + // Create the protocol with the defined driver + protocol = + new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1), + miosix::PRIORITY_MAX - 1); + + // Accept messages only from the main and RIG board + protocol->addFilter(static_cast<uint8_t>(Board::MAIN), + static_cast<uint8_t>(Board::BROADCAST)); + protocol->addFilter(static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST)); + protocol->addFilter(static_cast<uint8_t>(Board::PAYLOAD), + static_cast<uint8_t>(Board::BROADCAST)); + driver->init(); +} + +bool CanHandler::start() +{ + // 0 if fail + uint8_t result = scheduler->addTask( // status + [&]() + { + GroundModeManagerState state = ModuleManager::getInstance() + .get<GroundModeManager>() + ->getStatus() + .state; + protocol->enqueueSimplePacket( + static_cast<uint8_t>(Priority::MEDIUM), + static_cast<uint8_t>(PrimaryType::STATUS), + static_cast<uint8_t>(Board::RIG), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(state), + (state == GroundModeManagerState::ARMED) ? 0x01 : 0x00); + }, + STATUS_TRANSMISSION_PERIOD); + // TODO look at the priorities of the CAN protocol threads + return protocol->start() && result != 0; +} + +bool CanHandler::isStarted() +{ + return protocol->isStarted() && scheduler->isRunning(); +} + +void CanHandler::sendEvent(EventId event) +{ + protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL), + static_cast<uint8_t>(PrimaryType::EVENTS), + static_cast<uint8_t>(Board::RIG), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(event)); +} + +void CanHandler::sendCanServoCommand(ServosList servo, bool targetState, + uint32_t delay) +{ + uint64_t payload = delay; + payload = payload << 8; + payload = payload | targetState; + protocol->enqueueSimplePacket(static_cast<uint8_t>(Priority::CRITICAL), + static_cast<uint8_t>(PrimaryType::COMMAND), + static_cast<uint8_t>(Board::RIG), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(servo), payload); +} + +void CanHandler::handleCanMessage(const CanMessage &msg) +{ + PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType()); + + // Depending on the received message, call the handling method + switch (msgType) + { + case PrimaryType::EVENTS: + { + handleCanEvent(msg); + break; + } + case PrimaryType::SENSORS: + { + handleCanSensor(msg); + break; + } + case PrimaryType::STATUS: + { + handleCanStatus(msg); + break; + } + default: + { + LOG_WARN(logger, "Received unsupported message type: type={}", + msgType); + break; + } + } +} + +void CanHandler::handleCanEvent(const CanMessage &msg) +{ + EventId eventId = static_cast<EventId>(msg.getSecondaryType()); + auto it = eventToEvent.find(eventId); + + // Check if the event is valid + if (it != eventToEvent.end()) + { + EventBroker::getInstance().post(it->second, TOPIC_CAN); + } + else + { + LOG_WARN(logger, "Received unsupported event: id={}", eventId); + } +} + +void CanHandler::handleCanSensor(const CanMessage &msg) +{ + SensorId sensorId = static_cast<SensorId>(msg.getSecondaryType()); + ModuleManager &modules = ModuleManager::getInstance(); + + // Depending on the sensor call the corresponding setter on the Sensors + // module + switch (sensorId) + { + case SensorId::CC_PRESSURE: + { + PressureData data = pressureDataFromCanMessage(msg); + modules.get<Sensors>()->setCCPressure(data); + + // Log the data + CanPressureSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.pressure = data.pressure; + log.sensorId = static_cast<uint8_t>(SensorId::CC_PRESSURE); + Logger::getInstance().log(log); + + break; + } + case SensorId::BOTTOM_TANK_PRESSURE: + { + PressureData data = pressureDataFromCanMessage(msg); + modules.get<Sensors>()->setBottomTankPressure(data); + + // Log the data + CanPressureSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.pressure = data.pressure; + log.sensorId = static_cast<uint8_t>(SensorId::BOTTOM_TANK_PRESSURE); + Logger::getInstance().log(log); + + break; + } + case SensorId::TOP_TANK_PRESSURE: + { + PressureData data = pressureDataFromCanMessage(msg); + modules.get<Sensors>()->setTopTankPressure(data); + + // Log the data + CanPressureSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.pressure = data.pressure; + log.sensorId = static_cast<uint8_t>(SensorId::TOP_TANK_PRESSURE); + Logger::getInstance().log(log); + + break; + } + case SensorId::TANK_TEMPERATURE: + { + TemperatureData data = temperatureDataFromCanMessage(msg); + modules.get<Sensors>()->setTankTemperature(data); + + // Log the data + CanTemperatureSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.temperature = data.temperature; + log.sensorId = static_cast<uint8_t>(SensorId::TANK_TEMPERATURE); + + break; + } + case SensorId::MOTOR_ACTUATORS_CURRENT: + { + CurrentData data = currentDataFromCanMessage(msg); + modules.get<Sensors>()->setMotorCurrent(data); + + // Log the data + CanCurrentSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.sensorId = + static_cast<uint8_t>(SensorId::MOTOR_ACTUATORS_CURRENT); + log.current = data.current; + Logger::getInstance().log(log); + + break; + } + case SensorId::MAIN_BOARD_CURRENT: + { + CurrentData data = currentDataFromCanMessage(msg); + modules.get<Sensors>()->setMainCurrent(data); + + // Log the data + CanCurrentSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.sensorId = static_cast<uint8_t>(SensorId::MAIN_BOARD_CURRENT); + log.current = data.current; + Logger::getInstance().log(log); + + break; + } + case SensorId::PAYLOAD_BOARD_CURRENT: + { + CurrentData data = currentDataFromCanMessage(msg); + modules.get<Sensors>()->setPayloadCurrent(data); + + // Log the data + CanCurrentSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.sensorId = + static_cast<uint8_t>(SensorId::PAYLOAD_BOARD_CURRENT); + log.current = data.current; + Logger::getInstance().log(log); + + break; + } + case SensorId::MOTOR_BOARD_VOLTAGE: + { + BatteryVoltageSensorData data = voltageDataFromCanMessage(msg); + modules.get<Sensors>()->setMotorVoltage(data); + + // Log the data + CanVoltageSensor log; + log.timestamp = TimestampTimer::getTimestamp(); + log.sensorId = static_cast<uint8_t>(SensorId::MOTOR_BOARD_VOLTAGE); + log.voltage = data.batVoltage; + Logger::getInstance().log(log); + + break; + } + default: + { + LOG_WARN(logger, "Received unsupported sensor data: id={}", + sensorId); + } + } +} + +void CanHandler::handleCanStatus(const CanMessage &msg) +{ + Board source = static_cast<Board>(msg.getSource()); + // uint8_t status = msg.getSecondaryType(); + bool armed = msg.payload[0]; + + // Set the state inside the monitor + ModuleManager::getInstance().get<StatesMonitor>()->setBoardStatus( + source, armed ? 2 : 1); +} + +} // namespace RIG diff --git a/src/boards/RIG/CanHandler/CanHandler.h b/src/boards/RIG/CanHandler/CanHandler.h new file mode 100644 index 0000000000000000000000000000000000000000..071a9f45c9bf79a0f2d1a575686b786f70a7359f --- /dev/null +++ b/src/boards/RIG/CanHandler/CanHandler.h @@ -0,0 +1,86 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Federico Mandelli, 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 <RIG/BoardScheduler.h> +#include <common/CanConfig.h> +#include <common/Mavlink.h> +#include <drivers/canbus/CanProtocol/CanProtocol.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ + +class CanHandler : public Boardcore::Module +{ + +public: + explicit CanHandler(Boardcore::TaskScheduler *sched); + + /** + * @brief Adds the periodic task to the scheduler and starts the protocol + * threads + */ + bool start(); + + /** + * @brief Returns true if the protocol threads are started and the scheduler + * is running + */ + bool isStarted(); + + /** + * @brief Sends a CAN event on the bus + */ + void sendEvent(Common::CanConfig::EventId event); + + /** + * @brief Sends a can command (servo actuation command) specifying the + * target servo, the target state and eventually the delta [ms] in which the + * servo remains open + */ + void sendCanServoCommand(ServosList servo, bool targetState, + uint32_t delay); + +private: + /** + * @brief Handles a generic CAN message and dispatch the message to the + * correct handler + */ + void handleCanMessage(const Boardcore::Canbus::CanMessage &msg); + + // CAN message handlers + void handleCanEvent(const Boardcore::Canbus::CanMessage &msg); + void handleCanSensor(const Boardcore::Canbus::CanMessage &msg); + void handleCanStatus(const Boardcore::Canbus::CanMessage &msg); + + // CAN interfaces + Boardcore::Canbus::CanbusDriver *driver; + Boardcore::Canbus::CanProtocol *protocol; + + Boardcore::TaskScheduler *scheduler; + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler"); +}; + +} // namespace RIG diff --git a/src/boards/RIG/CanHandler/CanHandlerData.h b/src/boards/RIG/CanHandler/CanHandlerData.h new file mode 100644 index 0000000000000000000000000000000000000000..be4ab08bda9c6216467762ef3d8524ee9f57a78d --- /dev/null +++ b/src/boards/RIG/CanHandler/CanHandlerData.h @@ -0,0 +1,92 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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> + +#include <iostream> +#include <string> + +namespace RIG +{ +// This class defines all the types received from the CAN bus +struct CanPressureSensor +{ + uint64_t timestamp = 0; + uint8_t sensorId = 0; + float pressure = 0; + + static std::string header() { return "timestamp,sensorId,pressure"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)sensorId << "," << pressure << "\n"; + } +}; + +struct CanTemperatureSensor +{ + uint64_t timestamp = 0; + uint8_t sensorId = 0; + float temperature = 0; + + static std::string header() { return "timestamp,sensorId,temperature"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)sensorId << "," << temperature << "\n"; + } +}; + +struct CanCurrentSensor +{ + uint64_t timestamp = 0; + uint8_t sensorId = 0; + uint8_t boardId = 0; + float current = 0; + + static std::string header() { return "timestamp,sensorId,boardId,current"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)sensorId << "," << (int)boardId << "," + << current << "\n"; + } +}; + +struct CanVoltageSensor +{ + uint64_t timestamp = 0; + uint8_t sensorId = 0; + uint8_t boardId = 0; + float voltage = 0; + + static std::string header() { return "timestamp,sensorId,boardId,voltage"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)sensorId << "," << (int)boardId << "," + << voltage << "\n"; + } +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Configs/ActuatorsConfig.h b/src/boards/RIG/Configs/ActuatorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..78c7081d9f3a6f9afb8c063517130bc7f0c44460 --- /dev/null +++ b/src/boards/RIG/Configs/ActuatorsConfig.h @@ -0,0 +1,69 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <drivers/timer/PWM.h> +#include <drivers/timer/TimerUtils.h> + +namespace RIG +{ +namespace Config +{ +namespace Servos +{ +static TIM_TypeDef* const SERVO1_TIMER = TIM4; +static TIM_TypeDef* const SERVO2_TIMER = TIM11; +static TIM_TypeDef* const SERVO3_TIMER = TIM3; +static TIM_TypeDef* const SERVO4_TIMER = TIM10; +static TIM_TypeDef* const SERVO5_TIMER = TIM8; + +constexpr Boardcore::TimerUtils::Channel SERVO1_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_2; +constexpr Boardcore::TimerUtils::Channel SERVO2_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; +constexpr Boardcore::TimerUtils::Channel SERVO3_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; +constexpr Boardcore::TimerUtils::Channel SERVO4_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; +constexpr Boardcore::TimerUtils::Channel SERVO5_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; + +constexpr uint16_t MIN_PULSE = 900; +constexpr uint16_t MAX_PULSE = 2000; + +constexpr uint16_t SERVO_TIMINGS_CHECK_PERIOD = 100; +constexpr uint16_t SERVO_CONFIDENCE_TIME = 500; // 0.5s +constexpr float SERVO_CONFIDENCE = 1 / 50.0; // 2% + +constexpr uint32_t DEFAULT_FILLING_OPENING_TIME = 15000; // 15s +constexpr uint32_t DEFAULT_VENTING_OPENING_TIME = 15000; // 15s +constexpr uint32_t DEFAULT_MAIN_OPENING_TIME = 6000; // 6s +constexpr uint32_t DEFAULT_RELEASE_OPENING_TIME = 10000; // 10s +constexpr uint32_t DEFAULT_DISCONNECT_OPENING_TIME = 10000; // 10s + +constexpr float DEFAULT_FILLING_MAXIMUM_APERTURE = 0.97f; +constexpr float DEFAULT_VENTING_MAXIMUM_APERTURE = 0.40f; +constexpr float DEFAULT_MAIN_MAXIMUM_APERTURE = 0.87f; +constexpr float DEFAULT_RELEASE_MAXIMUM_APERTURE = 0.97f; +constexpr float DEFAULT_DISCONNECT_MAXIMUM_APERTURE = 0.110f; +} // namespace Servos +} // namespace Config +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Configs/CanHandlerConfig.h b/src/boards/RIG/Configs/CanHandlerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..bf32a6a5f94142f082b48348b2263803c359ee7e --- /dev/null +++ b/src/boards/RIG/Configs/CanHandlerConfig.h @@ -0,0 +1,38 @@ +/* Copyright (c) 2023 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 <common/Events.h> + +namespace RIG +{ + +namespace CanHandlerConfig +{ + +constexpr unsigned int PITOT_TRANSMISSION_PERIOD = 20; // ms +constexpr unsigned int STATUS_TRANSMISSION_PERIOD = 2000; // ms + +} // namespace CanHandlerConfig + +} // namespace RIG diff --git a/src/boards/RIG/Configs/GroundModeManagerConfig.h b/src/boards/RIG/Configs/GroundModeManagerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..c7e69b7a3c568b9c0904db3e9974052666848983 --- /dev/null +++ b/src/boards/RIG/Configs/GroundModeManagerConfig.h @@ -0,0 +1,40 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 RIG +{ +namespace Config +{ +namespace Ignition +{ +constexpr uint32_t DEFAULT_IGNITION_WAITING_TIME = 5451; // ms + +constexpr uint32_t DEAD_TIME_AFTER_BURN = 900; // ms +constexpr uint32_t NUMBER_NITROGEN_BUMPS = 15; +constexpr uint32_t NITROGEN_OPENING_TIME = 2700; // ms + +} // namespace Ignition +} // namespace Config +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Configs/RadioConfig.h b/src/boards/RIG/Configs/RadioConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..b84d67c11b3d4f3f1f1ae811ab9f3dd2a7947e6f --- /dev/null +++ b/src/boards/RIG/Configs/RadioConfig.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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> +#include <stdint.h> + +namespace RIG +{ +namespace Config +{ +namespace Radio +{ +// Mavlink driver template parameters +constexpr uint32_t RADIO_PKT_LENGTH = 255; +constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20; +constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; + +// Mavlink driver parameters +constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0; +constexpr uint16_t MAV_OUT_BUFFER_MAX_AGE = 10; + +// Mavlink ids +constexpr uint8_t MAV_SYSTEM_ID = 171; +constexpr uint8_t MAV_COMPONENT_ID = 96; + +// Circular buffer size (number of maximum messages that the RIG can answer +// after a ping message from conRIG) +constexpr uint8_t RADIO_CIRCULAR_BUFFER_SIZE = 6; + +// Threshold to consider a new command correct and not a false trigger. +// Expressed in [ms] +constexpr long long int RADIO_LAST_COMMAND_THRESHOLD = 1000; + +} // namespace Radio +} // namespace Config +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Configs/RefuelingConfig.h b/src/boards/RIG/Configs/RefuelingConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..7ab25b2ae9cac31a41a2df42e6a3ef8fa9f30c0f --- /dev/null +++ b/src/boards/RIG/Configs/RefuelingConfig.h @@ -0,0 +1,35 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 RIG +{ +namespace Config +{ +namespace Refueling +{ + +} // namespace Refueling +} // namespace Config +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Configs/SensorsConfig.h b/src/boards/RIG/Configs/SensorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..1747a340907bdcfca9a46d30bd8e3ac9183f9aa4 --- /dev/null +++ b/src/boards/RIG/Configs/SensorsConfig.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 RIG +{ +namespace Config +{ +namespace Sensors +{ +constexpr uint16_t ADC_SAMPLE_PERIOD = 10; +constexpr uint16_t LOAD_CELL_SAMPLE_PERIOD = 13; +constexpr uint16_t THERMOCOUPLE_SAMPLE_PERIOD = 100; + +constexpr float ADC1_CH1_SHUNT_RESISTANCE = 7.3; +constexpr float ADC1_CH2_SHUNT_RESISTANCE = 7.3; +constexpr float ADC1_CH3_SHUNT_RESISTANCE = 7.3; +constexpr float ADC1_CH4_SHUNT_RESISTANCE = 7.3; + +constexpr float ADC2_CH1_SHUNT_RESISTANCE = 7.3; + +constexpr float FILLING_MAX_PRESSURE = 100; // bar +constexpr float TANK_TOP_MAX_PRESSURE = 100; // bar +constexpr float TANK_BOTTOM_MAX_PRESSURE = 100; // bar +constexpr float VESSEL_MAX_PRESSURE = 400; // bar + +constexpr float SENSOR_MIN_CURRENT = 4; // mA +constexpr float SENSOR_MAX_CURRENT = 20; // mA + +constexpr float LOAD_CELL2_OFFSET = -38150; +constexpr float LOAD_CELL1_OFFSET = -93539.8f; + +constexpr float LOAD_CELL2_SCALE = -5.6188e-5f; +constexpr float LOAD_CELL1_SCALE = -2.251307e-5f; + +constexpr float VOLTAGE_CONVERSION_FACTOR = 13.58f; +constexpr float CURRENT_CONVERSION_FACTOR = 13.645f; +constexpr float VOLTAGE_CURRENT_CONVERSION_FACTOR = 0.020f; // 40 mV/A +} // namespace Sensors +} // namespace Config +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Configs/StatesMonitorConfig.h b/src/boards/RIG/Configs/StatesMonitorConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..37d3eeefec522bb5c1c2e3f61b9bd2b489d7a78f --- /dev/null +++ b/src/boards/RIG/Configs/StatesMonitorConfig.h @@ -0,0 +1,38 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 RIG +{ +namespace Config +{ +namespace StatesMonitor +{ +constexpr int BOARDS_NUMBER = 5; +constexpr long long int MAX_TIMEOUT = 5000; // [ms] +constexpr uint32_t UPDATE_PERIOD = 1000; +} // namespace StatesMonitor +} // namespace Config +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Configs/TARSConfig.h b/src/boards/RIG/Configs/TARSConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..a6031a3053a64e5986c3eae627b796a476e32c1c --- /dev/null +++ b/src/boards/RIG/Configs/TARSConfig.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 RIG +{ +namespace Config +{ +namespace TARS +{ +constexpr uint16_t TARS_UPDATE_PERIOD = 10; +constexpr uint8_t TARS_FILTER_SAMPLE_NUMBER = 10; +constexpr float TARS_TARGET_TEMPERATURE = 25; // Celsius + +constexpr uint32_t TARS_WASHING_OPENING_TIME = 5000; // [ms] +constexpr uint32_t TARS_WASHING_TIME_DELAY = + 1000; //[ms] Represents a delay of opening two different valves +constexpr uint32_t TARS_FILLING_OPENING_TIME = 900000; // [ms] 15min +constexpr uint32_t TARS_PRESSURE_STABILIZE_WAIT_TIME = 1000; // [ms] 1s + +constexpr uint16_t TARS_NUMBER_MASS_STABLE_ITERATIONS = 2; + +constexpr float TARS_MASS_TOLERANCE = 0.2; //[kg] +constexpr float TARS_PRESSURE_TOLERANCE = 0.035; //[bar] +} // namespace TARS +} // namespace Config +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Radio/Radio.cpp b/src/boards/RIG/Radio/Radio.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3febc05c03b92e5878092eec5f7a67abf165b43b --- /dev/null +++ b/src/boards/RIG/Radio/Radio.cpp @@ -0,0 +1,455 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Actuators/Actuators.h> +#include <RIG/Buses.h> +#include <RIG/CanHandler/CanHandler.h> +#include <RIG/Radio/Radio.h> +#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h> +#include <RIG/TMRepository/TMRepository.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/interrupt/external_interrupts.h> +#include <events/EventBroker.h> +#include <radio/SX1278/SX1278Frontends.h> +#include <radio/SerialTransceiver/SerialTransceiver.h> + +using namespace Boardcore; +using namespace miosix; + +void __attribute__((used)) EXTI5_IRQHandlerImpl() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<RIG::Radio>()->transceiver != nullptr) + { + modules.get<RIG::Radio>()->transceiver->handleDioIRQ(); + } +} + +void __attribute__((used)) EXTI12_IRQHandlerImpl() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<RIG::Radio>()->transceiver != nullptr) + { + modules.get<RIG::Radio>()->transceiver->handleDioIRQ(); + } +} + +void __attribute__((used)) EXTI13_IRQHandlerImpl() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<RIG::Radio>()->transceiver != nullptr) + { + modules.get<RIG::Radio>()->transceiver->handleDioIRQ(); + } +} +namespace RIG +{ + +Radio::Radio() +{ + // Initialize previous state to avoid undefined behaviour + previousState.arm_switch = 0; + previousState.filling_valve_btn = 0; + previousState.ignition_btn = 0; + previousState.quick_connector_btn = 0; + previousState.release_pressure_btn = 0; + previousState.start_tars_btn = 0; + previousState.venting_valve_btn = 0; +} + +Radio::~Radio() +{ + mavDriver->stop(); + delete mavDriver; +} + +bool Radio::start() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Config the transceiver + SX1278Lora::Config config{}; + config.power = 2; + config.ocp = 0; // Over current protection + config.coding_rate = SX1278Lora::Config::Cr::CR_1; + config.spreading_factor = SX1278Lora::Config::Sf::SF_7; + + std::unique_ptr<SX1278::ISX1278Frontend> frontend = + std::make_unique<EbyteFrontend>(radio::txEn::getPin(), + radio::rxEn::getPin()); + + transceiver = new SX1278Lora( + modules.get<Buses>()->spi4, radio::cs::getPin(), radio::dio0::getPin(), + radio::dio1::getPin(), radio::dio3::getPin(), SPI::ClockDivider::DIV_64, + std::move(frontend)); + + SX1278Lora::Error error = transceiver->init(config); + + // Config mavDriver + mavDriver = new MavDriver( + transceiver, + [=](MavDriver*, const mavlink_message_t& msg) + { this->handleMavlinkMessage(msg); }, + Config::Radio::MAV_SLEEP_AFTER_SEND, + Config::Radio::MAV_OUT_BUFFER_MAX_AGE); + + // This thread allows the radio to exit a possible deadlock situation where + // the driver waits for a missed interrupt. By default the priority is MAX - + // 1 + radioBackupDIO = std::thread( + [=]() + { + while (true) + { + Thread::sleep( + 5000); // wait 5 seconds and then launch the interrupt + { + FastInterruptDisableLock lock; + transceiver->handleDioIRQ(); + } + } + }); + + // In case of failure the mav driver must be created at least + if (error != SX1278Lora::Error::NONE) + { + return false; + } + + return mavDriver->start(); +} + +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); + buffer.put(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); + buffer.put(nackMsg); +} + +void Radio::logStatus() +{ + MavlinkStatus stats = mavDriver->getStatus(); + SDlogger.log(stats); +} + +bool Radio::isStarted() { return mavDriver->isStarted(); } + +void Radio::handleMavlinkMessage(const mavlink_message_t& msg) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (msg.msgid) + { + case MAVLINK_MSG_ID_PING_TC: + { + // Do nothing just add the ack to the queue + break; + } + case MAVLINK_MSG_ID_COMMAND_TC: + { + // Important to return instead of breaking because handle command + // can decide whether to send ack or nack + return handleCommand(msg); + } + case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC: + { + SystemTMList tmId = static_cast<SystemTMList>( + mavlink_msg_system_tm_request_tc_get_tm_id(&msg)); + switch (tmId) + { + default: + { + mavlink_message_t response = + modules.get<TMRepository>()->packSystemTm( + tmId, msg.msgid, msg.seq); + + // Add the response to the buffer + buffer.put(response); + + if (response.msgid == MAVLINK_MSG_ID_NACK_TM) + { + // No break to let tmRepo decide for nack + return; + } + break; + } + } + break; + } + case MAVLINK_MSG_ID_CONRIG_STATE_TC: + { + // In this case all the messages in the queue must be sent + // after adding the ack to the queue. DO NOT USE get AND isFull with + // the generic message class type + sendAck(msg); + size_t count = buffer.count(); + + for (size_t i = 0; i < count; i++) + { + // Handle the possible exception + try + { + mavDriver->enqueueMsg(buffer.pop()); + } + catch (const std::exception& e) + { + LOG_ERR(logger, "Circular buffer generated an exception"); + } + } + + // After sending all the packets in buffer send the RIG state + // messages + mavDriver->enqueueMsg(modules.get<TMRepository>()->packSystemTm( + SystemTMList::MAV_GSE_ID, msg.msgid, msg.seq)); + mavDriver->enqueueMsg(modules.get<TMRepository>()->packSystemTm( + SystemTMList::MAV_MOTOR_ID, msg.msgid, msg.seq)); + + mavlink_conrig_state_tc_t state; + mavlink_msg_conrig_state_tc_decode(&msg, &state); + + // Extract the buttons data and if there is a slope post the event + if (previousState.arm_switch == 0 && state.arm_switch == 1) + { + if (getTick() > lastManualCommand + + Config::Radio::RADIO_LAST_COMMAND_THRESHOLD) + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + EventBroker::getInstance().post(Common::TMTC_ARM, + Common::TOPIC_MOTOR); + modules.get<CanHandler>()->sendEvent( + Common::CanConfig::EventId::ARM); + + lastManualCommand = getTick(); + } + } + if (previousState.filling_valve_btn == 0 && + state.filling_valve_btn == 1) + { + if (getTick() > lastManualCommand + + Config::Radio::RADIO_LAST_COMMAND_THRESHOLD) + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + modules.get<Actuators>()->toggleServo( + ServosList::FILLING_VALVE); + + lastManualCommand = getTick(); + } + } + if (previousState.ignition_btn == 0 && state.ignition_btn == 1) + { + if (getTick() > lastManualCommand + + Config::Radio::RADIO_LAST_COMMAND_THRESHOLD) + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + EventBroker::getInstance().post(Common::MOTOR_IGNITION, + Common::TOPIC_MOTOR); + lastManualCommand = getTick(); + } + } + if (previousState.quick_connector_btn == 0 && + state.quick_connector_btn == 1) + { + if (getTick() > lastManualCommand + + Config::Radio::RADIO_LAST_COMMAND_THRESHOLD) + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + modules.get<Actuators>()->toggleServo( + ServosList::DISCONNECT_SERVO); + lastManualCommand = getTick(); + } + } + if (previousState.release_pressure_btn == 0 && + state.release_pressure_btn == 1) + { + if (getTick() > lastManualCommand + + Config::Radio::RADIO_LAST_COMMAND_THRESHOLD) + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + modules.get<Actuators>()->toggleServo( + ServosList::RELEASE_VALVE); + + lastManualCommand = getTick(); + } + } + if (previousState.start_tars_btn == 0 && state.start_tars_btn == 1) + { + if (getTick() > lastManualCommand + + Config::Radio::RADIO_LAST_COMMAND_THRESHOLD) + { + EventBroker::getInstance().post(Common::MOTOR_START_TARS, + Common::TOPIC_TARS); + lastManualCommand = getTick(); + } + } + if (previousState.venting_valve_btn == 0 && + state.venting_valve_btn == 1) + { + if (getTick() > lastManualCommand + + Config::Radio::RADIO_LAST_COMMAND_THRESHOLD) + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + modules.get<Actuators>()->toggleServo( + ServosList::VENTING_VALVE); + + lastManualCommand = getTick(); + } + } + + if (previousState.arm_switch == 1 && state.arm_switch == 0) + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + EventBroker::getInstance().post(Common::TMTC_DISARM, + Common::TOPIC_MOTOR); + modules.get<CanHandler>()->sendEvent( + Common::CanConfig::EventId::DISARM); + + lastManualCommand = getTick(); + } + + previousState = state; + + // Important to return and not to break to avoid a redundant ack + return; + } + case MAVLINK_MSG_ID_WIGGLE_SERVO_TC: + { + EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION, + Common::TOPIC_TARS); + // Wiggle only the machine is in ready state + ServosList servo = static_cast<ServosList>( + mavlink_msg_wiggle_servo_tc_get_servo_id(&msg)); + + if (modules.get<GroundModeManager>()->getStatus().state == + GroundModeManagerState::READY) + { + bool result = modules.get<Actuators>()->wiggleServo(servo); + + if (!result) + { + sendNack(msg); + return; + } + } + else + { + sendNack(msg); + return; + } + + break; + } + case MAVLINK_MSG_ID_SET_ATOMIC_VALVE_TIMING_TC: + { + uint32_t timing = + mavlink_msg_set_atomic_valve_timing_tc_get_maximum_timing(&msg); + ServosList servo = static_cast<ServosList>( + mavlink_msg_set_atomic_valve_timing_tc_get_servo_id(&msg)); + modules.get<Actuators>()->setTiming(servo, timing); + break; + } + case MAVLINK_MSG_ID_SET_VALVE_MAXIMUM_APERTURE_TC: + { + float aperture = + mavlink_msg_set_valve_maximum_aperture_tc_get_maximum_aperture( + &msg); + ServosList servo = static_cast<ServosList>( + mavlink_msg_set_valve_maximum_aperture_tc_get_servo_id(&msg)); + modules.get<Actuators>()->setMaximumAperture(servo, aperture); + break; + } + case MAVLINK_MSG_ID_SET_IGNITION_TIME_TC: + { + uint32_t timing = mavlink_msg_set_ignition_time_tc_get_timing(&msg); + modules.get<GroundModeManager>()->setIgnitionTime(timing); + break; + } + default: + { + // Unknown message + sendNack(msg); + return; + } + } + sendAck(msg); +} + +void Radio::handleCommand(const mavlink_message_t& msg) +{ + uint8_t command = mavlink_msg_command_tc_get_command_id(&msg); + switch (command) + { + case MAV_CMD_START_LOGGING: + { + bool result = Logger::getInstance().start(); + + // If the log reached the maximum allowed number then send a nack + if (!result || Logger::getInstance().getCurrentLogNumber() == + Logger::getMaxFilenameNumber()) + { + sendNack(msg); + return; + } + + break; + } + case MAV_CMD_STOP_LOGGING: + { + Logger::getInstance().stop(); + break; + } + case MAV_CMD_CALIBRATE: + { + EventBroker::getInstance().post(Common::TMTC_CALIBRATE, + Common::TOPIC_MOTOR); + ModuleManager::getInstance().get<CanHandler>()->sendEvent( + Common::CanConfig::EventId::CALIBRATE); + break; + } + default: + { + // Send a nack for unknown message + sendNack(msg); + return; + } + } + + sendAck(msg); +} + +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Radio/Radio.h b/src/boards/RIG/Radio/Radio.h new file mode 100644 index 0000000000000000000000000000000000000000..7f91e498ed11d0e89f11f46965523013103212c6 --- /dev/null +++ b/src/boards/RIG/Radio/Radio.h @@ -0,0 +1,102 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Configs/RadioConfig.h> +#include <common/Mavlink.h> +#include <radio/MavlinkDriver/MavlinkDriver.h> +#include <radio/SX1278/SX1278Lora.h> +#include <utils/collections/CircularBuffer.h> + +#include <thread> +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Lora::MTU, + Config::Radio::RADIO_OUT_QUEUE_SIZE, + Config::Radio::RADIO_MAV_MSG_LENGTH>; +class Radio : public Boardcore::Module +{ +public: + Radio(); + + ~Radio(); + + /** + * @brief Starts the MavlinkDriver. + */ + [[nodiscard]] bool start(); + + /** + * @brief Prepares and adds to the buffer an ack message for the given + * message. + * + * @param msg The message received that we need to acknowledge. + */ + void sendAck(const mavlink_message_t& msg); + + /** + * @brief Prepares and adds to the buffer a nack message for the given + * message. + * + * @param msg The message received that we need to not acknowledge. + */ + void sendNack(const mavlink_message_t& msg); + + /** + * @brief Saves the MavlinkDriver and transceiver status. + */ + void logStatus(); + + /** + * @brief Returns if the radio module is correctly started + */ + bool isStarted(); + + Boardcore::SX1278Lora* transceiver = nullptr; + MavDriver* mavDriver = nullptr; + +private: + /** + * @brief Called by the MavlinkDriver when a message is received. + */ + void handleMavlinkMessage(const mavlink_message_t& msg); + + /** + * @brief Called by handleMavlinkMessage to handle a command message. + */ + void handleCommand(const mavlink_message_t& msg); + + mavlink_conrig_state_tc_t previousState; + Boardcore::CircularBuffer<mavlink_message_t, + Config::Radio::RADIO_CIRCULAR_BUFFER_SIZE> + buffer; + std::thread radioBackupDIO; + + long long int lastManualCommand = + 0; // Specifies the last tick [ms] in which a command is executed + Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance(); + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio"); +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Sensors/ADCsData.h b/src/boards/RIG/Sensors/ADCsData.h new file mode 100644 index 0000000000000000000000000000000000000000..464f702fefd458f323c77af60749977d0e6602e3 --- /dev/null +++ b/src/boards/RIG/Sensors/ADCsData.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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/ADS131M04/ADS131M04Data.h> + +namespace RIG +{ +struct ADCsData : Boardcore::ADS131M04Data +{ + uint8_t ADCnumber; + + ADCsData() : ADS131M04Data{0, 0, 0, 0, 0} { ADCnumber = 0; } + + ADCsData(uint64_t time, uint8_t num, float ch1, float ch2, float ch3, + float ch4) + : ADS131M04Data{time, ch1, ch2, ch3, ch4} + { + ADCnumber = num; + } + + static std::string header() + { + return "timestamp,ADCnumber,voltage_channel_1,voltage_channel_2," + "voltage_channel_" + "3,voltage_channel_4\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)ADCnumber << "," << voltage[0] << "," + << voltage[1] << "," << voltage[2] << "," << voltage[3] << "\n"; + } +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Sensors/LoadCellsData.h b/src/boards/RIG/Sensors/LoadCellsData.h new file mode 100644 index 0000000000000000000000000000000000000000..6628bf0aafaac7b778d9281b2dddce9d886231e9 --- /dev/null +++ b/src/boards/RIG/Sensors/LoadCellsData.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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/SensorData.h> +#include <stdint.h> + +namespace RIG +{ +struct LoadCellsData : Boardcore::LoadCellData +{ + uint8_t cellNumber; + + LoadCellsData() : LoadCellData{0, 0} { cellNumber = 0; } + + LoadCellsData(uint64_t time, uint8_t cell, float l) + : Boardcore::LoadCellData{time, l} + { + cellNumber = cell; + } + + static std::string header() { return "loadTimestamp,cellNumber,load\n"; } + + void print(std::ostream& os) const + { + os << loadTimestamp << "," << (int)cellNumber << "," << load << "," + << "\n"; + } +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Sensors/Sensors.cpp b/src/boards/RIG/Sensors/Sensors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..96ecbd2648e0edc2378a137ac0635a2dc50a7c07 --- /dev/null +++ b/src/boards/RIG/Sensors/Sensors.cpp @@ -0,0 +1,491 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Buses.h> +#include <RIG/Configs/SensorsConfig.h> +#include <RIG/Sensors/ADCsData.h> +#include <RIG/Sensors/LoadCellsData.h> +#include <RIG/Sensors/Sensors.h> +#include <drivers/timer/TimestampTimer.h> + +using namespace Boardcore; +using namespace miosix; +using namespace std; + +namespace RIG +{ +ADS131M04Data Sensors::getADC1LastSample() +{ + PauseKernelLock l; + return adc1->getLastSample(); +} + +ADS131M04Data Sensors::getADC2LastSample() +{ + PauseKernelLock l; + return adc2->getLastSample(); +} + +HX711Data Sensors::getTankWeight() +{ + PauseKernelLock l; + HX711Data sample = loadCell2->getLastSample(); + sample.load -= offsetLoadCell2; + sample.load = -sample.load; + return sample; +} + +HX711Data Sensors::getVesselWeight() +{ + PauseKernelLock l; + HX711Data sample = loadCell1->getLastSample(); + sample.load -= offsetLoadCell1; + return sample; +} + +TemperatureData Sensors::getThermocoupleLastSample() +{ + PauseKernelLock l; + return ((!canSensorFlag) ? thermocouple->getLastSample() + : canTankTemperature); +} + +PressureData Sensors::getFillingPress() +{ + // It is important to call the getter method to separate the critical + // section from the calculation ones + ADS131M04Data lastSample = getADC1LastSample(); + + // Calculate the current and then based on linear relation calculate the + // pressure + float current = + lastSample.voltage[1] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE; + + // Convert the current in mA + current = current * 1000; + + // Calculate the pressure given the linear formula 4..20mA for + // 0..max_pressure + + // shift the current to let the line pass through 0 point + current = current - Config::Sensors::SENSOR_MIN_CURRENT; + + float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT - + Config::Sensors::SENSOR_MIN_CURRENT)) * + Config::Sensors::FILLING_MAX_PRESSURE; + + return PressureData{lastSample.timestamp, pressure}; +} + +PressureData Sensors::getTankTopPress() +{ + // It is important to call the getter method to separate the critical + // section from the calculation ones + ADS131M04Data lastSample = getADC1LastSample(); + + // Calculate the current and then based on linear relation calculate the + // pressure + float current = + lastSample.voltage[3] / Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE; + + // Convert the current in mA + current = current * 1000; + + // Calculate the pressure given the linear formula 4..20mA for + // 0..max_pressure + + // shift the current to let the line pass through 0 point + current = current - Config::Sensors::SENSOR_MIN_CURRENT; + + float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT - + Config::Sensors::SENSOR_MIN_CURRENT)) * + Config::Sensors::TANK_TOP_MAX_PRESSURE; + + return ((!canSensorFlag) ? PressureData{lastSample.timestamp, pressure} + : canTopTankPressure); +} + +PressureData Sensors::getTankBottomPress() +{ + // It is important to call the getter method to separate the critical + // section from the calculation ones + ADS131M04Data lastSample = getADC1LastSample(); + + // Calculate the current and then based on linear relation calculate the + // pressure + float current = + lastSample.voltage[2] / Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE; + + // Convert the current in mA + current = current * 1000; + + // Calculate the pressure given the linear formula 4..20mA for + // 0..max_pressure + + // shift the current to let the line pass through 0 point + current = current - Config::Sensors::SENSOR_MIN_CURRENT; + + float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT - + Config::Sensors::SENSOR_MIN_CURRENT)) * + Config::Sensors::TANK_BOTTOM_MAX_PRESSURE; + + return ((!canSensorFlag) ? PressureData{lastSample.timestamp, pressure} + : canBottomTankPressure); +} + +PressureData Sensors::getVesselPress() +{ + // It is important to call the getter method to separate the critical + // section from the calculation ones + ADS131M04Data lastSample = getADC1LastSample(); + + // Calculate the current and then based on linear relation calculate the + // pressure + float current = + lastSample.voltage[0] / Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE; + + // Convert the current in mA + current = current * 1000; + + // Calculate the pressure given the linear formula 4..20mA for + // 0..max_pressure + + // shift the current to let the line pass through 0 point + current = current - Config::Sensors::SENSOR_MIN_CURRENT; + + float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT - + Config::Sensors::SENSOR_MIN_CURRENT)) * + Config::Sensors::VESSEL_MAX_PRESSURE; + + return PressureData{lastSample.timestamp, pressure}; +} + +BatteryVoltageSensorData Sensors::getBatteryVoltage() +{ + // Get the analog ADC2 reading + ADS131M04Data lastSample = getADC2LastSample(); + + // Raw reading + float voltage = + Config::Sensors::VOLTAGE_CONVERSION_FACTOR * lastSample.voltage[2]; + + BatteryVoltageSensorData result{}; + + result.voltage = voltage; + result.voltageTimestamp = lastSample.timestamp; + return result; +} + +CurrentData Sensors::getCurrent() +{ + // Get the analog ADC2 reading + ADS131M04Data lastSample = getADC2LastSample(); + + // Raw voltage reading + float voltage = + Config::Sensors::CURRENT_CONVERSION_FACTOR * lastSample.voltage[3]; + + // 2.5V is half the dynamic + float current = -((voltage - 2.52) / + Config::Sensors::VOLTAGE_CURRENT_CONVERSION_FACTOR); + + CurrentData result{}; + + result.current = current; + result.currentTimestamp = lastSample.timestamp; + return result; +} + +PressureData Sensors::getCCPress() +{ + PauseKernelLock lock; + return canCCPressure; +} + +CurrentData Sensors::getMainCurrent() +{ + PauseKernelLock lock; + return (!canSensorFlag) ? CurrentData{} : canMainCurrent; +} + +CurrentData Sensors::getPayloadCurrent() +{ + PauseKernelLock lock; + return (!canSensorFlag) ? CurrentData{} : canPayloadCurrent; +} + +CurrentData Sensors::getMotorCurrent() +{ + PauseKernelLock lock; + return (!canSensorFlag) ? CurrentData{} : canMotorCurrent; +} + +BatteryVoltageSensorData Sensors::getMotorBatteryVoltage() +{ + PauseKernelLock lock; + return (!canSensorFlag) ? BatteryVoltageSensorData{} : canMotorVoltage; +} + +void Sensors::setCCPressure(PressureData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canCCPressure = data; + canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} + +void Sensors::setBottomTankPressure(PressureData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canBottomTankPressure = data; + canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} + +void Sensors::setTopTankPressure(PressureData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canTopTankPressure = data; + canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} + +void Sensors::setTankTemperature(TemperatureData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canTankTemperature = data; + canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp(); +} + +void Sensors::setMotorCurrent(CurrentData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canMotorCurrent = data; + canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp(); +} + +void Sensors::setMainCurrent(CurrentData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canMainCurrent = data; + canMainCurrent.currentTimestamp = TimestampTimer::getTimestamp(); +} + +void Sensors::setPayloadCurrent(CurrentData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canPayloadCurrent = data; + canPayloadCurrent.currentTimestamp = TimestampTimer::getTimestamp(); +} + +void Sensors::setMotorVoltage(BatteryVoltageSensorData data) +{ + PauseKernelLock lock; + canSensorFlag = true; + canMotorVoltage = data; + canMotorVoltage.voltageTimestamp = TimestampTimer::getTimestamp(); +} + +Sensors::Sensors(TaskScheduler* sched) : scheduler{sched} {} + +bool Sensors::start() +{ + // Init all the sensors + adc1Init(); + adc2Init(); + loadCell1Init(); + loadCell2Init(); + thermocoupleInit(); + + // Create the sensors manager with the desired scheduler and the sensors map + manager = new SensorManager(sensorMap, scheduler); + return manager->start(); +} + +void Sensors::stop() { manager->stop(); } + +bool Sensors::isStarted() { return manager->areAllSensorsInitialized(); } + +void Sensors::calibrate() +{ + Stats sample1, sample2; + + // Take some samples of the 2 load cells + for (int i = 0; i < 10; i++) + { + // DO NOT USE THE GETTERS BECAUSE THE OFFSET IS ALREADY APPLIED + { + PauseKernelLock lock; + sample1.add(loadCell1->getLastSample().load); + sample2.add(loadCell2->getLastSample().load); + } + + // Wait for some reasonable time + Thread::sleep(Config::Sensors::LOAD_CELL_SAMPLE_PERIOD * 3); + } + + offsetLoadCell1 = sample1.getStats().mean; + offsetLoadCell2 = sample2.getStats().mean; +} + +void Sensors::adc1Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + SPIBusConfig config; + config.mode = SPI::Mode::MODE_0; + config.clockDivider = SPI::ClockDivider::DIV_32; + + ADS131M04::Config sensorConfig; + sensorConfig.oversamplingRatio = ADS131M04Defs::OversamplingRatio::OSR_8192; + sensorConfig.globalChopModeEnabled = true; + + adc1 = new ADS131M04(modules.get<Buses>()->spi1, + sensors::ADS131_1::cs::getPin(), config, sensorConfig); + + // Config and enter the sensors info to feed to the sensors manager + SensorInfo info("ADS131_1", Config::Sensors::ADC_SAMPLE_PERIOD, + bind(&Sensors::adc1Callback, this)); + sensorMap.emplace(make_pair(adc1, info)); + LOG_INFO(logger, "Initialized ADC1"); +} +void Sensors::adc1Callback() +{ + ADS131M04Data sample = adc1->getLastSample(); + + ADCsData data{sample.timestamp, 1, + sample.voltage[0], sample.voltage[1], + sample.voltage[2], sample.voltage[3]}; + + SDlogger.log(data); +} + +void Sensors::adc2Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + SPIBusConfig config; + config.mode = SPI::Mode::MODE_0; + config.clockDivider = SPI::ClockDivider::DIV_32; + + ADS131M04::Config sensorConfig; + sensorConfig.oversamplingRatio = ADS131M04Defs::OversamplingRatio::OSR_8192; + sensorConfig.globalChopModeEnabled = true; + + adc2 = new ADS131M04(modules.get<Buses>()->spi1, + sensors::ADS131_2::cs::getPin(), config, sensorConfig); + + // Config and enter the sensors info to feed to the sensors manager + SensorInfo info("ADS131_2", Config::Sensors::ADC_SAMPLE_PERIOD, + bind(&Sensors::adc2Callback, this)); + sensorMap.emplace(make_pair(adc2, info)); + LOG_INFO(logger, "Initialized ADC2"); +} +void Sensors::adc2Callback() +{ + ADS131M04Data sample = adc2->getLastSample(); + + ADCsData data{sample.timestamp, 2, + sample.voltage[0], sample.voltage[1], + sample.voltage[2], sample.voltage[3]}; + + SDlogger.log(data); +} + +void Sensors::loadCell1Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + loadCell1 = + new HX711(modules.get<Buses>()->spi2, sensors::HX711_1::sck::getPin()); + + loadCell1->setOffset(Config::Sensors::LOAD_CELL1_OFFSET); + loadCell1->setScale(Config::Sensors::LOAD_CELL1_SCALE); + + // Config and enter the sensors info to feed to the sensors manager + SensorInfo info("HX711_1", Config::Sensors::LOAD_CELL_SAMPLE_PERIOD, + bind(&Sensors::loadCell1Callback, this)); + sensorMap.emplace(make_pair(loadCell1, info)); + LOG_INFO(logger, "Initialized loadCell1"); +} +void Sensors::loadCell1Callback() +{ + HX711Data sample = loadCell1->getLastSample(); + + LoadCellsData data; + + data.cellNumber = 1; + data.load = sample.load; + data.loadTimestamp = sample.loadTimestamp; + + SDlogger.log(data); +} + +void Sensors::loadCell2Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + loadCell2 = + new HX711(modules.get<Buses>()->spi6, sensors::HX711_2::sck::getPin()); + + loadCell2->setOffset(Config::Sensors::LOAD_CELL2_OFFSET); + loadCell2->setScale(Config::Sensors::LOAD_CELL2_SCALE); + + // Config and enter the sensors info to feed to the sensors manager + SensorInfo info("HX711_2", Config::Sensors::LOAD_CELL_SAMPLE_PERIOD, + bind(&Sensors::loadCell2Callback, this)); + sensorMap.emplace(make_pair(loadCell2, info)); + LOG_INFO(logger, "Initialized loadCell2"); +} +void Sensors::loadCell2Callback() +{ + HX711Data sample = loadCell2->getLastSample(); + + LoadCellsData data; + + data.cellNumber = 2; + data.load = sample.load; + data.loadTimestamp = sample.loadTimestamp; + + SDlogger.log(data); +} + +void Sensors::thermocoupleInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + thermocouple = new MAX31855(modules.get<Buses>()->spi1, + sensors::MAX31855::cs::getPin()); + + // Config and enter the sensors info to feed to the sensors manager + SensorInfo info("MAX31855", Config::Sensors::THERMOCOUPLE_SAMPLE_PERIOD, + bind(&Sensors::thermocoupleCallback, this)); + sensorMap.emplace(make_pair(thermocouple, info)); + LOG_INFO(logger, "Initialized thermocouple"); +} +void Sensors::thermocoupleCallback() +{ + SDlogger.log(thermocouple->getLastSample()); +} + +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/Sensors/Sensors.h b/src/boards/RIG/Sensors/Sensors.h new file mode 100644 index 0000000000000000000000000000000000000000..de7b1742b4c9e8c61d96cc39c113de91376707b5 --- /dev/null +++ b/src/boards/RIG/Sensors/Sensors.h @@ -0,0 +1,151 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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/ADS131M04/ADS131M04.h> +#include <sensors/HX711/HX711.h> +#include <sensors/MAX31855/MAX31855.h> +#include <sensors/SensorManager.h> +#include <sensors/analog/BatteryVoltageSensorData.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +class Sensors : public Boardcore::Module +{ +public: + /** + * @brief Construct a new Sensors object + * + * @param sched The scheduler to be used from the board scheduler + * @note The scheduler determines the priority + */ + Sensors(Boardcore::TaskScheduler* sched); + + [[nodiscard]] bool start(); + + /** + * @brief Stops the sensors manager + * @warning Stops the passed scheduler + */ + void stop(); + + /** + * @brief Returns if the module is correctly started + */ + bool isStarted(); + + /** + * @brief Calibrates the load cells with an offset + */ + void calibrate(); + + // Getters methods, they pause the kernel + Boardcore::ADS131M04Data getADC1LastSample(); + Boardcore::ADS131M04Data getADC2LastSample(); + Boardcore::HX711Data getTankWeight(); + Boardcore::HX711Data getVesselWeight(); + Boardcore::TemperatureData getThermocoupleLastSample(); + + // Processed getters + Boardcore::PressureData getFillingPress(); + Boardcore::PressureData getTankTopPress(); + Boardcore::PressureData getTankBottomPress(); + Boardcore::PressureData getVesselPress(); + Boardcore::BatteryVoltageSensorData getBatteryVoltage(); + Boardcore::CurrentData getCurrent(); + Boardcore::PressureData getCCPress(); + + // CAN sensors setters + void setCCPressure(Boardcore::PressureData data); + void setBottomTankPressure(Boardcore::PressureData data); + void setTopTankPressure(Boardcore::PressureData data); + void setTankTemperature(Boardcore::TemperatureData data); + void setMotorCurrent(Boardcore::CurrentData data); + void setMainCurrent(Boardcore::CurrentData data); + void setPayloadCurrent(Boardcore::CurrentData data); + void setMotorVoltage(Boardcore::BatteryVoltageSensorData data); + + // Can sensors getters + Boardcore::CurrentData getMotorCurrent(); + Boardcore::CurrentData getMainCurrent(); + Boardcore::CurrentData getPayloadCurrent(); + Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage(); + +private: + // Init and callback methods + void adc1Init(); + void adc1Callback(); + + void adc2Init(); + void adc2Callback(); + + void loadCell1Init(); + void loadCell1Callback(); + + void loadCell2Init(); + void loadCell2Callback(); + + void thermocoupleInit(); + void thermocoupleCallback(); + + // SD logger + Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance(); + + // Sensors + Boardcore::ADS131M04* adc1 = nullptr; + Boardcore::ADS131M04* adc2 = nullptr; + + Boardcore::HX711* loadCell1 = nullptr; + Boardcore::HX711* loadCell2 = nullptr; + + Boardcore::MAX31855* thermocouple = nullptr; + + // Manager components + Boardcore::SensorManager* manager = nullptr; + Boardcore::SensorManager::SensorMap_t sensorMap; + Boardcore::TaskScheduler* scheduler = nullptr; + + // CAN Sensors flag. By default the RIG outputs the sensors from the on + // board ADC, but as soon as a CAN sensor arrives, the sensors are switched + // from the CAN + bool canSensorFlag = false; + + // CAN fake sensors data + Boardcore::PressureData canCCPressure; + Boardcore::PressureData canBottomTankPressure; + Boardcore::PressureData canTopTankPressure; + Boardcore::TemperatureData canTankTemperature; + Boardcore::CurrentData canMotorCurrent; + Boardcore::CurrentData canMainCurrent; + Boardcore::CurrentData canPayloadCurrent; + Boardcore::BatteryVoltageSensorData canMotorVoltage; + + // Offsets (thread safe) + std::atomic<float> offsetLoadCell1{0.0}; + std::atomic<float> offsetLoadCell2{0.0}; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors"); +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp new file mode 100644 index 0000000000000000000000000000000000000000..36e9140c3c8477ac11520e91d8d79c7ea382ab7b --- /dev/null +++ b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp @@ -0,0 +1,199 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <RIG/Actuators/Actuators.h> +#include <RIG/CanHandler/CanHandler.h> +#include <RIG/Configs/GroundModeManagerConfig.h> +#include <RIG/Sensors/Sensors.h> +#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> + +using namespace Boardcore; +using namespace Common; + +namespace RIG +{ +GroundModeManager::GroundModeManager() + : FSM(&GroundModeManager::state_idle), + ignitionTime(Config::Ignition::DEFAULT_IGNITION_WAITING_TIME) +{ + EventBroker::getInstance().subscribe(this, TOPIC_MOTOR); +} + +void GroundModeManager::state_idle(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(GroundModeManagerState::IDLE); + } + case FMM_INIT_OK: + { + return transition(&GroundModeManager::state_ready); + } + } +} + +void GroundModeManager::state_ready(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + // Send an DISARM CAN event + modules.get<CanHandler>()->sendEvent( + Common::CanConfig::EventId::DISARM); + + // Disarm the relays + miosix::relays::ignition::high(); + miosix::relays::nitrogen::high(); + miosix::relays::ledLamp::high(); + + return logStatus(GroundModeManagerState::READY); + } + case TMTC_ARM: + { + return transition(&GroundModeManager::state_armed); + } + case TMTC_CALIBRATE: + { + // Send CAN event + modules.get<CanHandler>()->sendEvent( + Common::CanConfig::EventId::CALIBRATE); + + // Blocking (but not too complex) operation of calibration + modules.get<Sensors>()->calibrate(); + break; + } + } +} + +void GroundModeManager::state_armed(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + // Send an ARM CAN event + modules.get<CanHandler>()->sendEvent( + Common::CanConfig::EventId::ARM); + + // Turn on the light + miosix::relays::ledLamp::low(); + + // Expire all servos timings + modules.get<Actuators>()->closeAllServo(); + return logStatus(GroundModeManagerState::ARMED); + } + case TMTC_DISARM: + { + // Turn off the light + miosix::relays::ledLamp::high(); + + return transition(&GroundModeManager::state_ready); + } + case MOTOR_IGNITION: + { + return transition(&GroundModeManager::state_igniting); + } + } +} + +void GroundModeManager::state_igniting(const Event& event) +{ + // A static variable is not really necessary + static int oxidantTimeoutEventId = -1; + + ModuleManager& modules = ModuleManager::getInstance(); + + switch (event) + { + case EV_ENTRY: + { + // Fire the igniter + miosix::relays::ignition::low(); + + // Set the delayed event for oxidant + oxidantTimeoutEventId = EventBroker::getInstance().postDelayed( + MOTOR_OPEN_OXIDANT, TOPIC_MOTOR, ignitionTime); + + return logStatus(GroundModeManagerState::IGNITING); + } + case MOTOR_OPEN_OXIDANT: + { + // Shut down the igniter + miosix::relays::ignition::high(); + + modules.get<Actuators>()->toggleServo(ServosList::MAIN_VALVE); + break; + } + case MOTOR_CLOSE_FEED_VALVE: + case TMTC_DISARM: + { + // Shut down the igniter + miosix::relays::ignition::high(); + + // Close all the valves + modules.get<Actuators>()->closeAllServo(); + + EventBroker::getInstance().removeDelayed(oxidantTimeoutEventId); + + for (uint32_t i = 0; i < Config::Ignition::NUMBER_NITROGEN_BUMPS; + i++) + { + Thread::sleep(Config::Ignition::DEAD_TIME_AFTER_BURN); + // Open nitrogen + miosix::relays::nitrogen::low(); + Thread::sleep(Config::Ignition::NITROGEN_OPENING_TIME); + // Close nitrogen + miosix::relays::nitrogen::high(); + } + + return transition(&GroundModeManager::state_ready); + } + } +} + +void GroundModeManager::setIgnitionTime(uint32_t ignitionTime) +{ + this->ignitionTime = ignitionTime; +} + +void GroundModeManager::logStatus(GroundModeManagerState state) +{ + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + + Logger::getInstance().log(status); +} + +GroundModeManagerStatus GroundModeManager::getStatus() +{ + miosix::PauseKernelLock lock; + return status; +} +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.h b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.h new file mode 100644 index 0000000000000000000000000000000000000000..a5620f6b0df2604f68b20420123b6f2e4aa196e1 --- /dev/null +++ b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.h @@ -0,0 +1,70 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <RIG/StateMachines/GroundModeManager/GroundModeManagerData.h> +#include <events/EventBroker.h> +#include <events/FSM.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +class GroundModeManager : public Boardcore::Module, + public Boardcore::FSM<GroundModeManager> +{ +public: + GroundModeManager(); + + // FSM states + void state_idle(const Boardcore::Event& event); + void state_ready(const Boardcore::Event& event); + void state_armed(const Boardcore::Event& event); + void state_igniting(const Boardcore::Event& event); + + /** + * @brief Sets the ignition time + * + * @param ignitionTime Time in [ms] + */ + void setIgnitionTime(uint32_t ignitionTime); + + // Status getter + GroundModeManagerStatus getStatus(); + +private: + /** + * @brief Logs the current FSM status on SD + */ + void logStatus(GroundModeManagerState state); + + // User Radio set ignition time + uint32_t ignitionTime; + + // Current status + GroundModeManagerStatus status; + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("GroundModeManager"); +}; + +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManagerData.h b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManagerData.h new file mode 100644 index 0000000000000000000000000000000000000000..b7178a808fd103f7967688dcb25794d6400a437b --- /dev/null +++ b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManagerData.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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> + +#include <iostream> +#include <string> + +namespace RIG +{ + +enum class GroundModeManagerState : uint8_t +{ + UNINIT = 0, + IDLE, + READY, + ARMED, + IGNITING +}; + +struct GroundModeManagerStatus +{ + long long timestamp = 0; + GroundModeManagerState state = GroundModeManagerState::UNINIT; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +} // namespace RIG diff --git a/src/boards/RIG/StateMachines/TARS1/MedianFilter.h b/src/boards/RIG/StateMachines/TARS1/MedianFilter.h new file mode 100644 index 0000000000000000000000000000000000000000..b7cbd259cc404f026618340f072e8085b990a67d --- /dev/null +++ b/src/boards/RIG/StateMachines/TARS1/MedianFilter.h @@ -0,0 +1,202 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <iostream> + +namespace RIG +{ +/** + * @brief Median filter for embedded adapted to avoid recursion. + * The object calculates a static 1D median filter, which depends on the + * template parameters. + * + * @tparam T type of object to sort and median filter. + * @tparam S number of samples needed for a static analysis. + */ +template <typename T, int S> +class MedianFilter +{ +public: + MedianFilter() : m_idx(0), ready(false), m_med(0) + { + // Initialize the arrays + for (int i = 0; i < S; i++) + { + m_buf[i] = 0; + m_tmp[i] = 0; + } + } + + /** + * @brief Adds the sample S to the window and computes the + * median if enough samples have been gathered. + * + * @param s The sample to add to the median filter + */ + void add(T s) + { + m_buf[m_idx] = s; + m_idx = (m_idx + 1) % S; + + if (m_idx == 0) + { + calcMedian(); + ready = true; + } + else + { + ready = false; + } + } + + /** + * @brief returns the median computed when the last sample was + * added. Does not return anything meaningful if not enough samples + * have been gathered; check isReady() first. + * + * @return T the median element + */ + T getMedian() { return m_med; } + + /** + * @brief returns the state of the computed median. + * If the number of elements is not a multiple of the size, + * then ready is false and the median does not have sense with + * respect to the last inserted values. + * + * @note depending on the history of insertions, even if it is not ready, + * the median is consistent. In case of not previously inserted values it is + * 0 and in case of previously inserted values, the median is the previous + * one. + * + * @return true + * @return false + */ + bool isReady() { return ready; } + + /** + * @brief Resets the internal state of the object. + */ + void reset() + { + m_idx = 0; + m_med = 0; + ready = false; + + // Reset the arrays + for (int i = 0; i < S; i++) + { + m_buf[i] = 0; + m_tmp[i] = 0; + } + } + +private: + int m_idx; + bool ready; + T m_buf[S], m_tmp[S], m_med; + + /** + * @brief helper to calculate the median. Copies + * the buffer into the temp area, then calls Hoare's in-place + * selection algorithm to obtain the median. + */ + void calcMedian() + { + for (int i = 0; i < S; i++) + { + m_tmp[i] = m_buf[i]; + } + m_med = select(0, S - 1, S / 2); + } + + /** + * @brief partition function, like from quicksort. + * l and r are the left and right bounds of the array (m_tmp), + * respectively, and p is the pivot index. + * + * @param l left bound of m_tmp array + * @param r right bound of m_tmp array + * @param p pivot index + */ + int partition(int l, int r, int p) + { + T tmp; + T pv = m_tmp[p]; + m_tmp[p] = m_tmp[r]; + m_tmp[r] = pv; + int s = l; + for (int i = l; i < r; i++) + { + if (m_tmp[i] < pv) + { + tmp = m_tmp[s]; + m_tmp[s] = m_tmp[i]; + m_tmp[i] = tmp; + s++; + } + } + tmp = m_tmp[s]; + m_tmp[s] = m_tmp[r]; + m_tmp[r] = tmp; + return s; + } + + /** + * @brief Hoare's quickselect. l and r are the + * array bounds, and k conveys that we want to return + * the k-th value + * + * @param l left bound of m_tmp array + * @param r right bound of m_tmp array + * @param k wanted position of return value + */ + T select(int l, int r, int k) + { + int p = 0; + do + { + if (l == r) + { + return m_tmp[l]; + } + + p = (l + r) / 2; + p = partition(l, r, p); + + // The case in which k == p is evaluated outside the loop + if (k < p) + { + r = p - 1; + } + else if (k > p) + { + l = p + 1; + } + + } while (p != k); + + return m_tmp[k]; + } +}; +} // namespace RIG diff --git a/src/boards/RIG/StateMachines/TARS1/TARS1.cpp b/src/boards/RIG/StateMachines/TARS1/TARS1.cpp new file mode 100644 index 0000000000000000000000000000000000000000..83cad4fd56eb57ce5b2ef4835be3c15fd15ba1e7 --- /dev/null +++ b/src/boards/RIG/StateMachines/TARS1/TARS1.cpp @@ -0,0 +1,321 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Actuators/Actuators.h> +#include <RIG/Sensors/Sensors.h> +#include <RIG/StateMachines/TARS1/TARS1.h> +#include <drivers/timer/TimestampTimer.h> + +using namespace Boardcore; +using namespace miosix; +using namespace Common; + +namespace RIG +{ +TARS1::TARS1(TaskScheduler* sched) : FSM(&TARS1::state_idle), scheduler(sched) +{ + EventBroker::getInstance().subscribe(this, TOPIC_TARS); + EventBroker::getInstance().subscribe(this, TOPIC_MOTOR); +} + +bool TARS1::start() +{ + // Add task to the task scheduler to sample and compute the average of the + // sensors + uint8_t result = scheduler->addTask([=]() { sample(); }, + Config::TARS::TARS_UPDATE_PERIOD, + TaskScheduler::Policy::RECOVER); + + return ActiveObject::start() && result != 0; +} + +void TARS1::state_idle(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(TARS1State::IDLE); + } + case FMM_INIT_OK: + { + return transition(&TARS1::state_ready); + } + } +} + +void TARS1::state_ready(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + // Reset all the stats and samples + { + Lock<FastMutex> lock(mutex); + samples.mass = 0; + samples.pressure = 0; + + massAtVenting = 0; + previousMassAtVenting = 0; + previousPressure = 0; + + massStableCounter = 0; + sampleCounter = 0; + + filteredData.mass.reset(); + filteredData.pressure.reset(); + } + return logStatus(TARS1State::READY); + } + case MOTOR_START_TARS: + { + // By default close all valves + modules.get<Actuators>()->closeAllServo(); + return transition(&TARS1::state_washing); + } + } +} + +void TARS1::state_washing(const Event& event) +{ + // A static variable is not really necessary + static int washingTimeout = -1; + + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + // Open the feed and venting valves for washing time + modules.get<Actuators>()->openServoAtomic( + ServosList::VENTING_VALVE, + Config::TARS::TARS_WASHING_OPENING_TIME + + Config::TARS::TARS_WASHING_TIME_DELAY); + + // Sleep to avoid actuators turn at the same time + Thread::sleep(2 * Config::TARS::TARS_WASHING_TIME_DELAY); + + modules.get<Actuators>()->openServoAtomic( + ServosList::FILLING_VALVE, + Config::TARS::TARS_WASHING_OPENING_TIME); + + // After double the washing time it translates to the refueling + washingTimeout = EventBroker::getInstance().postDelayed( + TARS_WASHING_DONE, TOPIC_TARS, + Config::TARS::TARS_WASHING_OPENING_TIME * 2); + + return logStatus(TARS1State::WASHING); + } + case TARS_WASHING_DONE: + { + // Open the filling valve for a long time + modules.get<Actuators>()->openServoAtomic( + ServosList::FILLING_VALVE, + Config::TARS::TARS_FILLING_OPENING_TIME); + + // Wait for PRESSURE STABILIZE WAIT TIME + Thread::sleep(Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME); + + return transition(&TARS1::state_refueling); + } + case MOTOR_MANUAL_ACTION: + { + // Delete the post delayed event + EventBroker::getInstance().removeDelayed(washingTimeout); + return transition(&TARS1::state_ready); + } + case TMTC_ARM: + case MOTOR_STOP_TARS: + case MOTOR_IGNITION: + case MOTOR_START_TARS: + { + // Close all the valves + modules.get<Actuators>()->closeAllServo(); + // Delete the post delayed event + EventBroker::getInstance().removeDelayed(washingTimeout); + // If another type of event is thrown the FSM returns to ready state + return transition(&TARS1::state_ready); + } + } +} + +void TARS1::state_refueling(const Event& event) +{ + // A static variable is not really necessary + static int pressureStabilize = -1; + + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case TARS_PRESSURE_STABILIZED: + case EV_ENTRY: + { + // Check if the algorithm can fill more + if (isMaximumMass()) + { + if (massStableCounter >= + Config::TARS::TARS_NUMBER_MASS_STABLE_ITERATIONS) + { + massStableCounter = 0; + return EventBroker::getInstance().post(TARS_FILLING_DONE, + TOPIC_TARS); + } + else + { + massStableCounter++; + } + } + else + { + massStableCounter = 0; + } + + // Open the venting and wait for the pressure to stabilize + modules.get<Actuators>()->toggleServo(ServosList::VENTING_VALVE); + pressureStabilize = EventBroker::getInstance().postDelayed( + TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS, + modules.get<Actuators>()->getTiming(ServosList::VENTING_VALVE) + + Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME * 5); + + return logStatus(TARS1State::REFUELING); + } + case TARS_CHECK_PRESSURE_STABILIZE: + { + if (!isPressureStable()) + { + // Iterate until the pressure is stable + pressureStabilize = EventBroker::getInstance().postDelayed( + TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS, + Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME); + + // Update the previous pressure sampling + { + Lock<FastMutex> lock(mutex); + previousPressure = samples.pressure; + } + + return; + } + + Thread::sleep(Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME); + + // Update mass at venting + { + Lock<FastMutex> lock(mutex); + previousMassAtVenting = massAtVenting; + massAtVenting = samples.mass; + } + + return EventBroker::getInstance().post(TARS_PRESSURE_STABILIZED, + TOPIC_TARS); + } + case TARS_FILLING_DONE: + { + EventBroker::getInstance().removeDelayed(pressureStabilize); + // Close all the valves and stop the algorithm + modules.get<Actuators>()->closeAllServo(); + + return transition(&TARS1::state_ready); + } + case MOTOR_MANUAL_ACTION: + { + EventBroker::getInstance().removeDelayed(pressureStabilize); + return transition(&TARS1::state_ready); + } + case TMTC_ARM: + case MOTOR_STOP_TARS: + case MOTOR_IGNITION: + case MOTOR_START_TARS: + { + EventBroker::getInstance().removeDelayed(pressureStabilize); + // Close all the valves + modules.get<Actuators>()->closeAllServo(); + // If another type of event is thrown the FSM returns to ready state + return transition(&TARS1::state_ready); + } + } +} + +void TARS1::logStatus(TARS1State state) +{ + // Set the current state to be logged by the sample thread + status.state = state; +} + +TARS1Status TARS1::getStatus() +{ + PauseKernelLock lock; + return status; +} + +void TARS1::sample() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Sample the sensors and update the averages + { + Lock<FastMutex> lock(mutex); + + float pressure = modules.get<Sensors>()->getTankBottomPress().pressure; + float mass = modules.get<Sensors>()->getTankWeight().load; + + filteredData.pressure.add(pressure); + filteredData.mass.add(mass); + + sampleCounter++; + + // When the number of samples reaches the template buffer number + if (filteredData.pressure.isReady()) + { + samples.pressure = filteredData.pressure.getMedian(); + samples.mass = filteredData.mass.getMedian(); + sampleCounter = 0; + + // Log the TARS state + status.timestamp = TimestampTimer::getTimestamp(); + status.pressure = samples.pressure; + status.mass = samples.mass; + + Logger::getInstance().log(status); + } + } +} + +bool TARS1::isMaximumMass() +{ + { + Lock<FastMutex> lock(mutex); + return (abs(massAtVenting - previousMassAtVenting) < + Config::TARS::TARS_MASS_TOLERANCE); + } +} + +bool TARS1::isPressureStable() +{ + { + Lock<FastMutex> lock(mutex); + return (abs(samples.pressure - previousPressure) < + Config::TARS::TARS_PRESSURE_TOLERANCE); + } +} +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/StateMachines/TARS1/TARS1.h b/src/boards/RIG/StateMachines/TARS1/TARS1.h new file mode 100644 index 0000000000000000000000000000000000000000..3fa5d1d89887c817117bf1897ea496461f6f896a --- /dev/null +++ b/src/boards/RIG/StateMachines/TARS1/TARS1.h @@ -0,0 +1,119 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Configs/TARSConfig.h> +#include <RIG/StateMachines/TARS1/MedianFilter.h> +#include <RIG/StateMachines/TARS1/TARS1Data.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <events/EventBroker.h> +#include <events/FSM.h> +#include <scheduler/TaskScheduler.h> +#include <utils/Stats/Stats.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +class TARS1 : public Boardcore::Module, public Boardcore::FSM<TARS1> +{ +public: + TARS1(Boardcore::TaskScheduler* sched); + + /** + * @brief Overrides Active Object's method to add also the task to the task + * scheduler to sample the sensors and filter them + */ + [[nodiscard]] bool start() override; + + // FSM states + void state_idle(const Boardcore::Event& event); + void state_ready(const Boardcore::Event& event); + void state_washing(const Boardcore::Event& event); + void state_refueling(const Boardcore::Event& event); + + // Status getter + TARS1Status getStatus(); + +private: + /** + * @brief Logs the current FSM status on SD + */ + void logStatus(TARS1State state); + + /** + * @brief Samples the sensors and computes an average + */ + void sample(); + + /** + * @brief Evaluates if the algorithm reached its maximum capabilities in + * filling the tank + */ + bool isMaximumMass(); + + /** + * @brief Evaluates if the pressure inside the tank is at equilibrium + */ + bool isPressureStable(); + + // Data structure output of sampling + struct + { + float pressure = 0; + float mass = 0; + } samples; + + // Data structure for filtering purposes + struct + { + MedianFilter<float, Config::TARS::TARS_FILTER_SAMPLE_NUMBER> pressure; + MedianFilter<float, Config::TARS::TARS_FILTER_SAMPLE_NUMBER> mass; + } filteredData; + + // Registered mass after venting + float massAtVenting = 0; + float previousMassAtVenting = 0; + + // Registered pressure before checking + float previousPressure = 0; + + // Number of times the mass was stable + uint16_t massStableCounter = 0; + + // Counts the number of samples done for the average measure + uint16_t sampleCounter = 0; + + // Current status + TARS1Status status; + + // Dedicated scheduler for sampling sensors periodically + Boardcore::TaskScheduler* scheduler = nullptr; + + // Mutex to synchronize sampling and reading + miosix::FastMutex mutex; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("TARS1"); +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/StateMachines/TARS1/TARS1Data.h b/src/boards/RIG/StateMachines/TARS1/TARS1Data.h new file mode 100644 index 0000000000000000000000000000000000000000..3ce3da6852614ddc417383dbd7bb4df9a25112d6 --- /dev/null +++ b/src/boards/RIG/StateMachines/TARS1/TARS1Data.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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> + +#include <iostream> +#include <string> + +namespace RIG +{ +enum class TARS1State : uint8_t +{ + UNINIT = 0, + IDLE, + READY, + WASHING, + REFUELING, +}; + +struct TARS1Status +{ + long long timestamp = 0; + float pressure = 0; + float mass = 0; + TARS1State state = TARS1State::UNINIT; + + static std::string header() { return "timestamp,state,pressure,mass\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "," << pressure << "," << mass + << "\n"; + } +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/StatesMonitor/StatesMonitor.cpp b/src/boards/RIG/StatesMonitor/StatesMonitor.cpp new file mode 100644 index 0000000000000000000000000000000000000000..65144b1ac369098e0c8d75758fb016874c791173 --- /dev/null +++ b/src/boards/RIG/StatesMonitor/StatesMonitor.cpp @@ -0,0 +1,89 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/StatesMonitor/StatesMonitor.h> + +using namespace Boardcore; +using namespace miosix; + +namespace RIG +{ +StatesMonitor::StatesMonitor(TaskScheduler* sched) : scheduler(sched) +{ + // Init the arrays + for (int i = 0; i < Config::StatesMonitor::BOARDS_NUMBER; i++) + { + updateTimestamps[i] = 0; + boardStatuses[i] = 0; + } +} + +bool StatesMonitor::start() +{ + // Add the task to the scheduler + uint8_t result = scheduler->addTask([=]() { this->update(); }, + Config::StatesMonitor::UPDATE_PERIOD); + return result != 0; +} + +void StatesMonitor::update() +{ + Lock<FastMutex> lock(mutex); + + for (int i = 0; i < Config::StatesMonitor::BOARDS_NUMBER; i++) + { + // Check if the time since the last config expires + if (getTick() > + updateTimestamps[i] + Config::StatesMonitor::MAX_TIMEOUT) + { + boardStatuses[i] = 0; + } + } +} + +void StatesMonitor::setBoardStatus(Common::CanConfig::Board board, + uint8_t status) +{ + Lock<FastMutex> lock(mutex); + uint8_t index = static_cast<uint8_t>(board); + + // Check the validity of the board index + if (index < Config::StatesMonitor::BOARDS_NUMBER) + { + boardStatuses[index] = status; + updateTimestamps[index] = getTick(); + } +} + +uint8_t StatesMonitor::getStatus(Common::CanConfig::Board board) +{ + Lock<FastMutex> lock(mutex); + uint8_t index = static_cast<uint8_t>(board); + + if (index < Config::StatesMonitor::BOARDS_NUMBER) + { + return boardStatuses[index]; + } + return 0; +} + +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/StatesMonitor/StatesMonitor.h b/src/boards/RIG/StatesMonitor/StatesMonitor.h new file mode 100644 index 0000000000000000000000000000000000000000..7f4d62f68f4104226dd6e0ab9d3229b76ffb4962 --- /dev/null +++ b/src/boards/RIG/StatesMonitor/StatesMonitor.h @@ -0,0 +1,76 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Configs/StatesMonitorConfig.h> +#include <common/CanConfig.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +/** + * @brief The class monitors and updates the states of every board. + * The CAN module when receives a state update, sets the current state into this + * module. Every N seconds, the module checks if the states have been updated, + * and if not resets them. + */ +class StatesMonitor : public Boardcore::Module +{ +public: + StatesMonitor(Boardcore::TaskScheduler* sched); + + /** + * @brief Starts the update function + */ + bool start(); + + /** + * @brief Update function which checks periodically the last time in which + * the single states where updated. Eventually if the last update is greater + * than N seconds ago the state is set to 0. + */ + void update(); + + /** + * @brief Sets the passed Board to a specific status + */ + void setBoardStatus(Common::CanConfig::Board board, uint8_t status); + + /** + * @brief Get the status of a specific Board + */ + uint8_t getStatus(Common::CanConfig::Board board); + +private: + // Board info + long long int updateTimestamps[Config::StatesMonitor::BOARDS_NUMBER]; + uint8_t boardStatuses[Config::StatesMonitor::BOARDS_NUMBER]; + + // Sync mutex + miosix::FastMutex mutex; + + Boardcore::TaskScheduler* scheduler = nullptr; +}; +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/TMRepository/TMRepository.cpp b/src/boards/RIG/TMRepository/TMRepository.cpp new file mode 100644 index 0000000000000000000000000000000000000000..58ecc23fbfb29539e09f46acdf00ed34d8cd1e71 --- /dev/null +++ b/src/boards/RIG/TMRepository/TMRepository.cpp @@ -0,0 +1,382 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Actuators/Actuators.h> +#include <RIG/BoardScheduler.h> +#include <RIG/Radio/Radio.h> +#include <RIG/Sensors/Sensors.h> +#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h> +#include <RIG/StateMachines/TARS1/TARS1.h> +#include <RIG/StatesMonitor/StatesMonitor.h> +#include <RIG/TMRepository/TMRepository.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> + +using namespace Boardcore; +using namespace miosix; +using namespace Common; + +namespace RIG +{ +mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId, + uint8_t seq) +{ + mavlink_message_t msg; + ModuleManager& modules = ModuleManager::getInstance(); + + // Prevents preemption, THIS FUNCTION MUST NOT yeld OR USE KERNEL. THE ONLY + // ALLOWED OPERATION ARE COPIES OF DATA STRUCTURES. + PauseKernelLock lock; + + switch (tmId) + { + case SystemTMList::MAV_SYS_ID: + { + mavlink_sys_tm_t tm; + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.logger = Logger::getInstance().isStarted(); + tm.event_broker = EventBroker::getInstance().isRunning(); + tm.radio = modules.get<Radio>()->isStarted(); + tm.sensors = modules.get<Sensors>()->isStarted(); + tm.board_scheduler = modules.get<BoardScheduler>()->isStarted(); + + mavlink_msg_sys_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + &tm); + break; + } + case SystemTMList::MAV_LOGGER_ID: + { + mavlink_logger_tm_t tm; + + LoggerStats stats = Logger::getInstance().getStats(); + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.log_number = stats.logNumber; + tm.too_large_samples = stats.tooLargeSamples; + tm.dropped_samples = stats.droppedSamples; + tm.queued_samples = stats.queuedSamples; + tm.buffers_filled = stats.buffersFilled; + tm.buffers_written = stats.buffersWritten; + tm.writes_failed = stats.writesFailed; + tm.last_write_error = stats.lastWriteError; + tm.average_write_time = stats.averageWriteTime; + tm.max_write_time = stats.maxWriteTime; + + mavlink_msg_logger_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + &tm); + + break; + } + case SystemTMList::MAV_MAVLINK_STATS: + { + mavlink_mavlink_stats_tm_t tm; + + MavlinkStatus stats = modules.get<Radio>()->mavDriver->getStatus(); + + tm.timestamp = stats.timestamp; + tm.n_send_queue = stats.nSendQueue; + tm.max_send_queue = stats.maxSendQueue; + tm.n_send_errors = stats.nSendErrors; + tm.msg_received = stats.mavStats.msg_received; + tm.buffer_overrun = stats.mavStats.buffer_overrun; + tm.parse_error = stats.mavStats.parse_error; + tm.parse_state = stats.mavStats.parse_state; + tm.packet_idx = stats.mavStats.packet_idx; + tm.current_rx_seq = stats.mavStats.current_rx_seq; + tm.current_tx_seq = stats.mavStats.current_tx_seq; + tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count; + tm.packet_rx_drop_count = stats.mavStats.packet_rx_drop_count; + + mavlink_msg_mavlink_stats_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, + &msg, &tm); + break; + } + case SystemTMList::MAV_GSE_ID: + { + mavlink_gse_tm_t tm; + + HX711Data tank = modules.get<Sensors>()->getTankWeight(); + HX711Data vessel = modules.get<Sensors>()->getVesselWeight(); + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.loadcell_rocket = tank.load; + tm.loadcell_vessel = vessel.load; + tm.filling_pressure = + modules.get<Sensors>()->getFillingPress().pressure; + tm.vessel_pressure = + modules.get<Sensors>()->getVesselPress().pressure; + // TODO change the threshold (cannot be 0 due to angle tolerance) + tm.filling_valve_state = modules.get<Actuators>()->getServoPosition( + ServosList::FILLING_VALVE) > 0.3 + ? 1 + : 0; + tm.venting_valve_state = modules.get<Actuators>()->getServoPosition( + ServosList::VENTING_VALVE) > 0.3 + ? 1 + : 0; + tm.release_valve_state = modules.get<Actuators>()->getServoPosition( + ServosList::RELEASE_VALVE) > 0.3 + ? 1 + : 0; + + tm.main_valve_state = modules.get<Actuators>()->getServoPosition( + ServosList::MAIN_VALVE) > 0.3 + ? 1 + : 0; + + tm.arming_state = + modules.get<GroundModeManager>()->getStatus().state == + GroundModeManagerState::ARMED + ? 1 + : 0; + tm.ignition_state = + modules.get<GroundModeManager>()->getStatus().state == + GroundModeManagerState::IGNITING + ? 1 + : 0; + tm.tars_state = + static_cast<uint8_t>(modules.get<TARS1>()->getStatus().state); + tm.battery_voltage = + modules.get<Sensors>()->getBatteryVoltage().voltage; + tm.current_consumption = + modules.get<Sensors>()->getCurrent().current; + + // Board statuses + tm.main_board_status = + modules.get<StatesMonitor>()->getStatus(CanConfig::Board::MAIN); + tm.payload_board_status = modules.get<StatesMonitor>()->getStatus( + CanConfig::Board::PAYLOAD); + tm.motor_board_status = modules.get<StatesMonitor>()->getStatus( + CanConfig::Board::MOTOR); + + mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + &tm); + break; + } + case SystemTMList::MAV_MOTOR_ID: + { + mavlink_motor_tm_t tm; + + TemperatureData temp = + modules.get<Sensors>()->getThermocoupleLastSample(); + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.top_tank_pressure = + modules.get<Sensors>()->getTankTopPress().pressure; + tm.bottom_tank_pressure = + modules.get<Sensors>()->getTankBottomPress().pressure; + tm.combustion_chamber_pressure = + modules.get<Sensors>()->getCCPress().pressure; + tm.tank_temperature = temp.temperature; + tm.floating_level = 0; + + tm.battery_voltage = + modules.get<Sensors>()->getMotorBatteryVoltage().batVoltage; + tm.current_consumption = + modules.get<Sensors>()->getMotorCurrent().current; + + tm.main_valve_state = modules.get<Actuators>()->getServoPosition( + ServosList::MAIN_VALVE) > 0.3 + ? 1 + : 0; + + mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + &tm); + + break; + } + default: + { + // No system ID recognized, send a nack + mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + msgId, seq); + break; + } + } + return msg; +} + +mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId, + uint8_t msgId, uint8_t seq) +{ + mavlink_message_t msg; + ModuleManager& modules = ModuleManager::getInstance(); + + // NO NEED FOR KERNEL LOCK BECAUSE IT IS ALREADY PRESENT INSIDE THE GETTERS + switch (sensorId) + { + case SensorsTMList::MAV_FILLING_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData sample = modules.get<Sensors>()->getFillingPress(); + + tm.timestamp = sample.pressureTimestamp; + tm.pressure = sample.pressure; + + mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, + &msg, &tm); + + break; + } + case SensorsTMList::MAV_TANK_TOP_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData sample = modules.get<Sensors>()->getTankTopPress(); + + tm.timestamp = sample.pressureTimestamp; + tm.pressure = sample.pressure; + + mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, + &msg, &tm); + + break; + } + case SensorsTMList::MAV_TANK_BOTTOM_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData sample = modules.get<Sensors>()->getTankBottomPress(); + + tm.timestamp = sample.pressureTimestamp; + tm.pressure = sample.pressure; + + mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, + &msg, &tm); + + break; + } + case SensorsTMList::MAV_VESSEL_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData sample = modules.get<Sensors>()->getVesselPress(); + + tm.timestamp = sample.pressureTimestamp; + tm.pressure = sample.pressure; + + mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, + &msg, &tm); + + break; + } + case SensorsTMList::MAV_TANK_TEMP_ID: + { + mavlink_temp_tm_t tm; + + TemperatureData temp = + modules.get<Sensors>()->getThermocoupleLastSample(); + + tm.timestamp = temp.temperatureTimestamp; + tm.temperature = temp.temperature; + + mavlink_msg_temp_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + &tm); + + break; + } + case SensorsTMList::MAV_LOAD_CELL_VESSEL_ID: + { + mavlink_load_tm_t tm; + + HX711Data load = modules.get<Sensors>()->getVesselWeight(); + + tm.timestamp = load.loadTimestamp; + tm.load = load.load; + + mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + &tm); + break; + } + case SensorsTMList::MAV_LOAD_CELL_TANK_ID: + { + mavlink_load_tm_t tm; + + HX711Data load = modules.get<Sensors>()->getTankWeight(); + + tm.timestamp = load.loadTimestamp; + tm.load = load.load; + + mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + &tm); + break; + } + default: + { + // No sensor ID recognized, send a nack + mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, + msgId, seq); + break; + } + } + return msg; +} + +mavlink_message_t TMRepository::packServoTm(ServosList servoId, uint8_t msgId, + uint8_t seq) +{ + mavlink_message_t msg; + ModuleManager& modules = ModuleManager::getInstance(); + + // Prevents preemption, THIS FUNCTION MUST NOT yeld OR USE KERNEL. THE ONLY + // ALLOWED OPERATION ARE COPIES OF DATA STRUCTURES. + PauseKernelLock lock; + + if (servoId != MAIN_VALVE && servoId != VENTING_VALVE && + servoId != RELEASE_VALVE && servoId != FILLING_VALVE && + servoId != DISCONNECT_SERVO) + { + // No servo ID recognized, send a nack + mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, msgId, + seq); + } + else + { + mavlink_servo_tm_t tm; + + tm.servo_id = servoId; + tm.servo_position = modules.get<Actuators>()->getServoPosition(servoId); + + mavlink_msg_servo_tm_encode(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &msg, &tm); + } + + return msg; +} +} // namespace RIG \ No newline at end of file diff --git a/src/boards/RIG/TMRepository/TMRepository.h b/src/boards/RIG/TMRepository/TMRepository.h new file mode 100644 index 0000000000000000000000000000000000000000..c943398d4b5a884fd8a5e22f147ee711d8ae3fcb --- /dev/null +++ b/src/boards/RIG/TMRepository/TMRepository.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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> +#include <diagnostic/PrintLogger.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIG +{ +class TMRepository : public Boardcore::Module +{ +public: + inline TMRepository() {} + + // Packs the telemetry at system level (E.g. logger telemetry..) + mavlink_message_t packSystemTm(SystemTMList tmId, uint8_t msgId, + uint8_t seq); + + // Packs the telemetry at sensors level (E.g. pressure sensors..) + mavlink_message_t packSensorsTm(SensorsTMList sensorId, uint8_t msgId, + uint8_t seq); + + // Packs the telemetry about servo positions states + mavlink_message_t packServoTm(ServosList servoId, uint8_t msgId, + uint8_t seq); + +private: + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("TMRepository"); +}; +} // namespace RIG \ No newline at end of file diff --git a/src/entrypoints/RIG/rig-entry.cpp b/src/entrypoints/RIG/rig-entry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..00eb41ad707d7ae823b609671ed5f9571dfcac88 --- /dev/null +++ b/src/entrypoints/RIG/rig-entry.cpp @@ -0,0 +1,233 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Matteo Pignataro + * + * 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 <RIG/Actuators/Actuators.h> +#include <RIG/BoardScheduler.h> +#include <RIG/Buses.h> +#include <RIG/CanHandler/CanHandler.h> +#include <RIG/Radio/Radio.h> +#include <RIG/Sensors/Sensors.h> +#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h> +#include <RIG/StateMachines/TARS1/TARS1.h> +#include <RIG/StatesMonitor/StatesMonitor.h> +#include <RIG/TMRepository/TMRepository.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <diagnostic/PrintLogger.h> +#include <events/EventBroker.h> +#include <events/EventData.h> +#include <events/utils/EventSniffer.h> +#include <logger/Logger.h> +#include <miosix.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +using namespace Boardcore; +using namespace RIG; +using namespace miosix; +using namespace Common; + +int main() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Overall status. If at some point it becomes false, there is a problem + // somewhere + bool initResult = true; + PrintLogger logger = Logging::getLogger("main"); + + // Create modules + Buses* buses = new Buses(); + BoardScheduler* scheduler = new BoardScheduler(); + Actuators* actuators = new Actuators(scheduler->getScheduler(4)); + Sensors* sensors = new Sensors(scheduler->getScheduler(3)); + Radio* radio = new Radio(); + TMRepository* tmRepo = new TMRepository(); + GroundModeManager* groundModeManager = new GroundModeManager(); + TARS1* tars = new TARS1(scheduler->getScheduler(4)); + CanHandler* can = + new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); + StatesMonitor* states = + new StatesMonitor(scheduler->getScheduler(miosix::MAIN_PRIORITY)); + + // Initialize singleton modules + Logger& SDlogger = Logger::getInstance(); + EventBroker& broker = EventBroker::getInstance(); + + // Insert modules + if (!modules.insert<Buses>(buses)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the buses module"); + } + + if (!modules.insert<BoardScheduler>(scheduler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the scheduler module"); + } + + if (!modules.insert<Sensors>(sensors)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the sensors module"); + } + + if (!modules.insert<Actuators>(actuators)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the actuators module"); + } + + if (!modules.insert<Radio>(radio)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the radio module"); + } + + if (!modules.insert<TMRepository>(tmRepo)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the tm repository module"); + } + + if (!modules.insert<GroundModeManager>(groundModeManager)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the ground mode manager FSM module"); + } + + if (!modules.insert<TARS1>(tars)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the tars module"); + } + + if (!modules.insert<CanHandler>(can)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the CAN module"); + } + + if (!modules.insert<StatesMonitor>(states)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the States Monitor module"); + } + + // Start singleton modules + if (!SDlogger.testSDCard()) + { + initResult = false; + LOG_ERR(logger, "Error starting the SD logger"); + } + + if (!broker.start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Event Broker"); + } + + // Start modules + if (!modules.get<Actuators>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the actuators module"); + } + + if (!modules.get<BoardScheduler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the board scheduler"); + } + + if (!modules.get<Sensors>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the sensors module"); + } + + if (!modules.get<StatesMonitor>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the States Monitor module"); + } + + if (!modules.get<Radio>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the radio module"); + } + + if (!modules.get<GroundModeManager>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the ground mode manager module"); + } + + if (!modules.get<TARS1>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the tars module"); + } + + if (!modules.get<CanHandler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the CAN module"); + } + + if (initResult) + { + // POST OK + ui::redLed::low(); + EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_MOTOR); + LOG_INFO(logger, "Init success!"); + } + else + { + // POST ERROR + ui::redLed::high(); + EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_MOTOR); + } + + // Log all events + EventSniffer sniffer( + EventBroker::getInstance(), TOPICS_LIST, + [](uint8_t event, uint8_t topic) + { + EventData ev{TimestampTimer::getTimestamp(), event, topic}; + Logger::getInstance().log(ev); + }); + + // Periodic statistics + while (true) + { + Thread::sleep(1000); + Logger::getInstance().log(CpuMeter::getCpuStats()); + CpuMeter::resetCpuStats(); + StackLogger::getInstance().log(); + } + + return 0; +} \ No newline at end of file diff --git a/src/scripts/EventDumper/EventDumper.cpp b/src/scripts/EventDumper/EventDumper.cpp index 4fb6d768353e40f6f25ed01cf497920e8b3713b5..7f8e02f015244138af0e4acb4eeedeb9ed52282f 100644 --- a/src/scripts/EventDumper/EventDumper.cpp +++ b/src/scripts/EventDumper/EventDumper.cpp @@ -19,7 +19,6 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ - #include <common/Events.h> #include <iostream> diff --git a/src/scripts/logdecoder/RIG/Makefile b/src/scripts/logdecoder/RIG/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..6bbaf34fc2008754ca1425004f33514659c56baf --- /dev/null +++ b/src/scripts/logdecoder/RIG/Makefile @@ -0,0 +1,16 @@ +BOARDCORE := ../../../../skyward-boardcore/ +OBSW := ../../../../src/boards/ + +all: + g++ -std=c++17 -O2 -o logdecoder logdecoder.cpp \ + -DCOMPILE_FOR_X86 \ + -DCOMPILE_FOR_HOST \ + $(BOARDCORE)libs/tscpp/tscpp/stream.cpp \ + -I$(BOARDCORE)libs/miosix-host \ + -I$(BOARDCORE)libs/mavlink-skyward-lib \ + -I$(BOARDCORE)libs/eigen \ + -I$(BOARDCORE)libs/tscpp \ + -I$(BOARDCORE)src/shared \ + -I$(OBSW) +clean: + rm logdecoder diff --git a/src/scripts/logdecoder/RIG/logdecoder.cpp b/src/scripts/logdecoder/RIG/logdecoder.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8d9c602b10f0dd896cbf9f861995e02c68e669b8 --- /dev/null +++ b/src/scripts/logdecoder/RIG/logdecoder.cpp @@ -0,0 +1,134 @@ +/* Copyright (c) 2018-2022 Skyward Experimental Rocketry + * Author: Terrane Federico + * + * 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 <RIG/Actuators/ActuatorsData.h> +#include <RIG/CanHandler/CanHandlerData.h> +#include <RIG/Sensors/ADCsData.h> +#include <RIG/Sensors/LoadCellsData.h> +#include <RIG/StateMachines/GroundModeManager/GroundModeManagerData.h> +#include <RIG/StateMachines/TARS1/TARS1Data.h> +#include <logger/Deserializer.h> +#include <logger/LogTypes.h> +#include <tscpp/stream.h> + +#include <fstream> +#include <iostream> +#include <stdexcept> +#include <string> + +/** + * @brief Binary log files decoder. + * + * This program is to compile for you computer and decodes binary log files + * through the tscpp library. + * + * In LogTypes.h there should be included all the classes you want to + * deserialize. + */ + +using namespace tscpp; +using namespace Boardcore; +using namespace RIG; + +void registerTypes(Deserializer& ds) +{ + // Register all Boardcore types + LogTypes::registerTypes(ds); + + // Custom types + ds.registerType<GroundModeManagerStatus>(); + ds.registerType<LoadCellsData>(); + ds.registerType<ADCsData>(); + ds.registerType<ActuatorsData>(); + ds.registerType<TARS1Status>(); + ds.registerType<CanCurrentSensor>(); + ds.registerType<CanPressureSensor>(); + ds.registerType<CanTemperatureSensor>(); + ds.registerType<CanVoltageSensor>(); +} + +void showUsage(const string& cmdName) +{ + std::cerr << "Usage: " << cmdName << " {-a | <log_file_name> | -h}" + << "Options:\n" + << "\t-h,--help\t\tShow help message\n" + << "\t-a,--all Deserialize all logs in the current directory\n" + << std::endl; +} + +bool deserialize(string logName) +{ + std::cout << "Deserializing " << logName << "...\n"; + Deserializer d(logName); + LogTypes::registerTypes(d); + registerTypes(d); + + return d.deserialize(); +} + +bool deserializeAll() +{ + for (int i = 0; i < 100; i++) + { + char nextName[11]; + sprintf(nextName, "log%02d.dat", i); + struct stat st; + if (stat(nextName, &st) != 0) + continue; // File not found + // File found + if (!deserialize(string(nextName))) + return false; + } + return true; +} + +int main(int argc, char* argv[]) +{ + if (argc < 2) + { + showUsage(string(argv[0])); + return 1; // Error + } + + bool success = false; + string arg1 = string(argv[1]); + + // Help message + if (arg1 == "-h" || arg1 == "--help") + { + showUsage(string(argv[0])); + return 0; + } + + // File deserialization + if (arg1 == "-a" || arg1 == "--all") + success = deserializeAll(); + else + success = deserialize(arg1); + + // End + if (success) + std::cout << "Deserialization completed successfully\n"; + else + std::cout << "Deserialization ended with errors\n"; + return 0; +}