diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 8e174a4bb077962864dcdfd7238fabbbcc9b5e37..6b8c74e518d77f5c632aee70a51ee4070a179da5 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -17,7 +17,6 @@ "${workspaceFolder}/skyward-boardcore/src/shared", "${workspaceFolder}/skyward-boardcore/src/tests", "${workspaceFolder}/src/boards", - "${workspaceFolder}/src/tests", "${workspaceFolder}/src" ] }, diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 2874d30e76943630ecde23ae28e94aed5c3d895b..3cb66c2073de672883a6ace4b4bbfca3a426fd2e 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -90,7 +90,6 @@ set(PAYLOAD_COMPUTER src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp src/boards/Payload/StateMachines/WingController/WingController.cpp src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp - src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp src/boards/Payload/Wing/FileWingAlgorithm.cpp diff --git a/src/boards/Payload/Configs/VerticalVelocityConfig.h b/src/boards/Payload/Configs/VerticalVelocityConfig.h deleted file mode 100644 index 28e36e0701311f5c037e7501747c1bbff4fe3b79..0000000000000000000000000000000000000000 --- a/src/boards/Payload/Configs/VerticalVelocityConfig.h +++ /dev/null @@ -1,37 +0,0 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Raul Radu - * - * 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 - -namespace Payload -{ - -namespace FailSafe -{ - -constexpr int FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD = 10; // [ms] -constexpr float FAILSAFE_VERTICAL_VELOCITY_THRESHOLD = 12; // [m/s] -constexpr int FAILSAFE_VERTICAL_VELOCITY_TRIGGER_CONFIDENCE = 30; - -} // namespace FailSafe - -} // namespace Payload diff --git a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp deleted file mode 100644 index 7261da78c6e7221186be063b3ccfff4ea2bce8e4..0000000000000000000000000000000000000000 --- a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Authors: Raul Radu - * - * 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 <Payload/BoardScheduler.h> -#include <Payload/Configs/VerticalVelocityConfig.h> -#include <Payload/StateMachines/NASController/NASController.h> -#include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h> -#include <common/Events.h> -#include <events/EventBroker.h> - -using namespace Boardcore; -using namespace Common; - -namespace Payload -{ - -VerticalVelocityTrigger::VerticalVelocityTrigger() - : running(false), confidence(0) -{ -} - -bool VerticalVelocityTrigger::start() -{ - auto& scheduler = getModule<BoardScheduler>()->verticalVelocityTrigger(); - - return scheduler.addTask( - [this] { update(); }, - FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD); -} - -void VerticalVelocityTrigger::enable() -{ - confidence = 0; - running = true; -} - -void VerticalVelocityTrigger::disable() { running = false; } - -bool VerticalVelocityTrigger::isActive() { return running; } - -void VerticalVelocityTrigger::update() -{ - if (running) - { - float verticalVelocity = -getModule<NASController>()->getNasState().vd; - if (verticalVelocity < FailSafe::FAILSAFE_VERTICAL_VELOCITY_THRESHOLD) - { - confidence++; - } - else - { - confidence = 0; - } - if (confidence >= - FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_CONFIDENCE) - { - confidence = 0; - EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED, - TOPIC_FLIGHT); - running = false; - } - } -} -} // namespace Payload diff --git a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h deleted file mode 100644 index 25e2bff748f94990657a61e0f8c26dbfe66e9d34..0000000000000000000000000000000000000000 --- a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h +++ /dev/null @@ -1,90 +0,0 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Raul Radu - * - * 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 <utils/DependencyManager/DependencyManager.h> - -#include <atomic> - -namespace Payload -{ -class BoardScheduler; -class NASController; - -/** - * This class is used by the FailSafe logic to react to - * the premature opening of the parafoil by the payload - * Required modules: - * - Payload::BoardScheduler - * - Payload::NASController - */ -class VerticalVelocityTrigger - : public Boardcore::InjectableWithDeps<BoardScheduler, NASController> -{ -public: - /** - * Default constructor for VerticalVelocityTrigger - * Sets the trigger to disabled by default - */ - VerticalVelocityTrigger(); - - /** - * Starts the module by inserting it in the BoardScheduler - */ - bool start(); - - /** - * Enables the Vertical Velocity Trigger - */ - void enable(); - - /** - * Disables the Vertical Velocity Trigger - */ - void disable(); - - /** - * Checks if the trigger is enabled - * @return true if the trigger is enabled - * @return false if the trigger is not enabled - */ - bool isActive(); - -private: - /** - * This method is called every - * FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD - * and reads the state from the NAS and when the velocity is below - * the threshold for some confidence level it posts the event - * FLIGHT_FAILSAFE_TRIGGERED in topic TOPIC_FLIGHT - */ - void update(); - - std::atomic<bool> running; - - // Number of times that the algorithm detects to be slower than the velocity - // threshold - std::atomic<int> confidence; -}; - -} // namespace Payload diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp index 247a6217b387e2d5b11cd6b354e707695b447d80..166db6715526926a585b3ac6cd03d4002bca875d 100644 --- a/src/entrypoints/Payload/payload-entry.cpp +++ b/src/entrypoints/Payload/payload-entry.cpp @@ -34,7 +34,6 @@ #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> #include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h> #include <Payload/WindEstimationScheme/WindEstimation.h> #include <common/Events.h> #include <common/Topics.h> @@ -132,10 +131,9 @@ int main() auto canHandler = new CanHandler(); // Flight algorithms - auto altitudeTrigger = new AltitudeTrigger(); - auto wingController = new WingController(); - auto verticalVelocityTrigger = new VerticalVelocityTrigger(); - auto windEstimation = new WindEstimation(); + auto altitudeTrigger = new AltitudeTrigger(); + auto wingController = new WingController(); + auto windEstimation = new WindEstimation(); // Actuators auto actuators = new Actuators(); @@ -151,7 +149,6 @@ int main() depman.insert(radio) && depman.insert(canHandler) && depman.insert(altitudeTrigger) && depman.insert(wingController) && - depman.insert(verticalVelocityTrigger) && depman.insert(windEstimation) && depman.insert(actuators) && depman.insert(statsRecorder); @@ -183,7 +180,6 @@ int main() START_MODULE(nas); START_MODULE(altitudeTrigger); START_MODULE(wingController); - START_MODULE(verticalVelocityTrigger); START_MODULE(windEstimation); START_MODULE(actuators); START_MODULE(statsRecorder); diff --git a/src/tests/Payload/payload-test-VerticalVelocityTrigger.cpp b/src/tests/Payload/payload-test-VerticalVelocityTrigger.cpp deleted file mode 100644 index 3dc7ac5df34b09bf744f135f145db216e2183153..0000000000000000000000000000000000000000 --- a/src/tests/Payload/payload-test-VerticalVelocityTrigger.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Radu Raul - * - * 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 <Payload/BoardScheduler.h> -#include <Payload/Configs/FailSafeConfig.h> -#include <Payload/NASControllerMock/NASControllerMock.h> -#include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h> -#include <events/EventBroker.h> -#include <miosix.h> - -#include <utils/ModuleManager/ModuleManager.hpp> - -using namespace Boardcore; -using namespace Payload; - -// Initial velocity for the Vertical Velocity Trigger -constexpr float MOCK_INITIAL_VERTICAL_VELOCITY = -30; -constexpr float MOCK_VELOCITY_DECELERATION = 0.5; // m/s^2 - -/** - * This class mocks the NASController class since for this test we cannot start - * the NASController as it needs the sensor reading and we cannot test the - * velocity trigger unless we launch the rocket - */ -class NASMock : public NASController -{ -public: - /** - * Default constructor for NASMock - * It initializes the mockedVerticalSpeed - */ - NASMock() : mockedVerticalSpeed(MOCK_INITIAL_VERTICAL_VELOCITY) {} - - /** - * Default destructor for NASMock - */ - ~NASMock() {} - - /** - * This method mocks the NASController::getNasState() and returns a - * NASState with a personalized vertical velocity that starts at - * NASMock::MOCK_INITIAL_VERTICAL_VELOCITY and decreases by 0.5 m/s^2 - * - * @return NASState object that contains the vertical vector in NED - * orientation - */ - NASState getNasState() override - { - mockedVerticalSpeed += - MOCK_VELOCITY_DECELERATION * - ((float)FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD / - 1000.f); - NASState n = NASState(); - n.vd = mockedVerticalSpeed; - return n; - } - -private: - // Keeps trace of the vertical velocity in NED orientation - float mockedVerticalSpeed; -}; - -/** - * This function halts the board - */ -void halt() -{ - while (1) - { - } -} - -/** - * This function calculates the maximum wait time needed by the Vertical - * Velocity Trigger to trigger - */ -float calculateMaxWaitTime() -{ - float confidenceTime = - ((float)FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_CONFIDENCE * - (float)FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD) / - 1000.f; - float targetReachTime = (-MOCK_INITIAL_VERTICAL_VELOCITY - - FailSafe::FAILSAFE_VERTICAL_VELOCITY_THRESHOLD) / - MOCK_VELOCITY_DECELERATION; - TRACE("Target reach time: %f [s]\n", targetReachTime); - TRACE("Confidence time: %f [s]\n", confidenceTime); - - return confidenceTime + targetReachTime + 1; -} - -int main() -{ - - // Insert the modules - ModuleManager::getInstance().insert<BoardScheduler>(new BoardScheduler()); - ModuleManager::getInstance().insert<NASController>(new NASMock()); - ModuleManager::getInstance().insert<VerticalVelocityTrigger>( - new VerticalVelocityTrigger()); - - // start the scheduler - TRACE("Starting the board scheduler\n"); - if (!ModuleManager::getInstance().get<BoardScheduler>()->start()) - { - TRACE("Error starting the General Purpose Scheduler\n"); - halt(); - } - - // start the modules - if (!ModuleManager::getInstance().get<NASController>()->start()) - { - TRACE("Error starting the NAS Controller\n"); - halt(); - } - if (!ModuleManager::getInstance() - .get<VerticalVelocityTrigger>() - ->startModule()) - { - TRACE("Error starting the Vertical Velocity Trigger\n"); - halt(); - } - - ModuleManager::getInstance().get<VerticalVelocityTrigger>()->enable(); - - TRACE("Starting... \n"); - // wait for the trigger - int count = 0; - int maxWaitTime = calculateMaxWaitTime() * 100; - while ( - ModuleManager::getInstance().get<VerticalVelocityTrigger>()->isActive()) - { - Thread::sleep(100); - count++; - - if (count > maxWaitTime) - { - TRACE("Vertical Velocity Trigger not working properly\n"); - break; - } - } - if (count < maxWaitTime) - { - TRACE("Vertical Velocity Triggered \n"); - } - - halt(); -}