diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3bdae31b7848fdeca7f80db8b2cc704f9d842653..24bc7db738a3e58d3eb2b651d9b00cf9dc55724f 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -575,7 +575,7 @@ "HSE_VALUE=8000000", "SYSCLK_FREQ_168MHz=168000000", "_MIOSIX", - "__cplusplus=201103L", + "__cplusplus=201402L", "HILSimulation", "ROCCARASO" ], diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a161e91483ff7f6bc0ddfb9e9a81e8b75f6ac63..e25d87503b136e3c61413f26b678f48e6be839f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,4 +36,18 @@ project(OnBoardSoftware) add_executable(test-hil src/entrypoints/HIL/test-hil.cpp ${HIL}) target_include_directories(test-hil PRIVATE ${OBSW_INCLUDE_DIRS}) target_compile_definitions(test-hil PRIVATE HILTest) -sbs_target(test-hil stm32f767zi_compute_unit) \ No newline at end of file +sbs_target(test-hil stm32f767zi_compute_unit) + +add_executable(main-entry src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER}) +target_include_directories(main-entry PRIVATE ${OBSW_INCLUDE_DIRS}) +sbs_target(main-entry stm32f767zi_skyward_death_stack_v4) + +add_executable(main-entry-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER}) +target_include_directories(main-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS}) +target_compile_definitions(main-entry-roccaraso PRIVATE ROCCARASO) +sbs_target(main-entry-roccaraso stm32f767zi_skyward_death_stack_v4) + +add_executable(main-entry-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER}) +target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS}) +target_compile_definitions(main-entry-euroc PRIVATE EUROC) +sbs_target(main-entry-euroc stm32f767zi_skyward_death_stack_v4) diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 48463b4b699bdcad44db349d6061eb1b607859a7..91c76d1bbb9a664e0b10d376c2a6447333fc80b5 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -29,3 +29,22 @@ set(HIL src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp src/hardware_in_the_loop/HIL/HILTransceiver.cpp ) + +set(MAIN_COMPUTER + src/boards/Main/BoardScheduler.cpp + src/boards/Main/Sensors/Sensors.cpp + src/boards/Main/StateMachines/NASController/NASController.cpp + src/boards/Main/Radio/Radio.cpp + src/boards/Main/TMRepository/TMRepository.cpp + src/boards/Main/CanHandler/CanHandler.cpp + src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp + src/boards/Main/Actuators/Actuators.cpp + src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp + src/boards/Main/StateMachines/ADAController/ADAController.cpp + src/boards/Main/PinHandler/PinHandler.cpp + src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp + src/boards/Main/StateMachines/ABKController/ABKController.cpp + src/boards/Main/StateMachines/MEAController/MEAController.cpp + src/boards/Main/StateMachines/Deployment/Deployment.cpp + src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp +) diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main/Actuators/Actuators.cpp new file mode 100644 index 0000000000000000000000000000000000000000..788d526d31865d1f1e65d2a4193504beed6f7f71 --- /dev/null +++ b/src/boards/Main/Actuators/Actuators.cpp @@ -0,0 +1,207 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/CanHandler/CanHandler.h> +#include <Main/Configs/ActuatorsConfig.h> +#include <drivers/timer/TimestampTimer.h> +#include <interfaces-impl/hwmapping.h> + +using namespace Boardcore; +using namespace miosix; +using namespace Main::ActuatorsConfig; + +namespace Main +{ +Actuators::Actuators(TaskScheduler* sched) : scheduler(sched) +{ + // Create the servo instances + servoAbk = new Servo(SERVO_ABK_TIMER, SERVO_ABK_CHANNEL, ABK_MIN_PULSE, + ABK_MAX_PULSE); + servoExp = new Servo(SERVO_EXP_TIMER, SERVO_EXP_CHANNEL, EXP_MIN_PULSE, + EXP_MAX_PULSE); + + // Set the buzzer + buzzer = new PWM(BUZZER_TIMER, BUZZER_FREQUENCY); + buzzer->setDutyCycle(BUZZER_CHANNEL, BUZZER_DUTY_CYCLE); + + // Default disable + camOff(); + cutterOff(); + // gpios::status_led::low(); + + // Init by default the CAN servo positions + for (int i = 0; i < ServosList::ServosList_ENUM_END; i++) + { + CANPositions[i] = 0; + } +} + +bool Actuators::start() +{ + servoAbk->enable(); + servoExp->enable(); + + // Reset the servo position + servoAbk->setPosition(0); + servoExp->setPosition(0); + + return scheduler->addTask([&]() { updateBuzzer(); }, BUZZER_UPDATE_PERIOD, + TaskScheduler::Policy::RECOVER); +} + +void Actuators::setServoPosition(ServosList servo, float position) +{ + // Lock the mutex for thread sync + miosix::Lock<FastMutex> l(mutex); + Servo* requestedServo = getServo(servo); + + if (requestedServo != nullptr) + { + requestedServo->setPosition(position, true); + } + + // Log the position + Logger::getInstance().log(requestedServo->getState()); +} + +void Actuators::setCANServoPosition(ServosList servo, float position) +{ + // Lock the mutex for thread sync + miosix::Lock<FastMutex> l(mutex); + + CANPositions[servo] = position; +} + +void Actuators::wiggleServo(ServosList servo) +{ + Servo* requestedServo = getServo(servo); + + if (requestedServo != nullptr) + { + // Do not lock the mutex due to set position lock + setServoPosition(servo, 1); + Thread::sleep(1000); + setServoPosition(servo, 0); + } + else + { + // Run a CAN message + ModuleManager::getInstance().get<CanHandler>()->sendCanCommand(servo, 1, + 1000); + } +} + +float Actuators::getServoPosition(ServosList servo) +{ + miosix::Lock<FastMutex> l(mutex); + Servo* requestedServo = getServo(servo); + + if (requestedServo != nullptr) + { + return requestedServo->getPosition(); + } + + return CANPositions[servo]; +} + +void Actuators::camOn() +{ + miosix::Lock<FastMutex> l(mutex); + gpios::camera_enable::high(); +} + +void Actuators::camOff() +{ + miosix::Lock<FastMutex> l(mutex); + gpios::camera_enable::low(); +} + +void Actuators::cutterOn() +{ + miosix::Lock<FastMutex> l(mutex); + gpios::cut_trigger::high(); +} + +void Actuators::cutterOff() +{ + miosix::Lock<FastMutex> l(mutex); + gpios::cut_trigger::low(); +} + +Servo* Actuators::getServo(ServosList servo) +{ + switch (servo) + { + case AIR_BRAKES_SERVO: + { + return servoAbk; + } + case EXPULSION_SERVO: + { + return servoExp; + } + default: + { + return nullptr; + } + } +} + +void Actuators::setBuzzerArm() +{ + // Set the counter with respect to the update function period + buzzerCounterOverflow = BUZZER_ARM_PERIOD / BUZZER_UPDATE_PERIOD; +} + +void Actuators::setBuzzerLand() +{ + // Set the counter with respect to the update function period + buzzerCounterOverflow = BUZZER_LAND_PERIOD / BUZZER_UPDATE_PERIOD; +} + +void Actuators::setBuzzerOff() { buzzerCounterOverflow = 0; } + +void Actuators::updateBuzzer() +{ + if (buzzerCounterOverflow == 0) + { + // The buzzer is deactivated thus the channel is disabled + buzzer->disableChannel(BUZZER_CHANNEL); + } + else + { + if (buzzerCounter >= buzzerCounterOverflow) + { + // Enable the channel for this period + buzzer->enableChannel(BUZZER_CHANNEL); + buzzerCounter = 0; + } + else + { + buzzer->disableChannel(BUZZER_CHANNEL); + buzzerCounter++; + } + } +} + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Actuators/Actuators.h b/src/boards/Main/Actuators/Actuators.h new file mode 100644 index 0000000000000000000000000000000000000000..e1e3157cb961400ac6a25194d989441a08af0b3f --- /dev/null +++ b/src/boards/Main/Actuators/Actuators.h @@ -0,0 +1,120 @@ +/* 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 <actuators/Servo/Servo.h> +#include <common/Mavlink.h> +#include <diagnostic/PrintLogger.h> +#include <drivers/timer/PWM.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class Actuators : public Boardcore::Module +{ +public: + Actuators(Boardcore::TaskScheduler* sched); + + /** + * @brief Enables all the servos + */ + bool start(); + + /** + * @brief Sets the servo passed servo position in normalized value [0-1] + */ + void setServoPosition(ServosList servo, float position); + + /** + * @brief Sets the servo passed servo position inside the CAN structure + */ + void setCANServoPosition(ServosList servo, float position); + + /** + * @brief Wiggles the passed servo for 1 second + * @note The method is blocking during the second of opening + */ + void wiggleServo(ServosList servo); + + /** + * @brief Get the passed servo's position [0-1] + */ + float getServoPosition(ServosList servo); + + // Camera commands + void camOn(); + void camOff(); + + // Cutter commands + void cutterOn(); + void cutterOff(); + + // Setters for buzzer state + void setBuzzerArm(); + void setBuzzerLand(); + void setBuzzerOff(); + +private: + /** + * @brief Returns the selected servo pointer + */ + Boardcore::Servo* getServo(ServosList servo); + + /** + * @brief Automatic called method to update the buzzer status + */ + void updateBuzzer(); + + // Connected servos + Boardcore::Servo* servoAbk = nullptr; + Boardcore::Servo* servoExp = nullptr; + + // Buzzer + Boardcore::PWM* buzzer; + + // Status LED state + bool ledState = false; + + // Counter that enables and disables the buzzer + uint32_t buzzerCounter = 0; + + // Upper limit of the buzzer counter + std::atomic<uint32_t> buzzerCounterOverflow{0}; + + // Can set servo positions + float CANPositions[ServosList::ServosList_ENUM_END]; + + // Scheduler for buzzer + Boardcore::TaskScheduler* scheduler = nullptr; + + // Access to all the actuators mutex, instead of PauseKernel lock to avoid + // low priority stuff locking the whole kernel and let the component respect + // the priority + miosix::FastMutex mutex; + + // Debug logger + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Actuators"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3da65ed747a22aaef039095f86443d4e98e078a3 --- /dev/null +++ b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp @@ -0,0 +1,87 @@ +/* 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 <Main/AltitudeTrigger/AltitudeTrigger.h> +#include <Main/Configs/AltitudeTriggerConfig.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <events/EventBroker.h> + +using namespace Boardcore; +using namespace Common; + +namespace Main +{ +AltitudeTrigger::AltitudeTrigger(TaskScheduler* sched) : scheduler(sched) +{ + altitude = AltitudeTriggerConfig::ALTITUDE_REFERENCE; +} + +bool AltitudeTrigger::start() +{ + size_t result = scheduler->addTask( + [&]() { update(); }, AltitudeTriggerConfig::ALTITUDE_CHECKER_PERIOD, + TaskScheduler::Policy::RECOVER); + + return result != 0; +} + +void AltitudeTrigger::setDeploymentAltitude(float altitude) +{ + this->altitude = altitude; +} + +void AltitudeTrigger::update() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Check the FMM state + FlightModeManagerStatus status = + modules.get<FlightModeManager>()->getStatus(); + + if (status.state == FlightModeManagerState::DROGUE_DESCENT) + { + // Get the estimated ADA altitude + ADAState state = modules.get<ADAController>()->getADAState(); + + // Update the confidence + if (state.aglAltitude < altitude) + { + confidence++; + } + else + { + confidence = 0; + } + + if (confidence >= AltitudeTriggerConfig::ALTITUDE_CONFIDENCE_THRESHOLD) + { + // We reached the altitude + confidence = 0; + EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED, + TOPIC_FLIGHT); + } + } +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h new file mode 100644 index 0000000000000000000000000000000000000000..1eeea13dd25c8849921c35e9c9a1107e522620e7 --- /dev/null +++ b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h @@ -0,0 +1,73 @@ +/* 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 <diagnostic/PrintLogger.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class AltitudeTrigger : public Boardcore::Module +{ +public: + AltitudeTrigger(Boardcore::TaskScheduler* sched); + + /** + * @brief Adds to the task scheduler the periodic update task + */ + bool start(); + + /** + * @brief Set the Deployment Altitude that must be reached to open the Main + * parachute + */ + void setDeploymentAltitude(float alt); + + /** + * @brief The update function that is called every period to check the FMM + * state and the altitude reached + */ + void update(); + +private: + /** + * @brief Reference that must be reached + */ + std::atomic<float> altitude; + + /** + * @brief Counts how many times the altitude has been reached consequently. + */ + uint32_t confidence; + + /** + * @brief Scheduler to which add the periodic update task + */ + Boardcore::TaskScheduler* scheduler = nullptr; + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("AltitudeTrigger"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/BoardScheduler.cpp b/src/boards/Main/BoardScheduler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b380b32df78b25b55ba117af04f24126a79af6b7 --- /dev/null +++ b/src/boards/Main/BoardScheduler.cpp @@ -0,0 +1,65 @@ +/* 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 "BoardScheduler.h" + +using namespace Boardcore; + +namespace Main +{ +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 Main \ No newline at end of file diff --git a/src/boards/Main/BoardScheduler.h b/src/boards/Main/BoardScheduler.h new file mode 100644 index 0000000000000000000000000000000000000000..4b39c023c8a59a37ff42744587dfbc09e01f2812 --- /dev/null +++ b/src/boards/Main/BoardScheduler.h @@ -0,0 +1,62 @@ +/* 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 <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +/** + * @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 Main \ No newline at end of file diff --git a/src/boards/Main/Buses.h b/src/boards/Main/Buses.h new file mode 100644 index 0000000000000000000000000000000000000000..13552ec6be401c1d3be9ebd4456e4da4e94d3b9a --- /dev/null +++ b/src/boards/Main/Buses.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 <drivers/i2c/I2C.h> +#include <drivers/spi/SPIBus.h> +#include <drivers/usart/USART.h> +#include <interfaces-impl/hwmapping.h> + +#include <utils/ModuleManager/ModuleManager.hpp> +namespace Main +{ +class Buses : public Boardcore::Module +{ +public: + Boardcore::SPIBus spi1; + Boardcore::SPIBus spi3; + Boardcore::SPIBus spi4; + Boardcore::SPIBus spi6; + + Boardcore::I2C i2c1; + + Boardcore::USART usart1; + Boardcore::USART usart2; + Boardcore::USART uart4; + + Buses() + : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6), + i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(), + miosix::interfaces::i2c1::sda::getPin()), + usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200) + { + } +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/CanHandler/CanHandler.cpp b/src/boards/Main/CanHandler/CanHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cc172db9c0976f766d3a359a5d9f0c429629979a --- /dev/null +++ b/src/boards/Main/CanHandler/CanHandler.cpp @@ -0,0 +1,294 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/BoardScheduler.h> +#include <Main/Buses.h> +#include <Main/CanHandler/CanHandlerData.h> +#include <Main/Configs/CanHandlerConfig.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <common/CanConfig.h> +#include <common/Events.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 Main::CanHandlerConfig; + +namespace Main +{ +CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched) +{ + // Configure the CAN driver + CanbusDriver::AutoBitTiming bitTiming; + bitTiming.baudRate = BAUD_RATE; + bitTiming.samplePoint = SAMPLE_POINT; + + CanbusDriver::CanbusConfig config; + + // Configure the correct peripheral + driver = new CanbusDriver(CAN2, 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::PAYLOAD), + static_cast<uint8_t>(Board::BROADCAST)); + protocol->addFilter(static_cast<uint8_t>(Board::RIG), + static_cast<uint8_t>(Board::BROADCAST)); + protocol->addFilter(static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST)); + driver->init(); +} + +bool CanHandler::start() +{ + // 0 if fail + uint8_t result = scheduler->addTask( // status + [&]() + { + FlightModeManagerState state = ModuleManager::getInstance() + .get<FlightModeManager>() + ->getStatus() + .state; + protocol->enqueueSimplePacket( + static_cast<uint8_t>(Priority::MEDIUM), + static_cast<uint8_t>(PrimaryType::STATUS), + static_cast<uint8_t>(Board::MAIN), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(state), + ((state == FlightModeManagerState::ARMED) ? 0x01 : 0x00)); + }, + STATUS_TRANSMISSION_PERIOD); + + 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::MAIN), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(event)); +} + +void CanHandler::sendCanCommand(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::MAIN), + 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::ACTUATORS: + { + handleCanActuator(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::PITOT: + { + PitotData data = pitotDataFromCanMessage(msg); + modules.get<Sensors>()->setPitot(data); + + // Log the data + data.timestamp = TimestampTimer::getTimestamp(); + Logger::getInstance().log(data); + + break; + } + 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_BOARD_VOLTAGE: + { + BatteryVoltageSensorData data = voltageDataFromCanMessage(msg); + modules.get<Sensors>()->setMotorBatteryVoltage(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; + } + 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; + } + default: + { + LOG_WARN(logger, "Received unsupported sensor data: id={}", + sensorId); + } + } +} + +void CanHandler::handleCanActuator(const CanMessage &msg) +{ + ServosList servo = static_cast<ServosList>(msg.getSecondaryType()); + ModuleManager &modules = ModuleManager::getInstance(); + + // Set the servo position + modules.get<Actuators>()->setCANServoPosition( + servo, servoDataFromCanMessage(msg).position); +} + +} // namespace Main diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main/CanHandler/CanHandler.h new file mode 100644 index 0000000000000000000000000000000000000000..3e585f00c7c559c2b60e7807a0f325c0dcba188f --- /dev/null +++ b/src/boards/Main/CanHandler/CanHandler.h @@ -0,0 +1,85 @@ +/* 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 <Main/BoardScheduler.h> +#include <common/CanConfig.h> +#include <common/Mavlink.h> +#include <drivers/canbus/CanProtocol/CanProtocol.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ + +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 sendCanCommand(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 handleCanActuator(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 Main diff --git a/src/boards/Main/CanHandler/CanHandlerData.h b/src/boards/Main/CanHandler/CanHandlerData.h new file mode 100644 index 0000000000000000000000000000000000000000..e25d62bc71b907a6cb8f567f5ca5d6a4d8f04b6d --- /dev/null +++ b/src/boards/Main/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 Main +{ +// 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 Main \ No newline at end of file diff --git a/src/boards/Main/Configs/ABKConfig.h b/src/boards/Main/Configs/ABKConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..6e8aadec95bbfd5c3af985f68ef8dec4c564a6ac --- /dev/null +++ b/src/boards/Main/Configs/ABKConfig.h @@ -0,0 +1,60 @@ +/* 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 <algorithms/AirBrakes/AirBrakesInterp.h> + +namespace Main +{ +namespace ABKConfig +{ +// TODO remove this useless config from interpolation algorithm +static const Boardcore::AirBrakesConfig ABK_CONFIG{ + 0.4884, -1.4391, 6.6940, + -18.4272, 29.1044, -24.5585, + 8.6058, 9.0426, 159.5995, + 4.8188, -208.4471, 47.0771, + 1.9433e+03, -205.6689, -6.4634e+03, + 331.0332, 8.8763e+03, -161.8111, + -3.9917e+03, 2.8025e-06, 0.0373, + 20, -0.009216, 0.02492, + -0.01627, 0.03191, 0.017671458676443, + 0, +}; + +constexpr uint32_t UPDATE_PERIOD = 100; // [ms] -> 10Hz + +// ABK algorithm configs +constexpr float MINIMUM_ALTITUDE = 400; +constexpr float MAXIMUM_ALTITUDE = 1000; +constexpr float STARTING_FILTER_VALUE = 0.75f; +constexpr float ABK_CRITICAL_ALTITUDE = 970; +constexpr float DZ = 10; +constexpr float INITIAL_MASS = 26; +constexpr float DM = 0.1f; +constexpr uint16_t N_FORWARD = 0; + +// Shadow mode after motor shutdown to let MEA algorithm correctly estimate the +// mass +constexpr uint32_t DELAY_TIMEOUT = 500; // [ms] -> 0.5s +} // namespace ABKConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main/Configs/ADAConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..41be4c220825e2acc1ffd4fe8bcd2c80f9f5b24e --- /dev/null +++ b/src/boards/Main/Configs/ADAConfig.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: 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. + */ + +#pragma once + +#include <stdint.h> + +namespace Main +{ +namespace ADAConfig +{ +constexpr uint32_t UPDATE_PERIOD = 20; // [ms] 50Hz +constexpr float SAMPLING_PERIOD = UPDATE_PERIOD / 1000.0f; // [seconds] + +// Calibration constants +constexpr int CALIBRATION_SAMPLES_COUNT = 20; +constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms] + +// ADA shadow mode time, during which the ADA algorithm cannot trigger apogees +constexpr uint32_t SHADOW_MODE_TIMEOUT = 10000; // [ms] + +// When the vertical speed is smaller than this value, apogee is detected. +// If equal to 0 -> Exact apogee +// If greater than 0 -> Apogee detected ahead of time (while still going up) +constexpr float APOGEE_VERTICAL_SPEED_TARGET = 2.5; // [m/s] + +// Number of consecutive samples after which apogee is triggered. +constexpr unsigned int APOGEE_N_SAMPLES = 5; + +} // namespace ADAConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/ActuatorsConfig.h b/src/boards/Main/Configs/ActuatorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..18617d06cdbb17028e361c6131f87e0ea0583de6 --- /dev/null +++ b/src/boards/Main/Configs/ActuatorsConfig.h @@ -0,0 +1,60 @@ +/* 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 <drivers/timer/PWM.h> +#include <drivers/timer/TimerUtils.h> + +namespace Main +{ +namespace ActuatorsConfig +{ +static TIM_TypeDef* const SERVO_ABK_TIMER = TIM3; +static TIM_TypeDef* const SERVO_EXP_TIMER = TIM12; +static TIM_TypeDef* const BUZZER_TIMER = TIM1; + +// TODO change correspondent naming in hwmapping (channel different) +constexpr Boardcore::TimerUtils::Channel SERVO_ABK_CHANNEL = + Boardcore::TimerUtils::Channel::CHANNEL_2; +constexpr Boardcore::TimerUtils::Channel SERVO_EXP_CHANNEL = + Boardcore::TimerUtils::Channel::CHANNEL_2; +constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL = + Boardcore::TimerUtils::Channel::CHANNEL_1; + +constexpr uint16_t ABK_MIN_PULSE = 1950; +constexpr uint16_t ABK_MAX_PULSE = 1380; + +// Inverted to invert the servo logic +constexpr uint16_t EXP_MIN_PULSE = 900; +constexpr uint16_t EXP_MAX_PULSE = 2000; + +// Buzzer configs +constexpr uint32_t BUZZER_FREQUENCY = 1000; +constexpr float BUZZER_DUTY_CYCLE = 0.5; + +constexpr uint32_t BUZZER_UPDATE_PERIOD = 100; // [ms] + +constexpr uint32_t BUZZER_ARM_PERIOD = 500; // [ms] +constexpr uint32_t BUZZER_LAND_PERIOD = 1000; // [ms] + +} // namespace ActuatorsConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/AltitudeTriggerConfig.h b/src/boards/Main/Configs/AltitudeTriggerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..c3ec7f2a8d14204ee05b4aa03933e74f46c15f61 --- /dev/null +++ b/src/boards/Main/Configs/AltitudeTriggerConfig.h @@ -0,0 +1,35 @@ +/* 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 Main +{ +namespace AltitudeTriggerConfig +{ +constexpr uint32_t ALTITUDE_CHECKER_PERIOD = 100; //[ms] -> 10Hz +constexpr uint32_t ALTITUDE_REFERENCE = 350; //[m] +constexpr uint32_t ALTITUDE_CONFIDENCE_THRESHOLD = 5; +} // namespace AltitudeTriggerConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/CanHandlerConfig.h b/src/boards/Main/Configs/CanHandlerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..bc9ff47896cbd7c21348f25d156aa52e1facebb6 --- /dev/null +++ b/src/boards/Main/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 Main +{ + +namespace CanHandlerConfig +{ + +constexpr unsigned int PITOT_TRANSMISSION_PERIOD = 20; // ms +constexpr unsigned int STATUS_TRANSMISSION_PERIOD = 2000; // ms + +} // namespace CanHandlerConfig + +} // namespace Main diff --git a/src/boards/Main/Configs/DeploymentConfig.h b/src/boards/Main/Configs/DeploymentConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..83a237255382fdb1d78d1ef2c55031d68acd15dc --- /dev/null +++ b/src/boards/Main/Configs/DeploymentConfig.h @@ -0,0 +1,31 @@ +/* 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 + +namespace Main +{ +namespace DeploymentConfig +{ +constexpr unsigned int CUT_DURATION = 500; // [ms] +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/FlightModeManagerConfig.h b/src/boards/Main/Configs/FlightModeManagerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..de12450f4b2e4062565d1e3d26f815f91986519c --- /dev/null +++ b/src/boards/Main/Configs/FlightModeManagerConfig.h @@ -0,0 +1,34 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Angelo Prete + * + * 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 Main +{ +namespace FMMConfig +{ +constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000; +constexpr unsigned int ENGINE_SHUTDOWN_TIMEOUT = 4000; // [ms] +constexpr unsigned int APOGEE_EVENT_TIMEOUT = 40000; // [ms] +} // namespace FMMConfig + +} // namespace Main diff --git a/src/boards/Main/Configs/FlightStatsRecorderConfig.h b/src/boards/Main/Configs/FlightStatsRecorderConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..45a80b3ae65ba435bc0ee63848316d7726597dc1 --- /dev/null +++ b/src/boards/Main/Configs/FlightStatsRecorderConfig.h @@ -0,0 +1,33 @@ +/* Copyright (c) 2019-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 Main +{ +namespace FlightStatsRecorderConfig +{ +constexpr uint32_t FSR_UPDATE_PERIOD = 100; // [ms] +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/MEAConfig.h b/src/boards/Main/Configs/MEAConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..ed242b76f702baaa9ec8de5bd7ebde27aba41077 --- /dev/null +++ b/src/boards/Main/Configs/MEAConfig.h @@ -0,0 +1,54 @@ +/* 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 Main +{ +namespace MEAConfig +{ +constexpr uint32_t UPDATE_PERIOD = 20; // [ms] -> 50Hz + +constexpr float SENSOR_NOISE_VARIANCE = 0.36f; +constexpr float MODEL_NOISE_VARIANCE = 0.1f; +constexpr float DEFAULT_INITIAL_ROCKET_MASS = 28.9286227299384f; + +constexpr uint32_t SHADOW_MODE_TIMEOUT = 900000; // [ms] +constexpr float SHUTDOWN_THRESHOLD_ALTITUDE = 1000; // [m] +constexpr unsigned int SHUTDOWN_N_SAMPLES = 5; + +// Pressure threshold after which the kalman is updated +constexpr float CC_PRESSURE_THRESHOLD = 1.f; + +// Aerodynamics coefficients +constexpr float n000 = 0.554395329698886; +constexpr float n100 = -1.71019994711676; +constexpr float n200 = 8.05103009321528; +constexpr float n300 = -22.2129400612042; +constexpr float n400 = 34.6990670331566; +constexpr float n500 = -28.6219169089691; +constexpr float n600 = 9.73349655723146; + +} // namespace MEAConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..486c4e8d3d577f78b7824b1169d902f78cbefbc2 --- /dev/null +++ b/src/boards/Main/Configs/NASConfig.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2022 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 <algorithms/NAS/NASConfig.h> +#include <algorithms/ReferenceValues.h> +#include <common/ReferenceConfig.h> + +namespace Main +{ + +namespace NASConfig +{ + +constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz + +constexpr int CALIBRATION_SAMPLES_COUNT = 20; +constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms] + +constexpr float ACCELERATION_THRESHOLD = 0.5; // [m/s^2] + +// Threshold to re-enable the accelerometer readings +constexpr int ACCELERATION_THRESHOLD_SAMPLE = 20; + +static const Boardcore::NASConfig config = { + UPDATE_PERIOD / 1000.0, // T + 0.0001f, // SIGMA_BETA + 0.3f, // SIGMA_W + 0.1f, // SIGMA_ACC + 0.1f, // SIGMA_MAG + 10.0f, // SIGMA_GPS + 4.3f, // SIGMA_BAR + 10.0f, // SIGMA_POS + 10.0f, // SIGMA_VEL + 10.0f, // SIGMA_PITOT + 1.0f, // P_POS + 10.0f, // P_POS_VERTICAL + 1.0f, // P_VEL + 10.0f, // P_VEL_VERTICAL + 0.01f, // P_ATT + 0.01f, // P_BIAS + 6.0f, // SATS_NUM + Common::ReferenceConfig::nedMag // NED_MAG +}; + +} // namespace NASConfig + +} // namespace Main diff --git a/src/boards/Main/Configs/PinHandlerConfig.h b/src/boards/Main/Configs/PinHandlerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..d8b3f38274a7c65f2632d44dee9b940295803d3e --- /dev/null +++ b/src/boards/Main/Configs/PinHandlerConfig.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2019-2023 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio, 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 <utils/PinObserver/PinObserver.h> + +namespace Main +{ +namespace PinHandlerConfig +{ +constexpr unsigned int LAUNCH_PIN_THRESHOLD = 20; +constexpr Boardcore::PinTransition LAUNCH_PIN_TRIGGER = + Boardcore::PinTransition::RISING_EDGE; + +constexpr unsigned int NC_DETACH_PIN_THRESHOLD = 20; +constexpr Boardcore::PinTransition NC_DETACH_PIN_TRIGGER = + Boardcore::PinTransition::RISING_EDGE; + +constexpr unsigned int CUTTER_SENSE_PIN_THRESHOLD = 20; +constexpr unsigned int EXPULSION_PIN_THRESHOLD = 20; +} // namespace PinHandlerConfig +} // namespace Main diff --git a/src/boards/Main/Configs/RadioConfig.h b/src/boards/Main/Configs/RadioConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..42354f01172a5f62b245cef278681457b5e1681f --- /dev/null +++ b/src/boards/Main/Configs/RadioConfig.h @@ -0,0 +1,46 @@ +/* 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 <common/Mavlink.h> + +namespace Main +{ +namespace RadioConfig +{ +// 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; + +constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 250; + +constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50; +constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10; + +constexpr uint8_t MAV_SYSTEM_ID = 171; +constexpr uint8_t MAV_COMP_ID = 96; + +constexpr uint32_t MAVLINK_QUEUE_SIZE = 10; + +} // namespace RadioConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..e6d686fe21cebaaac4a59f9655712fd82bbd1d02 --- /dev/null +++ b/src/boards/Main/Configs/SensorsConfig.h @@ -0,0 +1,121 @@ +/* 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 <sensors/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/LPS28DFW/LPS28DFW.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/UBXGPS/UBXGPSSpi.h> + +namespace Main +{ +namespace SensorsConfig +{ +constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4; +constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100; + +constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4; +constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100; +constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR = + Boardcore::LPS28DFW::FS_1260; + +constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR = + Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400; +constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU = + Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE; +constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR = + Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100; + +constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE = + Boardcore::LIS2MDL::MD_CONTINUOUS; +constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ; +constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5; + +constexpr uint8_t UBXGPS_SAMPLE_RATE = 5; + +constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU = + Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE; + +constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS = + Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16; +constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR = + Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416; +constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE = + Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL; + +constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS = + Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000; +constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR = + Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416; + +constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE = + Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS; +constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION + LSM6DSRX_FIFO_TIMESTAMP_DECIMATION = + Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; +constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR + LSM6DSRX_FIFO_TEMPERATURE_BDR = + Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; + +constexpr Boardcore::ADS131M08Defs::OversamplingRatio + ADS131M08_OVERSAMPLING_RATIO = + Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192; +constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true; + +constexpr uint32_t LPS22DF_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t LPS28DFW_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t H3LIS331DL_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t LIS2MDL_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t UBXGPS_PERIOD = 1000 / UBXGPS_SAMPLE_RATE; // [ms] +constexpr uint32_t LSM6DSRX_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t ADS131M08_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t IMU_PERIOD = 20; // [ms] Fake processed IMU 50Hz +constexpr uint32_t MAG_CALIBRATION_PERIOD = 100; // [ms] 10Hz + +// ADC sensors configs +constexpr float ADC_VOLTAGE_RANGE = 1.2f; // [V] Voltage reading range +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_DPL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_3; +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_1 = + Boardcore::ADS131M08Defs::Channel::CHANNEL_4; +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_2 = + Boardcore::ADS131M08Defs::Channel::CHANNEL_2; + +// ADC Voltage divider +constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR = + (20.f / 2.4f) + + 1; // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider +constexpr float CURRENT_CONVERSION_FACTOR = + (20.f / 4.f) / (12.f / (12.f + 33.f)); +constexpr float CURRENT_OFFSET = 0.133333333f; // V in ADC + +// Calibration samples +constexpr unsigned int CALIBRATION_SAMPLES = 20; +constexpr unsigned int CALIBRATION_PERIOD = 100; + +constexpr unsigned int NUMBER_OF_SENSORS = 8; + +} // namespace SensorsConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp new file mode 100644 index 0000000000000000000000000000000000000000..95766cffc0a0099484772e9d4d680e57a342b560 --- /dev/null +++ b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp @@ -0,0 +1,222 @@ +/* Copyright (c) 2019-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 <Main/Configs/FlightStatsRecorderConfig.h> +#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/NASController/NASController.h> + +#include <Eigen/Core> + +using namespace Boardcore; +using namespace Main::FlightStatsRecorderConfig; +using namespace Eigen; +using namespace miosix; + +namespace Main +{ +FlightStatsRecorder::FlightStatsRecorder(TaskScheduler* sched) + : scheduler(sched) +{ + // Init the data structure to avoid UB + stats.ada_min_pressure = 0; + stats.apogee_alt = 0; + stats.apogee_lat = 0; + stats.apogee_lon = 0; + stats.apogee_ts = 0; + stats.cpu_load = 0; + stats.dpl_max_acc = 0; + stats.dpl_ts = 0; + stats.dpl_vane_max_pressure = 0; + stats.free_heap = 0; + stats.liftoff_max_acc = 0; + stats.liftoff_max_acc_ts = 0; + stats.liftoff_ts = 0; + stats.max_airspeed_pitot = 0; + stats.max_speed_altitude = 0; + stats.max_z_speed = 0; + stats.max_z_speed_ts = 0; + stats.min_pressure = 0; +} + +bool FlightStatsRecorder::start() +{ + return scheduler->addTask([&]() { update(); }, FSR_UPDATE_PERIOD, + TaskScheduler::Policy::RECOVER); +} + +void FlightStatsRecorder::update() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // FMM state + FlightModeManagerState flightState = + modules.get<FlightModeManager>()->getStatus().state; + + // Data gathering + AccelerometerData accData = modules.get<Sensors>()->getIMULastSample(); + PressureData baroData = + modules.get<Sensors>()->getStaticPressure1LastSample(); + PressureData DPLPressure = + modules.get<Sensors>()->getDeploymentPressureLastSample(); + PitotData pitotData = modules.get<Sensors>()->getPitotLastSample(); + GPSData gpsData = modules.get<Sensors>()->getGPSLastSample(); + NASState nasData = modules.get<NASController>()->getNasState(); + ADAState adaData = modules.get<ADAController>()->getADAState(); + + // Store the apogee ts + uint64_t previousApogee = stats.apogee_ts; + + // Update the data + updateAcc(flightState, accData); + updateBaro(flightState, baroData); + updateDPLPressure(flightState, DPLPressure); + updatePitot(flightState, pitotData); + updateNAS(flightState, nasData); + updateADA(flightState, adaData); + + // If the apogee ts is different update the GPS apogee coordinates + if (previousApogee != stats.apogee_ts) + { + Lock<FastMutex> l(mutex); + + stats.apogee_lat = gpsData.latitude; + stats.apogee_lon = gpsData.longitude; + } +} + +mavlink_rocket_stats_tm_t FlightStatsRecorder::getStats() +{ + Lock<FastMutex> l(mutex); + return stats; +} + +void FlightStatsRecorder::updateAcc(FlightModeManagerState flightState, + AccelerometerData data) +{ + Vector3f accelerations = static_cast<Vector3f>(data); + float norm = accelerations.norm(); + + if (flightState == FlightModeManagerState::POWERED_ASCENT || + flightState == FlightModeManagerState::UNPOWERED_ASCENT) + { + Lock<FastMutex> l(mutex); + + stats.liftoff_max_acc = std::max(stats.liftoff_max_acc, norm); + bool changed = stats.liftoff_max_acc == norm; + + // In case of a change update the timestamp + stats.liftoff_max_acc_ts = + changed ? data.accelerationTimestamp : stats.liftoff_max_acc_ts; + } + else if (flightState == FlightModeManagerState::DROGUE_DESCENT) + { + Lock<FastMutex> l(mutex); + + stats.dpl_max_acc = std::max(stats.dpl_max_acc, norm); + + // Change the timestamp of DPL if not already done + stats.dpl_ts = + stats.dpl_ts == 0 ? data.accelerationTimestamp : stats.dpl_ts; + } +} + +void FlightStatsRecorder::updateBaro(FlightModeManagerState flightState, + PressureData data) +{ + Lock<FastMutex> l(mutex); + + if (stats.min_pressure == 0) + { + stats.min_pressure = data.pressure; + } + + // Update the min + stats.min_pressure = std::min(stats.min_pressure, data.pressure); +} + +void FlightStatsRecorder::updateDPLPressure(FlightModeManagerState flightState, + PressureData data) +{ + if (flightState == FlightModeManagerState::UNPOWERED_ASCENT || + flightState == FlightModeManagerState::DROGUE_DESCENT) + { + Lock<FastMutex> l(mutex); + + stats.dpl_vane_max_pressure = + std::max(stats.dpl_vane_max_pressure, data.pressure); + } +} + +void FlightStatsRecorder::updatePitot(FlightModeManagerState flightState, + PitotData data) +{ + Lock<FastMutex> l(mutex); + + stats.max_airspeed_pitot = + std::max(stats.max_airspeed_pitot, data.airspeed); +} + +void FlightStatsRecorder::updateNAS(FlightModeManagerState flightState, + NASState state) +{ + if (flightState == FlightModeManagerState::POWERED_ASCENT || + flightState == FlightModeManagerState::UNPOWERED_ASCENT) + { + Lock<FastMutex> l(mutex); + + stats.max_z_speed = std::max(stats.max_z_speed, -state.vd); + bool changed = stats.max_z_speed == -state.vd; + + // Change the max speed altitude and timestamp if the max speed changed + stats.max_speed_altitude = + changed ? -state.d : stats.max_speed_altitude; + + stats.max_z_speed_ts = changed ? state.timestamp : stats.max_z_speed_ts; + } +} + +void FlightStatsRecorder::updateADA(FlightModeManagerState flightState, + ADAState state) +{ + if (flightState == FlightModeManagerState::POWERED_ASCENT || + flightState == FlightModeManagerState::UNPOWERED_ASCENT || + flightState == FlightModeManagerState::DROGUE_DESCENT) + { + Lock<FastMutex> l(mutex); + + if (stats.ada_min_pressure == 0) + { + stats.ada_min_pressure = state.x0; + } + + stats.ada_min_pressure = std::min(stats.ada_min_pressure, state.x0); + bool changed = stats.ada_min_pressure == state.x0; + + stats.apogee_alt = std::max(stats.apogee_alt, state.aglAltitude); + stats.apogee_ts = changed ? state.timestamp : stats.apogee_ts; + } +} + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h new file mode 100644 index 0000000000000000000000000000000000000000..59e4fe5c7edc4cadf754848abcd69579c9af145a --- /dev/null +++ b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h @@ -0,0 +1,82 @@ +/* Copyright (c) 2019-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 <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <algorithms/ADA/ADAData.h> +#include <algorithms/NAS/NASState.h> +#include <common/Mavlink.h> +#include <scheduler/TaskScheduler.h> +#include <sensors/SensorData.h> +#include <sensors/analog/Pitot/PitotData.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class FlightStatsRecorder : public Boardcore::Module +{ +public: + FlightStatsRecorder(Boardcore::TaskScheduler* sched); + + /** + * @brief Adds a task to the scheduler to update the stats + */ + bool start(); + + /** + * @brief Update method that gathers all the info to update the data + * structure + */ + void update(); + + /** + * @brief Gets the packet already populated + */ + mavlink_rocket_stats_tm_t getStats(); + +private: + // Update methods + void updateAcc(FlightModeManagerState flightState, + Boardcore::AccelerometerData data); + void updateBaro(FlightModeManagerState flightState, + Boardcore::PressureData data); + void updateDPLPressure(FlightModeManagerState flightState, + Boardcore::PressureData data); + void updatePitot(FlightModeManagerState flightState, + Boardcore::PitotData data); + void updateNAS(FlightModeManagerState flightState, + Boardcore::NASState data); + void updateADA(FlightModeManagerState flightState, + Boardcore::ADAState data); + + // Update scheduler to update all the data + Boardcore::TaskScheduler* scheduler = nullptr; + + // Data structure + mavlink_rocket_stats_tm_t stats; + + // Update mutex + miosix::FastMutex mutex; +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/PinHandler/PinData.h b/src/boards/Main/PinHandler/PinData.h new file mode 100644 index 0000000000000000000000000000000000000000..a100872330b1265659c74462e1c310cdd37b8245 --- /dev/null +++ b/src/boards/Main/PinHandler/PinData.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2019-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> + +namespace Main +{ + +struct PinChangeData +{ + uint64_t timestamp = 0; + uint8_t pinID = 0; + uint32_t changesCount = 0; + + PinChangeData(uint64_t timestamp, uint8_t pinID, uint32_t changesCount) + : timestamp(timestamp), pinID(pinID), changesCount(changesCount) + { + } + + PinChangeData() : PinChangeData{0, 0, 0} {} + + static std::string header() { return "timestamp,pinID,changesCount"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << pinID << "," << changesCount << "\n"; + } +}; + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/PinHandler/PinHandler.cpp b/src/boards/Main/PinHandler/PinHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b882c20ba0ac44a7035e5ca577e0dc0e02662ef8 --- /dev/null +++ b/src/boards/Main/PinHandler/PinHandler.cpp @@ -0,0 +1,145 @@ +/* Copyright (c) 2019-2023 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio, 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 <Main/Configs/PinHandlerConfig.h> +#include <Main/PinHandler/PinHandler.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <interfaces-impl/hwmapping.h> + +#include <functional> + +using namespace Boardcore; +using namespace std; +using namespace std::placeholders; +using namespace Common; + +namespace Main +{ +PinHandler::PinHandler() +{ + PinObserver::getInstance().registerPinCallback( + miosix::gpios::liftoff_detach::getPin(), + bind(&PinHandler::onLaunchPinTransition, this, _1), + PinHandlerConfig::LAUNCH_PIN_THRESHOLD); + + PinObserver::getInstance().registerPinCallback( + miosix::gpios::nosecone_detach::getPin(), + bind(&PinHandler::onNoseconeTransition, this, _1), + PinHandlerConfig::NC_DETACH_PIN_THRESHOLD); + + PinObserver::getInstance().registerPinCallback( + miosix::gpios::cut_sense::getPin(), + bind(&PinHandler::onCutterSenseTransition, this, _1), + PinHandlerConfig::CUTTER_SENSE_PIN_THRESHOLD); + + PinObserver::getInstance().registerPinCallback( + miosix::gpios::exp_sense::getPin(), + bind(&PinHandler::onExpulsionSenseTransition, this, _1), + PinHandlerConfig::EXPULSION_PIN_THRESHOLD); +} + +bool PinHandler::start() { return PinObserver::getInstance().start(); } + +bool PinHandler::isStarted() { return PinObserver::getInstance().isRunning(); } + +void PinHandler::onLaunchPinTransition(PinTransition transition) +{ + if (transition == PinHandlerConfig::LAUNCH_PIN_TRIGGER) + { + EventBroker::getInstance().post(FLIGHT_LAUNCH_PIN_DETACHED, + TOPIC_FLIGHT); + + // Log the event + PinData data = getPinData(PinList::LAUNCH_PIN); + PinChangeData log{TimestampTimer::getTimestamp(), PinList::LAUNCH_PIN, + data.changesCount}; + Logger::getInstance().log(log); + } +} + +void PinHandler::onNoseconeTransition(PinTransition transition) +{ + if (transition == PinHandlerConfig::NC_DETACH_PIN_TRIGGER) + { + EventBroker::getInstance().post(FLIGHT_NC_DETACHED, TOPIC_FLIGHT); + + // Log the event + PinData data = getPinData(PinList::NOSECONE_PIN); + PinChangeData log{TimestampTimer::getTimestamp(), PinList::NOSECONE_PIN, + data.changesCount}; + Logger::getInstance().log(log); + } +} + +void PinHandler::onCutterSenseTransition(PinTransition transition) +{ + // Log the event + PinData data = getPinData(PinList::CUTTER_PRESENCE); + PinChangeData log{TimestampTimer::getTimestamp(), PinList::CUTTER_PRESENCE, + data.changesCount}; + Logger::getInstance().log(log); +} + +void PinHandler::onExpulsionSenseTransition(PinTransition transition) +{ + // Log the event + PinData data = getPinData(PinList::PIN_EXPULSION); + PinChangeData log{TimestampTimer::getTimestamp(), PinList::PIN_EXPULSION, + data.changesCount}; + Logger::getInstance().log(log); +} + +PinData PinHandler::getPinData(PinList pin) +{ + switch (pin) + { + case PinList::LAUNCH_PIN: + { + return PinObserver::getInstance().getPinData( + miosix::gpios::liftoff_detach::getPin()); + } + case PinList::NOSECONE_PIN: + { + return PinObserver::getInstance().getPinData( + miosix::gpios::nosecone_detach::getPin()); + } + case PinList::CUTTER_PRESENCE: + { + return PinObserver::getInstance().getPinData( + miosix::gpios::cut_sense::getPin()); + } + case PinList::PIN_EXPULSION: + { + return PinObserver::getInstance().getPinData( + miosix::gpios::exp_sense::getPin()); + } + default: + { + LOG_ERR(logger, "Requested non valid pin"); + return PinData{}; + } + } +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/PinHandler/PinHandler.h b/src/boards/Main/PinHandler/PinHandler.h new file mode 100644 index 0000000000000000000000000000000000000000..c3f1f1275c2263664e7a6a9a44430df1749b880d --- /dev/null +++ b/src/boards/Main/PinHandler/PinHandler.h @@ -0,0 +1,85 @@ +/* Copyright (c) 2019-2023 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio, 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 <Main/PinHandler/PinData.h> +#include <diagnostic/PrintLogger.h> +#include <utils/PinObserver/PinObserver.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class PinHandler : public Boardcore::Module +{ +public: + enum PinList : uint8_t + { + LAUNCH_PIN = 0, + NOSECONE_PIN, + CUTTER_PRESENCE, + PIN_EXPULSION, + PIN_END + }; + + PinHandler(); + + /** + * @brief Starts the PinObserver module thread + */ + bool start(); + + /** + * @brief Checks if the module has started correctly + */ + bool isStarted(); + + /** + * @brief Called when the launch pin detaches + */ + void onLaunchPinTransition(Boardcore::PinTransition transition); + + /** + * @brief Called when the nosecone pin detaches + */ + void onNoseconeTransition(Boardcore::PinTransition transition); + + /** + * @brief Called when the cutter igniter ignites + */ + void onCutterSenseTransition(Boardcore::PinTransition transition); + + /** + * @brief Called when the expulsion sensor changes state + */ + void onExpulsionSenseTransition(Boardcore::PinTransition transition); + + /** + * @brief Returns the status of the requested pin + */ + Boardcore::PinData getPinData(PinList pin); + +private: + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("PinHandler"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp new file mode 100644 index 0000000000000000000000000000000000000000..73e8afe6131d9610837259c552c72e21389b593c --- /dev/null +++ b/src/boards/Main/Radio/Radio.cpp @@ -0,0 +1,491 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/AltitudeTrigger/AltitudeTrigger.h> +#include <Main/Buses.h> +#include <Main/PinHandler/PinHandler.h> +#include <Main/Radio/Radio.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <Main/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 <Eigen/Core> + +using namespace Boardcore; +using namespace Common; +using namespace Eigen; + +#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl +#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl +#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl + +void __attribute__((used)) SX1278_IRQ_DIO0() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<Main::Radio>()->transceiver) + { + modules.get<Main::Radio>()->transceiver->handleDioIRQ(); + } +} + +void __attribute__((used)) SX1278_IRQ_DIO1() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<Main::Radio>()->transceiver) + { + modules.get<Main::Radio>()->transceiver->handleDioIRQ(); + } +} + +void __attribute__((used)) SX1278_IRQ_DIO3() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<Main::Radio>()->transceiver) + { + modules.get<Main::Radio>()->transceiver->handleDioIRQ(); + } +} +namespace Main +{ + +Radio::Radio(TaskScheduler* sched) : scheduler(sched) {} + +bool Radio::start() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Config the transceiver + SX1278Fsk::Config config; + config.freq_rf = 419000000; + config.freq_dev = 50000; + config.bitrate = 48000; + config.rx_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000; + config.afc_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000; + config.ocp = 120; + config.power = 13; + config.shaping = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0; + config.dc_free = Boardcore::SX1278Fsk::Config::DcFree::WHITENING; + config.enable_crc = false; + + std::unique_ptr<SX1278::ISX1278Frontend> frontend = + std::make_unique<Skyward433Frontend>(); + + transceiver = new SX1278Fsk( + modules.get<Buses>()->spi6, miosix::radio::cs::getPin(), + miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(), + miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_64, + std::move(frontend)); + + // Config the radio + SX1278Fsk::Error error = transceiver->init(config); + + // Add periodic telemetry send task + uint8_t result1 = + scheduler->addTask([&]() { this->sendPeriodicMessage(); }, + RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD, + TaskScheduler::Policy::RECOVER); + uint8_t result2 = scheduler->addTask( + [&]() + { + this->enqueueMsg( + modules.get<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0)); + }, + RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2, + TaskScheduler::Policy::RECOVER); + + // Config mavDriver + mavDriver = new MavDriver( + transceiver, + [=](MavDriver*, const mavlink_message_t& msg) + { this->handleMavlinkMessage(msg); }, + RadioConfig::RADIO_SLEEP_AFTER_SEND, + RadioConfig::RADIO_OUT_BUFFER_MAX_AGE); + + // Check radio failure + if (error != SX1278Fsk::Error::NONE) + { + return false; + } + + // Start the mavlink driver thread + return mavDriver->start() && result1 != 0 && result2 != 0; +} + +void Radio::sendAck(const mavlink_message_t& msg) +{ + mavlink_message_t ackMsg; + mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid, + msg.seq); + enqueueMsg(ackMsg); +} + +void Radio::sendNack(const mavlink_message_t& msg) +{ + mavlink_message_t nackMsg; + mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid, + msg.seq); + enqueueMsg(nackMsg); +} + +void Radio::logStatus() {} + +bool Radio::isStarted() +{ + return mavDriver->isStarted() && scheduler->isRunning(); +} + +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: + { + // Let the handle command reply to the message + 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)); + + if (tmId == MAV_PIN_OBS_ID) + { + for (uint8_t i = 0; i < PinHandler::PIN_END; i++) + { + mavlink_message_t msg; + mavlink_pin_tm_t tm; + + PinData data = modules.get<PinHandler>()->getPinData( + static_cast<PinHandler::PinList>(i)); + + tm.changes_counter = data.changesCount; + tm.current_state = tm.current_state; + tm.last_change_timestamp = tm.last_change_timestamp; + tm.pin_id = tm.pin_id; + tm.timestamp = TimestampTimer::getTimestamp(); + + mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, + &tm); + enqueueMsg(msg); + } + } + else if (tmId == MAV_SENSORS_STATE_ID) + { + auto sensorsState = modules.get<Sensors>()->getSensorInfo(); + + for (uint8_t i = 0; i < sensorsState.size(); i++) + { + mavlink_message_t msg; + mavlink_sensor_state_tm_t tm; + + strcpy(tm.sensor_name, sensorsState.at(i).id.c_str()); + tm.state = sensorsState.at(i).isInitialized; + + mavlink_msg_sensor_state_tm_encode( + RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID, + &msg, &tm); + enqueueMsg(msg); + } + } + else + { + // Add to the queue the respose + mavlink_message_t response = + modules.get<TMRepository>()->packSystemTm(tmId, msg.msgid, + msg.seq); + // Add the response to the queue + enqueueMsg(response); + + // Check if the TM repo answered with a NACK. If so the function + // must return to avoid sending a default ack + if (response.msgid == MAVLINK_MSG_ID_NACK_TM) + { + return; + } + } + + break; + } + case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC: + { + SensorsTMList sensorId = static_cast<SensorsTMList>( + mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg)); + mavlink_message_t response = + modules.get<TMRepository>()->packSensorsTm(sensorId, msg.msgid, + msg.seq); + enqueueMsg(response); + + // If the response is a nack the method returns + if (response.msgid == MAVLINK_MSG_ID_NACK_TM) + { + return; + } + break; + } + case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC: + { + ServosList servo = static_cast<ServosList>( + mavlink_msg_servo_tm_request_tc_get_servo_id(&msg)); + mavlink_message_t response = + modules.get<TMRepository>()->packServoTm(servo, msg.msgid, + msg.seq); + enqueueMsg(response); + + // If the response is a nack the method returns + if (response.msgid == MAVLINK_MSG_ID_NACK_TM) + { + return; + } + break; + } + case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC: + { + ServosList servoId = static_cast<ServosList>( + mavlink_msg_set_servo_angle_tc_get_servo_id(&msg)); + float position = mavlink_msg_set_servo_angle_tc_get_angle(&msg); + + // Send nack if the FMM is not in test mode + if (!modules.get<FlightModeManager>()->testState( + &FlightModeManager::state_test_mode)) + { + return sendNack(msg); + } + + // If the state is test mode, the servo is set to the correct angle + modules.get<Actuators>()->setServoPosition(servoId, position); + break; + } + case MAVLINK_MSG_ID_WIGGLE_SERVO_TC: + { + ServosList servoId = static_cast<ServosList>( + mavlink_msg_wiggle_servo_tc_get_servo_id(&msg)); + + // Send nack if the FMM is not in test mode + if (!modules.get<FlightModeManager>()->testState( + &FlightModeManager::state_test_mode)) + { + return sendNack(msg); + } + + // If the state is test mode, the wiggle is done + modules.get<Actuators>()->wiggleServo(servoId); + + break; + } + case MAVLINK_MSG_ID_RESET_SERVO_TC: + { + ServosList servoId = static_cast<ServosList>( + mavlink_msg_reset_servo_tc_get_servo_id(&msg)); + + // Send nack if the FMM is not in test mode + if (!modules.get<FlightModeManager>()->testState( + &FlightModeManager::state_test_mode)) + { + return sendNack(msg); + } + + // Set the servo position to 0 + modules.get<Actuators>()->setServoPosition(servoId, 0); + + break; + } + case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: + { + float altitude = + mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg); + + modules.get<AltitudeTrigger>()->setDeploymentAltitude(altitude); + break; + } + case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC: + { + float altitude = + mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg); + + modules.get<ADAController>()->setReferenceAltitude(altitude); + modules.get<NASController>()->setReferenceAltitude(altitude); + + break; + } + case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC: + { + float temperature = + mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg); + + modules.get<ADAController>()->setReferenceTemperature(temperature); + modules.get<NASController>()->setReferenceTemperature(temperature); + + break; + } + case MAVLINK_MSG_ID_SET_COORDINATES_TC: + { + float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg); + float longitude = + mavlink_msg_set_coordinates_tc_get_longitude(&msg); + + Vector2f coordinates{latitude, longitude}; + + modules.get<NASController>()->setCoordinates(coordinates); + break; + } + case MAVLINK_MSG_ID_SET_ORIENTATION_TC: + { + float yaw = mavlink_msg_set_orientation_tc_get_yaw(&msg); + float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg); + float roll = mavlink_msg_set_orientation_tc_get_roll(&msg); + + modules.get<NASController>()->setOrientation(yaw, pitch, roll); + break; + } + default: + { + LOG_DEBUG(logger, "Received message is not of a known type"); + return sendNack(msg); + } + } + + // At the end send the ack message + sendAck(msg); +} + +void Radio::handleCommand(const mavlink_message_t& msg) +{ + MavCommandList commandId = static_cast<MavCommandList>( + mavlink_msg_command_tc_get_command_id(&msg)); + ModuleManager& modules = ModuleManager::getInstance(); + + // Create the map between the commands and the corresponding events + static const std::map<MavCommandList, Events> commandToEvent{ + {MAV_CMD_ARM, TMTC_ARM}, + {MAV_CMD_DISARM, TMTC_DISARM}, + {MAV_CMD_CALIBRATE, TMTC_CALIBRATE}, + {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT}, + {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH}, + {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING}, + {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE}, + {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION}, + {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT}, + {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD}, + {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE}, + {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE}, + {MAV_CMD_START_RECORDING, TMTC_START_RECORDING}, + {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING}, + }; + + switch (commandId) + { + case MAV_CMD_SAVE_CALIBRATION: + { + // Save the sensor calibration and adopt it + modules.get<Sensors>()->writeMagCalibration(); + break; + } + case MAV_CMD_START_LOGGING: + { + bool result = Logger::getInstance().start(); + + // In case the logger is not started send to GS the result + if (!result) + { + return sendNack(msg); + } + break; + } + case MAV_CMD_STOP_LOGGING: + { + Logger::getInstance().stop(); + break; + } + default: + { + // If the command is not a particular one, look for it inside the + // map + auto it = commandToEvent.find(commandId); + + if (it != commandToEvent.end()) + { + EventBroker::getInstance().post(it->second, TOPIC_TMTC); + } + else + { + return sendNack(msg); + } + } + } + // Acknowledge the message + sendAck(msg); +} + +void Radio::sendPeriodicMessage() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Send all the queue messages + { + Lock<FastMutex> lock(queueMutex); + + for (uint8_t i = 0; i < messageQueueIndex; i++) + { + mavDriver->enqueueMsg(messageQueue[i]); + } + + // Reset the index + messageQueueIndex = 0; + } + + mavDriver->enqueueMsg( + modules.get<TMRepository>()->packSystemTm(MAV_MOTOR_ID, 0, 0)); + mavDriver->enqueueMsg( + modules.get<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0)); +} + +void Radio::enqueueMsg(const mavlink_message_t& msg) +{ + { + Lock<FastMutex> lock(queueMutex); + + // Insert the message inside the queue only if there is enough space + if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE) + { + messageQueue[messageQueueIndex] = msg; + messageQueueIndex++; + } + } +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h new file mode 100644 index 0000000000000000000000000000000000000000..8c1fd4cde31fcbde4b190b31c0e3c56291b728e6 --- /dev/null +++ b/src/boards/Main/Radio/Radio.h @@ -0,0 +1,102 @@ +/* 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 <Main/Configs/RadioConfig.h> +#include <common/Mavlink.h> +#include <radio/MavlinkDriver/MavlinkDriver.h> +#include <radio/SX1278/SX1278Fsk.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU, + RadioConfig::RADIO_OUT_QUEUE_SIZE, + RadioConfig::RADIO_MAV_MSG_LENGTH>; + +class Radio : public Boardcore::Module +{ +public: + Radio(Boardcore::TaskScheduler* sched); + + /** + * @brief Starts the MavlinkDriver + */ + [[nodiscard]] bool start(); + + /** + * @brief Sends via radio an acknowledge message about the parameter passed + * message + */ + void sendAck(const mavlink_message_t& msg); + + /** + * @brief Sends via radio an non-acknowledge message about the parameter + * passed message + */ + 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::SX1278Fsk* 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 the handleMavlinkMessage to handle a command message + */ + void handleCommand(const mavlink_message_t& msg); + + /** + * @brief Sends the periodic telemetry + */ + void sendPeriodicMessage(); + + /** + * @brief Inserts the mavlink message into the queue + */ + void enqueueMsg(const mavlink_message_t& msg); + + // Messages queue + mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE]; + uint32_t messageQueueIndex = 0; + miosix::FastMutex queueMutex; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio"); + Boardcore::TaskScheduler* scheduler = nullptr; +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2bee48f748fe0a98661f009f1e427432e2c04233 --- /dev/null +++ b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp @@ -0,0 +1,103 @@ +/* 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 <Main/Sensors/RotatedIMU/RotatedIMU.h> + +using namespace Eigen; +using namespace Boardcore; + +namespace Main +{ + +RotatedIMU::RotatedIMU(std::function<AccelerometerData()> accSampleFunction, + std::function<MagnetometerData()> magSampleFunction, + std::function<GyroscopeData()> gyroSampleFunction) + : accSample(accSampleFunction), magSample(magSampleFunction), + gyroSample(gyroSampleFunction), accT(Matrix3f::Identity()), + magT(Matrix3f::Identity()), gyroT(Matrix3f::Identity()) +{ +} + +void RotatedIMU::addAccTransformation(const Matrix3f& t) { accT = t * accT; } +void RotatedIMU::addMagTransformation(const Matrix3f& t) { magT = t * magT; } +void RotatedIMU::addGyroTransformation(const Matrix3f& t) { gyroT = t * gyroT; } + +void RotatedIMU::resetTransformations() +{ + accT = Matrix3f::Identity(); + magT = Matrix3f::Identity(); + gyroT = Matrix3f::Identity(); +} + +RotatedIMUData RotatedIMU::sampleImpl() +{ + Vector3f accResult = accT * static_cast<Vector3f>(accSample()); + Vector3f magResult = magT * static_cast<Vector3f>(magSample()); + Vector3f gyroResult = gyroT * static_cast<Vector3f>(gyroSample()); + + return {AccelerometerData{accResult}, GyroscopeData{gyroResult}, + MagnetometerData{magResult}}; +} + +// Static functions +Matrix3f RotatedIMU::rotateAroundX(float angle) +{ + Matrix3f rotation; + angle = angle * EIGEN_PI / 180.f; + + // clang-format off + rotation = Matrix3f{{1, 0, 0}, + {0, cosf(angle), -sinf(angle)}, + {0, sinf(angle), cosf(angle)}}; + // clang-format on + + return rotation; +} + +Matrix3f RotatedIMU::rotateAroundY(float angle) +{ + Matrix3f rotation; + angle = angle * EIGEN_PI / 180.f; + + // clang-format off + rotation = Matrix3f{{cosf(angle), 0, sinf(angle)}, + {0, 1, 0}, + {-sinf(angle), 0, cosf(angle)}}; + // clang-format on + + return rotation; +} + +Matrix3f RotatedIMU::rotateAroundZ(float angle) +{ + Matrix3f rotation; + angle = angle * EIGEN_PI / 180.f; + + // clang-format off + rotation = Matrix3f{{cosf(angle), -sinf(angle), 0}, + {sinf(angle), cosf(angle), 0}, + {0, 0, 1}}; + // clang-format on + + return rotation; +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h new file mode 100644 index 0000000000000000000000000000000000000000..dc26b01524f2764e00bdc1e2193d2698f8ee123f --- /dev/null +++ b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h @@ -0,0 +1,120 @@ +/* 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 <Main/Sensors/RotatedIMU/RotatedIMUData.h> +#include <sensors/Sensor.h> + +#include <Eigen/Eigen> +#include <functional> + +namespace Main +{ +/** + * @brief Creates a "fake" sensor which allows the user + * to transform the values with transformation matrices. + * The default configuration (thus the reset one) + * comprehends identity matrices as transformation ones. + * + * @warning This is NOT a thread safe class + */ +class RotatedIMU : public Boardcore::Sensor<RotatedIMUData> +{ +public: + /** + * @brief Construct a new Rotated IMU object + * + * @param accSampleFunction Function to call to retrieve an accelerometer + * sample + * @param magSampleFunction Function to call to retrieve a magnetometer + * sample + * @param gyroSampleFunction Function to call to retrieve a gyroscope sample + */ + RotatedIMU( + std::function<Boardcore::AccelerometerData()> accSampleFunction = []() + { return Boardcore::AccelerometerData{}; }, + std::function<Boardcore::MagnetometerData()> magSampleFunction = []() + { return Boardcore::MagnetometerData{}; }, + std::function<Boardcore::GyroscopeData()> gyroSampleFunction = []() + { return Boardcore::GyroscopeData{}; }); + + /** + * @brief Multiplies the current accelerometer transformation + * @param transformation Transformation matrix to be multiplied to the + * current one + */ + void addAccTransformation(const Eigen::Matrix3f& t); + + /** + * @brief Multiplies the current magnetometer transformation + * @param transformation Transformation matrix to be multiplied to the + * current one + */ + void addMagTransformation(const Eigen::Matrix3f& t); + + /** + * @brief Multiplies the current gyroscope transformation + * @param transformation Transformation matrix to be multiplied to the + * current one + */ + void addGyroTransformation(const Eigen::Matrix3f& t); + + /** + * @brief Resets all the transformations to the original (Identity) ones + */ + void resetTransformations(); + + bool init() override { return true; } + bool selfTest() override { return true; } + + /** + * @brief Creates a rotation matrix around the X axis + * @param angle Angle in degrees + */ + static Eigen::Matrix3f rotateAroundX(float angle); + + /** + * @brief Creates a rotation matrix around the Y axis + * @param angle Angle in degrees + */ + static Eigen::Matrix3f rotateAroundY(float angle); + + /** + * @brief Creates a rotation matrix around the Z axis + * @param angle Angle in degrees + */ + static Eigen::Matrix3f rotateAroundZ(float angle); + +private: + RotatedIMUData sampleImpl() override; + + // Functions to sample the under neath sensors + std::function<Boardcore::AccelerometerData()> accSample; + std::function<Boardcore::MagnetometerData()> magSample; + std::function<Boardcore::GyroscopeData()> gyroSample; + + // Transformation matrices + Eigen::Matrix3f accT; + Eigen::Matrix3f magT; + Eigen::Matrix3f gyroT; +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h b/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h new file mode 100644 index 0000000000000000000000000000000000000000..48a6fbcd4fc20185828379d46fc396a4c4a6affc --- /dev/null +++ b/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h @@ -0,0 +1,60 @@ +/* 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 <sensors/SensorData.h> + +struct RotatedIMUData : public Boardcore::AccelerometerData, + public Boardcore::MagnetometerData, + public Boardcore::GyroscopeData +{ + RotatedIMUData() + : AccelerometerData{0, 0.0, 0.0, 0.0}, + MagnetometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0} + + { + } + + RotatedIMUData(AccelerometerData acc, GyroscopeData gyr, + MagnetometerData mag) + : AccelerometerData(acc), MagnetometerData(mag), GyroscopeData(gyr) + { + } + + static std::string header() + { + return "accelerationTimestamp,accelerationX,accelerationY," + "accelerationZ,angularSpeedTimestamp,angularSpeedX," + "angularSpeedY,angularSpeedZ,magneticFieldTimestamp," + "magneticFieldX,magneticFieldY,magneticFieldZ\n"; + } + + void print(std::ostream& os) const + { + os << accelerationTimestamp << "," << accelerationX << "," + << accelerationY << "," << accelerationZ << "," + << angularSpeedTimestamp << "," << angularSpeedX << "," + << angularSpeedY << "," << angularSpeedZ << "," + << magneticFieldTimestamp << "," << magneticFieldX << "," + << magneticFieldY << "," << magneticFieldZ << "\n"; + } +}; \ No newline at end of file diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..572a222201f46ce7e3f422eb11b7a4eeb5418bb3 --- /dev/null +++ b/src/boards/Main/Sensors/Sensors.cpp @@ -0,0 +1,731 @@ +/* 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 "Sensors.h" + +#include <Main/Buses.h> +#include <Main/Configs/SensorsConfig.h> +#include <interfaces-impl/hwmapping.h> + +using namespace Boardcore; +using namespace std; +using namespace Main::SensorsConfig; +namespace Main +{ +LPS22DFData Sensors::getLPS22DFLastSample() +{ + miosix::PauseKernelLock lock; + return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{}; +} +LPS28DFWData Sensors::getLPS28DFW_1LastSample() +{ + miosix::PauseKernelLock lock; + return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{}; +} +LPS28DFWData Sensors::getLPS28DFW_2LastSample() +{ + miosix::PauseKernelLock lock; + return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{}; +} +H3LIS331DLData Sensors::getH3LIS331DLLastSample() +{ + miosix::PauseKernelLock lock; + return h3lis331dl != nullptr ? h3lis331dl->getLastSample() + : H3LIS331DLData{}; +} +LIS2MDLData Sensors::getLIS2MDLLastSample() +{ + miosix::PauseKernelLock lock; + return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{}; +} +UBXGPSData Sensors::getGPSLastSample() +{ + miosix::PauseKernelLock lock; + return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{}; +} +LSM6DSRXData Sensors::getLSM6DSRXLastSample() +{ + miosix::PauseKernelLock lock; + return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; +} +ADS131M08Data Sensors::getADS131M08LastSample() +{ + miosix::PauseKernelLock lock; + return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{}; +} + +PitotData Sensors::getPitotLastSample() +{ + miosix::PauseKernelLock lock; + return canPitot; +} +PressureData Sensors::getCCPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canCCPressure; +} +PressureData Sensors::getBottomTankPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canBottomTankPressure; +} +PressureData Sensors::getTopTankPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canTopTankPressure; +} +TemperatureData Sensors::getTankTemperatureLastSample() +{ + miosix::PauseKernelLock lock; + return canTankTemperature; +} +BatteryVoltageSensorData Sensors::getMotorBatteryVoltage() +{ + miosix::PauseKernelLock lock; + return canMotorBatteryVoltage; +} +CurrentData Sensors::getMotorCurrent() +{ + miosix::PauseKernelLock lock; + return canMotorCurrent; +} + +// Processed Getters +BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + BatteryVoltageSensorData data; + + // Populate the data + data.voltageTimestamp = sample.timestamp; + data.channelId = 1; + data.voltage = sample.voltage[1]; + data.batVoltage = sample.voltage[1] * BATTERY_VOLTAGE_CONVERSION_FACTOR; + return data; +} + +BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + BatteryVoltageSensorData data; + + // Populate the data + data.voltageTimestamp = sample.timestamp; + data.channelId = 7; + data.voltage = sample.voltage[7]; + data.batVoltage = sample.voltage[7] * BATTERY_VOLTAGE_CONVERSION_FACTOR; + return data; +} + +CurrentData Sensors::getCurrentLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + CurrentData data; + + // Populate the data + data.currentTimestamp = sample.timestamp; + data.current = + (sample.voltage[5] - CURRENT_OFFSET) * CURRENT_CONVERSION_FACTOR; + return data; +} + +MPXH6400AData Sensors::getDeploymentPressureLastSample() +{ + miosix::PauseKernelLock lock; + return mpxh6400a != nullptr ? mpxh6400a->getLastSample() : MPXH6400AData{}; +} + +HSCMRNN015PAData Sensors::getStaticPressure1LastSample() +{ + miosix::PauseKernelLock lock; + return hscmrnn015pa_1 != nullptr ? hscmrnn015pa_1->getLastSample() + : HSCMRNN015PAData{}; +} + +HSCMRNN015PAData Sensors::getStaticPressure2LastSample() +{ + miosix::PauseKernelLock lock; + return hscmrnn015pa_2 != nullptr ? hscmrnn015pa_2->getLastSample() + : HSCMRNN015PAData{}; +} + +RotatedIMUData Sensors::getIMULastSample() +{ + miosix::PauseKernelLock lock; + return imu != nullptr ? imu->getLastSample() : RotatedIMUData{}; +} + +MagnetometerData Sensors::getCalibratedMagnetometerLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + MagnetometerData lastSample = getLIS2MDLLastSample(); + MagnetometerData result; + + // Correct the result and copy the timestamp + { + miosix::Lock<FastMutex> l(calibrationMutex); + result = + static_cast<MagnetometerData>(magCalibration.correct(lastSample)); + } + + result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp; + return result; +} + +void Sensors::setPitot(PitotData data) +{ + miosix::PauseKernelLock lock; + canPitot.timestamp = TimestampTimer::getTimestamp(); + canPitot = data; +} +void Sensors::setCCPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp(); + canCCPressure = data; +} +void Sensors::setBottomTankPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); + canBottomTankPressure = data; +} +void Sensors::setTopTankPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); + canTopTankPressure = data; +} +void Sensors::setTankTemperature(TemperatureData data) +{ + miosix::PauseKernelLock lock; + canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp(); + canTankTemperature = data; +} +void Sensors::setMotorBatteryVoltage(BatteryVoltageSensorData data) +{ + miosix::PauseKernelLock lock; + canMotorBatteryVoltage.voltageTimestamp = TimestampTimer::getTimestamp(); + canMotorBatteryVoltage.batVoltage = data.batVoltage; +} +void Sensors::setMotorCurrent(CurrentData data) +{ + miosix::PauseKernelLock lock; + canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp(); + canMotorCurrent.current = data.current; +} + +Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {} + +bool Sensors::start() +{ + // Read the magnetometer calibration from predefined file + magCalibration.fromFile("magCalibration.csv"); + + // Init all the sensors + lps22dfInit(); + lps28dfw_1Init(); + // lps28dfw_2Init(); + h3lis331dlInit(); + lis2mdlInit(); + ubxgpsInit(); + lsm6dsrxInit(); + ads131m08Init(); + deploymentPressureInit(); + staticPressure1Init(); + staticPressure2Init(); + imuInit(); + + // Add the magnetometer calibration to the scheduler + size_t result = scheduler->addTask( + [&]() + { + // Gather the last sample data + MagnetometerData lastSample = getLIS2MDLLastSample(); + + // Feed the data to the calibrator inside a protected area. + // Contention is not high and the use of a mutex is suitable to + // avoid pausing the kernel for this calibration operation + { + miosix::Lock<FastMutex> l(calibrationMutex); + magCalibrator.feed(lastSample); + } + }, + MAG_CALIBRATION_PERIOD); + + // Create sensor manager with populated map and configured scheduler + manager = new SensorManager(sensorMap, scheduler); + return manager->start() && result != 0; +} + +void Sensors::stop() { manager->stop(); } + +bool Sensors::isStarted() +{ + return manager->areAllSensorsInitialized() && scheduler->isRunning(); +} + +void Sensors::calibrate() +{ + // Create the stats to calibrate the barometers + Stats lps28dfw1Stats; + // Stats lps28dfw2Stats; + Stats staticPressure1Stats; + Stats staticPressure2Stats; + Stats deploymentPressureStats; + + // Add N samples to the stats + for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++) + { + lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure); + // lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure); + staticPressure1Stats.add(getStaticPressure1LastSample().pressure); + staticPressure2Stats.add(getStaticPressure2LastSample().pressure); + deploymentPressureStats.add(getDeploymentPressureLastSample().pressure); + + // Delay for the expected period + miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD); + } + + // Compute the difference between the mean value from LPS28DFW + float reference = lps28dfw1Stats.getStats().mean; + + hscmrnn015pa_1->updateOffset(staticPressure1Stats.getStats().mean - + reference); + hscmrnn015pa_2->updateOffset(staticPressure2Stats.getStats().mean - + reference); + mpxh6400a->updateOffset(deploymentPressureStats.getStats().mean - + reference); + + // Log the offsets + SensorsCalibrationParameter cal{}; + cal.timestamp = TimestampTimer::getTimestamp(); + cal.offsetStatic1 = staticPressure1Stats.getStats().mean - reference; + cal.offsetStatic2 = staticPressure2Stats.getStats().mean - reference; + cal.offsetDeployment = deploymentPressureStats.getStats().mean - reference; + cal.referencePressure = reference; + + Logger::getInstance().log(cal); +} + +void Sensors::writeMagCalibration() +{ + // Compute the calibration result in protected area + { + miosix::Lock<FastMutex> l(calibrationMutex); + SixParametersCorrector cal = magCalibrator.computeResult(); + + // Check result validity + if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) && + !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) && + !isnan(cal.getA()[1]) && !isnan(cal.getA()[2])) + { + magCalibration = cal; + + // Save the calibration to the calibration file + magCalibration.toFile("magCalibration.csv"); + } + } +} + +std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> +Sensors::getSensorInfo() +{ + std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> sensorState; + for (size_t i = 0; i < sensorsInit.size(); i++) + { + // Check wether the lambda is existent + if (sensorsInit[i]) + { + sensorState[i] = sensorsInit[i](); + } + } + return sensorState; +} + +void Sensors::lps22dfInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = LPS22DF::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Configure the device + LPS22DF::Config sensorConfig; + sensorConfig.avg = LPS22DF_AVG; + sensorConfig.odr = LPS22DF_ODR; + + // Create sensor instance with configured parameters + lps22df = new LPS22DF(modules.get<Buses>()->spi3, + miosix::sensors::LPS22DF::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("LPS22DF", LPS22DF_PERIOD, + bind(&Sensors::lps22dfCallback, this)); + sensorMap.emplace(make_pair(lps22df, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lps22df); }; +} +void Sensors::lps28dfw_1Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the sensor + LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG, + LPS28DFW_ODR, false}; + + // Create sensor instance with configured parameters + lps28dfw_1 = new LPS28DFW(modules.get<Buses>()->i2c1, config); + + // Emplace the sensor inside the map + SensorInfo info("LPS28DFW_1", LPS28DFW_PERIOD, + bind(&Sensors::lps28dfw_1Callback, this)); + sensorMap.emplace(make_pair(lps28dfw_1, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lps28dfw_1); }; +} +void Sensors::lps28dfw_2Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the sensor + LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG, + LPS28DFW_ODR, false}; + + // Create sensor instance with configured parameters + lps28dfw_2 = new LPS28DFW(modules.get<Buses>()->i2c1, config); + + // Emplace the sensor inside the map + SensorInfo info("LPS28DFW_2", LPS28DFW_PERIOD, + bind(&Sensors::lps28dfw_2Callback, this)); + sensorMap.emplace(make_pair(lps28dfw_2, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lps28dfw_2); }; +} +void Sensors::h3lis331dlInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Create sensor instance with configured parameters + h3lis331dl = new H3LIS331DL( + modules.get<Buses>()->spi3, miosix::sensors::H3LIS331DL::cs::getPin(), + config, H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR); + + // Emplace the sensor inside the map + SensorInfo info("H3LIS331DL", H3LIS331DL_PERIOD, + bind(&Sensors::h3lis331dlCallback, this)); + sensorMap.emplace(make_pair(h3lis331dl, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(h3lis331dl); }; +} +void Sensors::lis2mdlInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = LIS2MDL::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Configure the sensor + LIS2MDL::Config sensorConfig; + sensorConfig.deviceMode = LIS2MDL_OPERATIVE_MODE; + sensorConfig.odr = LIS2MDL_ODR; + sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER; + + // Create sensor instance with configured parameters + lis2mdl = new LIS2MDL(modules.get<Buses>()->spi3, + miosix::sensors::LIS2MDL::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("LIS2MDL", LIS2MDL_PERIOD, + bind(&Sensors::lis2mdlCallback, this)); + sensorMap.emplace(make_pair(lis2mdl, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lis2mdl); }; +} + +void Sensors::ubxgpsInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_64; + + // Create sensor instance with configured parameters + ubxgps = new UBXGPSSpi(modules.get<Buses>()->spi4, + miosix::sensors::GPS::cs::getPin(), config, 5); + + // Emplace the sensor inside the map + SensorInfo info("UBXGPS", UBXGPS_PERIOD, + bind(&Sensors::ubxgpsCallback, this)); + sensorMap.emplace(make_pair(ubxgps, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(ubxgps); }; +} + +void Sensors::lsm6dsrxInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the SPI + SPIBusConfig config; + config.clockDivider = SPI::ClockDivider::DIV_32; + config.mode = SPI::Mode::MODE_0; + + // Configure the sensor + LSM6DSRXConfig sensorConfig; + sensorConfig.bdu = LSM6DSRX_BDU; + + // Accelerometer + sensorConfig.fsAcc = LSM6DSRX_ACC_FS; + sensorConfig.odrAcc = LSM6DSRX_ACC_ODR; + sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE; + + // Gyroscope + sensorConfig.fsGyr = LSM6DSRX_GYR_FS; + sensorConfig.odrGyr = LSM6DSRX_GYR_ODR; + sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE; + + // Fifo + sensorConfig.fifoMode = LSM6DSRX_FIFO_MODE; + sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION; + sensorConfig.fifoTemperatureBdr = LSM6DSRX_FIFO_TEMPERATURE_BDR; + + // Create sensor instance with configured parameters + lsm6dsrx = new LSM6DSRX(modules.get<Buses>()->spi1, + miosix::sensors::LSM6DSRX::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD, + bind(&Sensors::lsm6dsrxCallback, this)); + sensorMap.emplace(make_pair(lsm6dsrx, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lsm6dsrx); }; +} + +void Sensors::ads131m08Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the SPI + SPIBusConfig config; + config.clockDivider = SPI::ClockDivider::DIV_32; + + // Configure the device + ADS131M08::Config sensorConfig; + sensorConfig.oversamplingRatio = ADS131M08_OVERSAMPLING_RATIO; + sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE; + + // Create the sensor instance with configured parameters + ads131m08 = new ADS131M08(modules.get<Buses>()->spi4, + miosix::sensors::ADS131::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("ADS131M08", ADS131M08_PERIOD, + bind(&Sensors::ads131m08Callback, this)); + sensorMap.emplace(make_pair(ads131m08, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(ads131m08); }; +} + +void Sensors::deploymentPressureInit() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_DPL); + }; + + // Create the sensor instance with created function + mpxh6400a = new MPXH6400A(getVoltage, ADC_VOLTAGE_RANGE); + + // Emplace the sensor inside the map + SensorInfo info("MPXH6400A", ADS131M08_PERIOD, + bind(&Sensors::deploymentPressureCallback, this)); + sensorMap.emplace(make_pair(mpxh6400a, info)); +} + +void Sensors::staticPressure1Init() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_STATIC_1); + }; + + // Create the sensor instance with created function + hscmrnn015pa_1 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE); + + // Emplace the sensor inside the map + SensorInfo info("HSCMRNN015PA_1", ADS131M08_PERIOD, + bind(&Sensors::staticPressure1Callback, this)); + sensorMap.emplace(make_pair(hscmrnn015pa_1, info)); +} + +void Sensors::staticPressure2Init() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_STATIC_2); + }; + + // Create the sensor instance with created function + hscmrnn015pa_2 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE); + + // Emplace the sensor inside the map + SensorInfo info("HSCMRNN015PA_2", ADS131M08_PERIOD, + bind(&Sensors::staticPressure2Callback, this)); + sensorMap.emplace(make_pair(hscmrnn015pa_2, info)); +} + +void Sensors::imuInit() +{ + // Register the IMU as the fake sensor, passing as parameters the methods to + // retrieve real data. The sensor is not synchronized, but the sampling + // thread is always the same. + imu = new RotatedIMU( + bind(&LSM6DSRX::getLastSample, lsm6dsrx), + bind(&Sensors::getCalibratedMagnetometerLastSample, this), + bind(&LSM6DSRX::getLastSample, lsm6dsrx)); + + // Invert the Y axis on the magnetometer + Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}}; + imu->addMagTransformation(m); + + // Emplace the sensor inside the map + SensorInfo info("RotatedIMU", IMU_PERIOD, + bind(&Sensors::imuCallback, this)); + sensorMap.emplace(make_pair(imu, info)); +} + +void Sensors::lps22dfCallback() +{ + LPS22DFData lastSample = lps22df->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lps28dfw_1Callback() +{ + LPS28DFW_1Data lastSample = + static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::lps28dfw_2Callback() +{ + LPS28DFW_2Data lastSample = + static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::h3lis331dlCallback() +{ + H3LIS331DLData lastSample = h3lis331dl->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lis2mdlCallback() +{ + LIS2MDLData lastSample = lis2mdl->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::ubxgpsCallback() +{ + UBXGPSData lastSample = ubxgps->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lsm6dsrxCallback() +{ + auto& fifo = lsm6dsrx->getLastFifo(); + uint16_t fifoSize = lsm6dsrx->getLastFifoSize(); + + // For every instance inside the fifo log the sample + for (uint16_t i = 0; i < fifoSize; i++) + { + Logger::getInstance().log(fifo.at(i)); + } +} +void Sensors::ads131m08Callback() +{ + ADS131M08Data lastSample = ads131m08->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::deploymentPressureCallback() +{ + MPXH6400AData lastSample = mpxh6400a->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::staticPressure1Callback() +{ + HSCMRNN015PA_1Data lastSample = + static_cast<HSCMRNN015PA_1Data>(hscmrnn015pa_1->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::staticPressure2Callback() +{ + HSCMRNN015PA_2Data lastSample = + static_cast<HSCMRNN015PA_2Data>(hscmrnn015pa_2->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::imuCallback() +{ + RotatedIMUData lastSample = imu->getLastSample(); + Logger::getInstance().log(lastSample); +} + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h new file mode 100644 index 0000000000000000000000000000000000000000..a9dafb632f1fa8aced819b5e773d551e9dbb2639 --- /dev/null +++ b/src/boards/Main/Sensors/Sensors.h @@ -0,0 +1,202 @@ +/* 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 <Main/Configs/SensorsConfig.h> +#include <Main/Sensors/RotatedIMU/RotatedIMU.h> +#include <Main/Sensors/SensorsData.h> +#include <sensors/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/LPS28DFW/LPS28DFW.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/SensorData.h> +#include <sensors/SensorManager.h> +#include <sensors/UBXGPS/UBXGPSSpi.h> +#include <sensors/analog/BatteryVoltageSensorData.h> +#include <sensors/analog/Pitot/PitotData.h> +#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h> +#include <sensors/analog/pressure/nxp/MPXH6400A.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> +#include <stdint.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class Sensors : public Boardcore::Module +{ +public: + explicit Sensors(Boardcore::TaskScheduler* sched); + + [[nodiscard]] bool start(); + + /** + * @brief Stops the sensor manager + * @warning Stops the passed scheduler + */ + void stop(); + + /** + * @brief Returns if all the sensors are started successfully + */ + bool isStarted(); + + /** + * @brief Calibrates the sensors with an offset + */ + void calibrate(); + + /** + * @brief Takes the result of the live magnetometer calibration and applies + * it to the current calibration + writes it in the csv file + */ + void writeMagCalibration(); + + // Sensor getters + Boardcore::LPS22DFData getLPS22DFLastSample(); + Boardcore::LPS28DFWData getLPS28DFW_1LastSample(); + Boardcore::LPS28DFWData getLPS28DFW_2LastSample(); + Boardcore::H3LIS331DLData getH3LIS331DLLastSample(); + Boardcore::LIS2MDLData getLIS2MDLLastSample(); + Boardcore::UBXGPSData getGPSLastSample(); + Boardcore::LSM6DSRXData getLSM6DSRXLastSample(); + Boardcore::ADS131M08Data getADS131M08LastSample(); + + // Processed getters + Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample(); + Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample(); + Boardcore::CurrentData getCurrentLastSample(); + Boardcore::MPXH6400AData getDeploymentPressureLastSample(); + Boardcore::HSCMRNN015PAData getStaticPressure1LastSample(); + Boardcore::HSCMRNN015PAData getStaticPressure2LastSample(); + RotatedIMUData getIMULastSample(); + Boardcore::MagnetometerData getCalibratedMagnetometerLastSample(); + + // CAN fake sensors setters + void setPitot(Boardcore::PitotData data); + void setCCPressure(Boardcore::PressureData data); + void setBottomTankPressure(Boardcore::PressureData data); + void setTopTankPressure(Boardcore::PressureData data); + void setTankTemperature(Boardcore::TemperatureData data); + void setMotorBatteryVoltage(Boardcore::BatteryVoltageSensorData data); + void setMotorCurrent(Boardcore::CurrentData data); + + // CAN fake sensors getters + Boardcore::PitotData getPitotLastSample(); + Boardcore::PressureData getCCPressureLastSample(); + Boardcore::PressureData getBottomTankPressureLastSample(); + Boardcore::PressureData getTopTankPressureLastSample(); + Boardcore::TemperatureData getTankTemperatureLastSample(); + Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage(); + Boardcore::CurrentData getMotorCurrent(); + + // Returns the sensors statuses + std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> + getSensorInfo(); + +private: + // Init and callbacks methods + void lps22dfInit(); + void lps22dfCallback(); + + void lps28dfw_1Init(); + void lps28dfw_1Callback(); + + void lps28dfw_2Init(); + void lps28dfw_2Callback(); + + void h3lis331dlInit(); + void h3lis331dlCallback(); + + void lis2mdlInit(); + void lis2mdlCallback(); + + void ubxgpsInit(); + void ubxgpsCallback(); + + void lsm6dsrxInit(); + void lsm6dsrxCallback(); + + void ads131m08Init(); + void ads131m08Callback(); + + void deploymentPressureInit(); + void deploymentPressureCallback(); + + void staticPressure1Init(); + void staticPressure1Callback(); + + void staticPressure2Init(); + void staticPressure2Callback(); + + void imuInit(); + void imuCallback(); + + // Sensors instances + Boardcore::LPS22DF* lps22df = nullptr; + Boardcore::LPS28DFW* lps28dfw_1 = nullptr; + Boardcore::LPS28DFW* lps28dfw_2 = nullptr; + Boardcore::H3LIS331DL* h3lis331dl = nullptr; + Boardcore::LIS2MDL* lis2mdl = nullptr; + Boardcore::UBXGPSSpi* ubxgps = nullptr; + Boardcore::LSM6DSRX* lsm6dsrx = nullptr; + Boardcore::ADS131M08* ads131m08 = nullptr; + + // Can sensors + Boardcore::PitotData canPitot{}; + Boardcore::PressureData canCCPressure{}; + Boardcore::PressureData canBottomTankPressure{}; + Boardcore::PressureData canTopTankPressure{}; + Boardcore::TemperatureData canTankTemperature{}; + Boardcore::BatteryVoltageSensorData canMotorBatteryVoltage{}; + Boardcore::CurrentData canMotorCurrent{}; + + // Fake processed sensors + RotatedIMU* imu = nullptr; + Boardcore::MPXH6400A* mpxh6400a = nullptr; + Boardcore::HSCMRNN015PA* hscmrnn015pa_1 = nullptr; + Boardcore::HSCMRNN015PA* hscmrnn015pa_2 = nullptr; + + // Magnetometer live calibration + Boardcore::SoftAndHardIronCalibration magCalibrator; + Boardcore::SixParametersCorrector magCalibration; + miosix::FastMutex calibrationMutex; + + // Sensor manager + Boardcore::SensorManager* manager = nullptr; + Boardcore::SensorManager::SensorMap_t sensorMap; + Boardcore::TaskScheduler* scheduler = nullptr; + + // Collection of lambdas to get the sensor init statuses + std::array<std::function<Boardcore::SensorInfo()>, + SensorsConfig::NUMBER_OF_SENSORS> + sensorsInit; + uint8_t sensorsId = 0; + + // SD logger + Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance(); + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/SensorsData.h b/src/boards/Main/Sensors/SensorsData.h new file mode 100644 index 0000000000000000000000000000000000000000..eda2f6980b808a9f4678eab2514b8000b02f3931 --- /dev/null +++ b/src/boards/Main/Sensors/SensorsData.h @@ -0,0 +1,140 @@ +/* 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 <sensors/LPS28DFW/LPS28DFWData.h> +#include <sensors/analog/pressure/honeywell/HSCMRNN015PAData.h> + +namespace Main +{ +struct SensorsCalibrationParameter +{ + uint64_t timestamp; + float referencePressure; + float offsetStatic1; + float offsetStatic2; + float offsetDeployment; + + SensorsCalibrationParameter(uint64_t timestamp, float referencePressure, + float offsetStatic1, float offsetStatic2, + float offsetDeployment) + : timestamp(timestamp), referencePressure(referencePressure), + offsetStatic1(offsetStatic1), offsetStatic2(offsetStatic2), + offsetDeployment(offsetDeployment) + { + } + + SensorsCalibrationParameter() : SensorsCalibrationParameter(0, 0, 0, 0, 0) + { + } + + static std::string header() + { + return "timestamp,referencePressure,offsetStatic1,offsetStatic2," + "offsetDeployment"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << referencePressure << "," << offsetStatic1 + << "," << offsetStatic2 << "," << offsetDeployment << "\n"; + } +}; +struct LPS28DFW_1Data : Boardcore::LPS28DFWData +{ + explicit LPS28DFW_1Data(const Boardcore::LPS28DFWData& data) + : Boardcore::LPS28DFWData(data) + { + } + + LPS28DFW_1Data() {} + + static std::string header() + { + return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n "; + } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << temperatureTimestamp << "," << temperature << "\n"; + } +}; + +struct LPS28DFW_2Data : Boardcore::LPS28DFWData +{ + explicit LPS28DFW_2Data(const Boardcore::LPS28DFWData& data) + : Boardcore::LPS28DFWData(data) + { + } + + LPS28DFW_2Data() {} + + static std::string header() + { + return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n "; + } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << temperatureTimestamp << "," << temperature << "\n"; + } +}; + +struct HSCMRNN015PA_1Data : Boardcore::HSCMRNN015PAData +{ + explicit HSCMRNN015PA_1Data(const Boardcore::HSCMRNN015PAData& data) + : Boardcore::HSCMRNN015PAData(data) + { + } + + HSCMRNN015PA_1Data() {} + + static std::string header() { return "pressureTimestamp,pressure\n "; } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << "\n"; + } +}; + +struct HSCMRNN015PA_2Data : Boardcore::HSCMRNN015PAData +{ + explicit HSCMRNN015PA_2Data(const Boardcore::HSCMRNN015PAData& data) + : Boardcore::HSCMRNN015PAData(data) + { + } + + HSCMRNN015PA_2Data() {} + + static std::string header() { return "pressureTimestamp,pressure\n "; } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << "\n"; + } +}; + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.cpp b/src/boards/Main/StateMachines/ABKController/ABKController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..06f9c0d3cac0e3f502e5053a3ea6760d481e4783 --- /dev/null +++ b/src/boards/Main/StateMachines/ABKController/ABKController.cpp @@ -0,0 +1,219 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/Configs/ABKConfig.h> +#include <Main/StateMachines/ABKController/ABKController.h> +#include <Main/StateMachines/ABKController/TrajectorySet.h> +#include <Main/StateMachines/MEAController/MEAController.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> + +using namespace Boardcore; +using namespace Main::ABKTrajectories; +using namespace Common; + +namespace Main +{ + +ABKController::ABKController(TaskScheduler* sched) + : FSM(&ABKController::state_init), + abk( + []() + { + return TimedTrajectoryPoint(ModuleManager::getInstance() + .get<NASController>() + ->getNasState()); + }, + OPEN_TRAJECTORY_SET, CLOSED_TRAJECTORY_SET, ABKConfig::ABK_CONFIG, + getConfig(), + [](float position) + { + ModuleManager::getInstance().get<Actuators>()->setServoPosition( + ServosList::AIR_BRAKES_SERVO, position); + }), + scheduler(sched) +{ + EventBroker::getInstance().subscribe(this, TOPIC_ABK); + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); +} + +bool ABKController::start() +{ + // Insert the update task into the task scheduler + size_t result = + scheduler->addTask([&]() { update(); }, ABKConfig::UPDATE_PERIOD, + TaskScheduler::Policy::RECOVER); + + return result != 0 && ActiveObject::start(); +} + +void ABKController::update() +{ + // No need for pause kernel due to its presence inside the getter + ABKControllerStatus status = getStatus(); + + if (!abk.isRunning() && status.state == ABKControllerState::ACTIVE) + { + // Begin the algorithm with the last estimated mass + abk.begin(ModuleManager::getInstance() + .get<MEAController>() + ->getStatus() + .estimatedMass); + } + + // Update the algorithm if in Active mode + if (status.state == ABKControllerState::ACTIVE) + { + abk.update(); + } +} + +ABKControllerStatus ABKController::getStatus() +{ + miosix::PauseKernelLock lock; + return status; +} + +void ABKController::state_init(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(ABKControllerState::INIT); + + // Reset the servo position + modules.get<Actuators>()->setServoPosition( + ServosList::AIR_BRAKES_SERVO, 0); + + return transition(&ABKController::state_idle); + } + } +} + +void ABKController::state_idle(const Event& event) +{ + static uint16_t delayTimeoutEventId = 0; + + switch (event) + { + case EV_ENTRY: + { + return logStatus(ABKControllerState::IDLE); + } + case FLIGHT_MOTOR_SHUTDOWN: + { + // Wait a fixed time to start the ABK algorithm, in order to let the + // MEA algorithm estimate correctly the mass + delayTimeoutEventId = EventBroker::getInstance().postDelayed( + ABK_SHADOW_MODE_TIMEOUT, TOPIC_ABK, ABKConfig::DELAY_TIMEOUT); + break; + } + case ABK_SHADOW_MODE_TIMEOUT: + { + return transition(&ABKController::state_active); + } + case FLIGHT_LANDING_DETECTED: + { + return transition(&ABKController::state_end); + } + case EV_EXIT: + { + // Remove the shadow mode event. This works even though the event is + // expired (aka after shadow_mode_timeout) because the event broker + // assigns a progressive number for every delayed event. If and only + // if the number of registered delayed event is less than 2^16, then + // this technique is valid. + return EventBroker::getInstance().removeDelayed( + delayTimeoutEventId); + } + } +} + +void ABKController::state_active(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(ABKControllerState::ACTIVE); + } + case FLIGHT_LANDING_DETECTED: + case FLIGHT_APOGEE_DETECTED: + { + return transition(&ABKController::state_end); + } + } +} + +void ABKController::state_end(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(ABKControllerState::END); + abk.end(); + + // Close the servo + modules.get<Actuators>()->setServoPosition( + ServosList::AIR_BRAKES_SERVO, 0); + + return; + } + } +} + +void ABKController::logStatus(ABKControllerState state) +{ + { + miosix::PauseKernelLock lock; + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + } + + Logger::getInstance().log(status); +} + +AirBrakesInterpConfig ABKController::getConfig() +{ + AirBrakesInterpConfig config; + + config.ABK_CRITICAL_ALTITUDE = ABKConfig::ABK_CRITICAL_ALTITUDE; + config.STARTING_FILTER_VALUE = ABKConfig::STARTING_FILTER_VALUE; + config.FILTER_MAXIMUM_ALTITUDE = ABKConfig::MAXIMUM_ALTITUDE; + config.FILTER_MINIMUM_ALTITUDE = ABKConfig::MINIMUM_ALTITUDE; + config.INITIAL_MASS = ABKConfig::INITIAL_MASS; + config.N_FORWARD = ABKConfig::N_FORWARD; + config.DM = ABKConfig::DM; + config.DZ = ABKConfig::DZ; + + return config; +} + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.h b/src/boards/Main/StateMachines/ABKController/ABKController.h new file mode 100644 index 0000000000000000000000000000000000000000..f1c829578622e7d113eb3e9d88d02d2f792bc692 --- /dev/null +++ b/src/boards/Main/StateMachines/ABKController/ABKController.h @@ -0,0 +1,80 @@ +/* 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 <Main/StateMachines/ABKController/ABKControllerData.h> +#include <algorithms/AirBrakes/AirBrakesInterp.h> +#include <events/FSM.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class ABKController : public Boardcore::Module, + public Boardcore::FSM<ABKController> +{ +public: + ABKController(Boardcore::TaskScheduler* sched); + + /** + * @brief Starts the update task and the FSM + */ + bool start() override; + + /** + * @brief Periodic called by the Task scheduler, updates the ABKInterp + * algorithm + */ + void update(); + + /** + * @brief Returns the ABK FSM status along with the last update timestamp + */ + ABKControllerStatus getStatus(); + + // FSM states + void state_init(const Boardcore::Event& event); + void state_idle(const Boardcore::Event& event); + void state_active(const Boardcore::Event& event); + void state_end(const Boardcore::Event& event); + +private: + /** + * @brief Updates the internal status and logs it + */ + void logStatus(ABKControllerState state); + + /** + * @brief Returns the default config applied to the ABK algorithm + */ + Boardcore::AirBrakesInterpConfig getConfig(); + + Boardcore::AirBrakesInterp abk; + + ABKControllerStatus status; + Boardcore::TaskScheduler* scheduler = nullptr; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ABK"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/ABKController/ABKControllerData.h b/src/boards/Main/StateMachines/ABKController/ABKControllerData.h new file mode 100644 index 0000000000000000000000000000000000000000..eff84f995e33b2e8e9aa07eeac613bce18eaff73 --- /dev/null +++ b/src/boards/Main/StateMachines/ABKController/ABKControllerData.h @@ -0,0 +1,52 @@ +/* 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> + +namespace Main +{ +enum class ABKControllerState : uint8_t +{ + UNINIT = 0, + INIT, + IDLE, + ACTIVE, + END +}; + +struct ABKControllerStatus +{ + uint64_t timestamp = 0; + ABKControllerState state = ABKControllerState::UNINIT; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/ABKController/TrajectorySet.h b/src/boards/Main/StateMachines/ABKController/TrajectorySet.h new file mode 100644 index 0000000000000000000000000000000000000000..f46a3e1857aaa725100074530a4a2e3c305a1d7a --- /dev/null +++ b/src/boards/Main/StateMachines/ABKController/TrajectorySet.h @@ -0,0 +1,2443 @@ +/* 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 <algorithms/AirBrakes/TrajectorySet.h> + +namespace Main +{ +namespace ABKTrajectories +{ +//['Heights ', 'Vz_closed_m26 ', 'Vz_closed_m26_1 ', 'Vz_closed_m26_2 ', +//'Vz_closed_m26_3 ', 'Vz_closed_m26_4 ', 'Vz_closed_m26_5 ', 'Vz_closed_m26_6 +//', 'Vz_closed_m26_7 ', 'Vz_closed_m26_8 ', 'Vz_closed_m26_9 ', 'Vz_closed_m27 +//', 'Vz_open_m26 ', 'Vz_open_m26_1 ', 'Vz_open_m26_2 ', 'Vz_open_m26_3 ', +//'Vz_open_m26_4 ', 'Vz_open_m26_5 ', 'Vz_open_m26_6 ', 'Vz_open_m26_7 ', +//'Vz_open_m26_8 ', 'Vz_open_m26_9 ', 'Vz_open_m27 '] +Boardcore::TrajectoryPoint t0_closed[] = { + Boardcore::TrajectoryPoint(0, 152.281183589541), + Boardcore::TrajectoryPoint(10, 151.46876639657), + Boardcore::TrajectoryPoint(20, 150.654145389855), + Boardcore::TrajectoryPoint(30, 149.836780149099), + Boardcore::TrajectoryPoint(40, 149.016705742141), + Boardcore::TrajectoryPoint(50, 148.194090806251), + Boardcore::TrajectoryPoint(60, 147.368545299231), + Boardcore::TrajectoryPoint(70, 146.540184456041), + Boardcore::TrajectoryPoint(80, 145.709156237422), + Boardcore::TrajectoryPoint(90, 144.874895029495), + Boardcore::TrajectoryPoint(100, 144.037811823631), + Boardcore::TrajectoryPoint(110, 143.197745207749), + Boardcore::TrajectoryPoint(120, 142.354379620129), + Boardcore::TrajectoryPoint(130, 141.508113667175), + Boardcore::TrajectoryPoint(140, 140.65835031699), + Boardcore::TrajectoryPoint(150, 139.805461647332), + Boardcore::TrajectoryPoint(160, 138.949190516325), + Boardcore::TrajectoryPoint(170, 138.089427939597), + Boardcore::TrajectoryPoint(180, 137.226341439041), + Boardcore::TrajectoryPoint(190, 136.359442928428), + Boardcore::TrajectoryPoint(200, 135.489217534117), + Boardcore::TrajectoryPoint(210, 134.614909634047), + Boardcore::TrajectoryPoint(220, 133.737204982698), + Boardcore::TrajectoryPoint(230, 132.855202173581), + Boardcore::TrajectoryPoint(240, 131.969660088597), + Boardcore::TrajectoryPoint(250, 131.079664105217), + Boardcore::TrajectoryPoint(260, 130.185907545998), + Boardcore::TrajectoryPoint(270, 129.287606645828), + Boardcore::TrajectoryPoint(280, 128.385238574269), + Boardcore::TrajectoryPoint(290, 127.478306751639), + Boardcore::TrajectoryPoint(300, 126.566908908799), + Boardcore::TrajectoryPoint(310, 125.651005050447), + Boardcore::TrajectoryPoint(320, 124.730136635695), + Boardcore::TrajectoryPoint(330, 123.804728493886), + Boardcore::TrajectoryPoint(340, 122.874099856926), + Boardcore::TrajectoryPoint(350, 121.938494092651), + Boardcore::TrajectoryPoint(360, 120.997934171153), + Boardcore::TrajectoryPoint(370, 120.051624825784), + Boardcore::TrajectoryPoint(380, 119.100084871254), + Boardcore::TrajectoryPoint(390, 118.14315807336), + Boardcore::TrajectoryPoint(400, 117.180100240481), + Boardcore::TrajectoryPoint(410, 116.211315398257), + Boardcore::TrajectoryPoint(420, 115.236655226956), + Boardcore::TrajectoryPoint(430, 114.255726725382), + Boardcore::TrajectoryPoint(440, 113.268259505295), + Boardcore::TrajectoryPoint(450, 112.274378373159), + Boardcore::TrajectoryPoint(460, 111.273884006837), + Boardcore::TrajectoryPoint(470, 110.266569375787), + Boardcore::TrajectoryPoint(480, 109.251883070934), + Boardcore::TrajectoryPoint(490, 108.229915553916), + Boardcore::TrajectoryPoint(500, 107.200487643897), + Boardcore::TrajectoryPoint(510, 106.163359979054), + Boardcore::TrajectoryPoint(520, 105.118283626543), + Boardcore::TrajectoryPoint(530, 104.064999619219), + Boardcore::TrajectoryPoint(540, 103.003238464434), + Boardcore::TrajectoryPoint(550, 101.932719622999), + Boardcore::TrajectoryPoint(560, 100.853150956205), + Boardcore::TrajectoryPoint(570, 99.7642281386292), + Boardcore::TrajectoryPoint(580, 98.665634034297), + Boardcore::TrajectoryPoint(590, 97.5570380335149), + Boardcore::TrajectoryPoint(600, 96.4380953475082), + Boardcore::TrajectoryPoint(610, 95.3084462577238), + Boardcore::TrajectoryPoint(620, 94.1677153163993), + Boardcore::TrajectoryPoint(630, 93.0155104946976), + Boardcore::TrajectoryPoint(640, 91.8514222743819), + Boardcore::TrajectoryPoint(650, 90.6750226786436), + Boardcore::TrajectoryPoint(660, 89.4856019474907), + Boardcore::TrajectoryPoint(670, 88.282533767652), + Boardcore::TrajectoryPoint(680, 87.0656475005727), + Boardcore::TrajectoryPoint(690, 85.8344235136038), + Boardcore::TrajectoryPoint(700, 84.5874947465647), + Boardcore::TrajectoryPoint(710, 83.324827217275), + Boardcore::TrajectoryPoint(720, 82.0457096278374), + Boardcore::TrajectoryPoint(730, 80.7486749628618), + Boardcore::TrajectoryPoint(740, 79.4338607615884), + Boardcore::TrajectoryPoint(750, 78.0991665033548), + Boardcore::TrajectoryPoint(760, 76.7446619169102), + Boardcore::TrajectoryPoint(770, 75.3685866225035), + Boardcore::TrajectoryPoint(780, 73.9698278034401), + Boardcore::TrajectoryPoint(790, 72.5474663089349), + Boardcore::TrajectoryPoint(800, 71.0998994747218), + Boardcore::TrajectoryPoint(810, 69.6249411642998), + Boardcore::TrajectoryPoint(820, 68.1214404232699), + Boardcore::TrajectoryPoint(830, 66.5873975730555), + Boardcore::TrajectoryPoint(840, 65.0206274307197), + Boardcore::TrajectoryPoint(850, 63.4187375411119), + Boardcore::TrajectoryPoint(860, 61.7788913095777), + Boardcore::TrajectoryPoint(870, 60.0974916035063), + Boardcore::TrajectoryPoint(880, 58.3718398588616), + Boardcore::TrajectoryPoint(890, 56.597541226983), + Boardcore::TrajectoryPoint(900, 54.7696610514191), + Boardcore::TrajectoryPoint(910, 52.8826329147933), + Boardcore::TrajectoryPoint(920, 50.9302854325946), + Boardcore::TrajectoryPoint(930, 48.9044877152189), + Boardcore::TrajectoryPoint(940, 46.7957255802411), + Boardcore::TrajectoryPoint(950, 44.592796083963), + Boardcore::TrajectoryPoint(960, 42.2794707199437), + Boardcore::TrajectoryPoint(970, 39.8381425902815), + Boardcore::TrajectoryPoint(980, 37.2430292543444), + Boardcore::TrajectoryPoint(990, 34.4599032639387), + Boardcore::TrajectoryPoint(1000, 31.4379167461659), + Boardcore::TrajectoryPoint(1010, 28.1016053399742), + Boardcore::TrajectoryPoint(1020, 24.3197095473458), + Boardcore::TrajectoryPoint(1030, 19.8426201220378), + Boardcore::TrajectoryPoint(1040, 14.0185018340729), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t1_closed[] = { + Boardcore::TrajectoryPoint(0, 152.245755350089), + Boardcore::TrajectoryPoint(10, 151.433869072696), + Boardcore::TrajectoryPoint(20, 150.619758851732), + Boardcore::TrajectoryPoint(30, 149.802933721734), + Boardcore::TrajectoryPoint(40, 148.983362845367), + Boardcore::TrajectoryPoint(50, 148.161248211031), + Boardcore::TrajectoryPoint(60, 147.336231679034), + Boardcore::TrajectoryPoint(70, 146.508363917621), + Boardcore::TrajectoryPoint(80, 145.677825579755), + Boardcore::TrajectoryPoint(90, 144.844082149046), + Boardcore::TrajectoryPoint(100, 144.007481641361), + Boardcore::TrajectoryPoint(110, 143.167925542373), + Boardcore::TrajectoryPoint(120, 142.325035490906), + Boardcore::TrajectoryPoint(130, 141.479241919132), + Boardcore::TrajectoryPoint(140, 140.629977782493), + Boardcore::TrajectoryPoint(150, 139.777554371874), + Boardcore::TrajectoryPoint(160, 138.921775088473), + Boardcore::TrajectoryPoint(170, 138.062470671407), + Boardcore::TrajectoryPoint(180, 137.199868605701), + Boardcore::TrajectoryPoint(190, 136.333421179004), + Boardcore::TrajectoryPoint(200, 135.463672764336), + Boardcore::TrajectoryPoint(210, 134.589808896816), + Boardcore::TrajectoryPoint(220, 133.712554201462), + Boardcore::TrajectoryPoint(230, 132.831007928586), + Boardcore::TrajectoryPoint(240, 131.945927801359), + Boardcore::TrajectoryPoint(250, 131.056361823404), + Boardcore::TrajectoryPoint(260, 130.163059662486), + Boardcore::TrajectoryPoint(270, 129.265181792979), + Boardcore::TrajectoryPoint(280, 128.363260532298), + Boardcore::TrajectoryPoint(290, 127.45674479197), + Boardcore::TrajectoryPoint(300, 126.5457861476), + Boardcore::TrajectoryPoint(310, 125.630291449902), + Boardcore::TrajectoryPoint(320, 124.709854598997), + Boardcore::TrajectoryPoint(330, 123.784874274836), + Boardcore::TrajectoryPoint(340, 122.854643995785), + Boardcore::TrajectoryPoint(350, 121.919458367271), + Boardcore::TrajectoryPoint(360, 120.97928994655), + Boardcore::TrajectoryPoint(370, 120.033393039748), + Boardcore::TrajectoryPoint(380, 119.0822616981), + Boardcore::TrajectoryPoint(390, 118.125739662359), + Boardcore::TrajectoryPoint(400, 117.163058733959), + Boardcore::TrajectoryPoint(410, 116.194670899962), + Boardcore::TrajectoryPoint(420, 115.22040384241), + Boardcore::TrajectoryPoint(430, 114.239841598892), + Boardcore::TrajectoryPoint(440, 113.252759712339), + Boardcore::TrajectoryPoint(450, 112.259259980483), + Boardcore::TrajectoryPoint(460, 111.259143061607), + Boardcore::TrajectoryPoint(470, 110.252201906462), + Boardcore::TrajectoryPoint(480, 109.237863577153), + Boardcore::TrajectoryPoint(490, 108.216261706941), + Boardcore::TrajectoryPoint(500, 107.18719544172), + Boardcore::TrajectoryPoint(510, 106.150425403584), + Boardcore::TrajectoryPoint(520, 105.105702644359), + Boardcore::TrajectoryPoint(530, 104.052768182294), + Boardcore::TrajectoryPoint(540, 102.991352510841), + Boardcore::TrajectoryPoint(550, 101.921175077596), + Boardcore::TrajectoryPoint(560, 100.841943731293), + Boardcore::TrajectoryPoint(570, 99.7533541346046), + Boardcore::TrajectoryPoint(580, 98.6550891402744), + Boardcore::TrajectoryPoint(590, 97.5468181279407), + Boardcore::TrajectoryPoint(600, 96.4281962987566), + Boardcore::TrajectoryPoint(610, 95.298863924679), + Boardcore::TrajectoryPoint(620, 94.1584455490239), + Boardcore::TrajectoryPoint(630, 93.006549134589), + Boardcore::TrajectoryPoint(640, 91.842765155316), + Boardcore::TrajectoryPoint(650, 90.6666656271074), + Boardcore::TrajectoryPoint(660, 89.4775565308792), + Boardcore::TrajectoryPoint(670, 88.2747795855256), + Boardcore::TrajectoryPoint(680, 87.0581803478825), + Boardcore::TrajectoryPoint(690, 85.8272391803842), + Boardcore::TrajectoryPoint(700, 84.5806033657714), + Boardcore::TrajectoryPoint(710, 83.3182096973134), + Boardcore::TrajectoryPoint(720, 82.0393755312979), + Boardcore::TrajectoryPoint(730, 80.7426057000813), + Boardcore::TrajectoryPoint(740, 79.4280652371814), + Boardcore::TrajectoryPoint(750, 78.0936267256959), + Boardcore::TrajectoryPoint(760, 76.7393860502793), + Boardcore::TrajectoryPoint(770, 75.3635573637401), + Boardcore::TrajectoryPoint(780, 73.9650524991158), + Boardcore::TrajectoryPoint(790, 72.5429398221279), + Boardcore::TrajectoryPoint(800, 71.095605476199), + Boardcore::TrajectoryPoint(810, 69.6208858323125), + Boardcore::TrajectoryPoint(820, 68.1176184924483), + Boardcore::TrajectoryPoint(830, 66.583803729791), + Boardcore::TrajectoryPoint(840, 65.0172563157423), + Boardcore::TrajectoryPoint(850, 63.4155837519961), + Boardcore::TrajectoryPoint(860, 61.7759584045937), + Boardcore::TrajectoryPoint(870, 60.0947646527577), + Boardcore::TrajectoryPoint(880, 58.3693133212936), + Boardcore::TrajectoryPoint(890, 56.595217401955), + Boardcore::TrajectoryPoint(900, 54.7675259349765), + Boardcore::TrajectoryPoint(910, 52.8806879096757), + Boardcore::TrajectoryPoint(920, 50.9285239468375), + Boardcore::TrajectoryPoint(930, 48.9029030431781), + Boardcore::TrajectoryPoint(940, 46.794310910424), + Boardcore::TrajectoryPoint(950, 44.5915445070942), + Boardcore::TrajectoryPoint(960, 42.2783803832867), + Boardcore::TrajectoryPoint(970, 39.8372054912072), + Boardcore::TrajectoryPoint(980, 37.2422371834903), + Boardcore::TrajectoryPoint(990, 34.4592517906086), + Boardcore::TrajectoryPoint(1000, 31.4373963163521), + Boardcore::TrajectoryPoint(1010, 28.1012061102367), + Boardcore::TrajectoryPoint(1020, 24.3194264604742), + Boardcore::TrajectoryPoint(1030, 19.842442589616), + Boardcore::TrajectoryPoint(1040, 14.0184197802997), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t2_closed[] = { + Boardcore::TrajectoryPoint(0, 152.210609519524), + Boardcore::TrajectoryPoint(10, 151.399251019435), + Boardcore::TrajectoryPoint(20, 150.585647363659), + Boardcore::TrajectoryPoint(30, 149.769357969649), + Boardcore::TrajectoryPoint(40, 148.950286467789), + Boardcore::TrajectoryPoint(50, 148.128668008159), + Boardcore::TrajectoryPoint(60, 147.304176169823), + Boardcore::TrajectoryPoint(70, 146.476797427427), + Boardcore::TrajectoryPoint(80, 145.646744936456), + Boardcore::TrajectoryPoint(90, 144.813515095859), + Boardcore::TrajectoryPoint(100, 143.977393316162), + Boardcore::TrajectoryPoint(110, 143.138343607042), + Boardcore::TrajectoryPoint(120, 142.295925184969), + Boardcore::TrajectoryPoint(130, 141.450600115973), + Boardcore::TrajectoryPoint(140, 140.601831161473), + Boardcore::TrajectoryPoint(150, 139.74986919445), + Boardcore::TrajectoryPoint(160, 138.894577788639), + Boardcore::TrajectoryPoint(170, 138.035727778478), + Boardcore::TrajectoryPoint(180, 137.173606239555), + Boardcore::TrajectoryPoint(190, 136.307606206417), + Boardcore::TrajectoryPoint(200, 135.438330589448), + Boardcore::TrajectoryPoint(210, 134.56490746254), + Boardcore::TrajectoryPoint(220, 133.688090125555), + Boardcore::TrajectoryPoint(230, 132.807005637333), + Boardcore::TrajectoryPoint(240, 131.922356150717), + Boardcore::TrajectoryPoint(250, 131.033244270852), + Boardcore::TrajectoryPoint(260, 130.140392852358), + Boardcore::TrajectoryPoint(270, 129.242934569645), + Boardcore::TrajectoryPoint(280, 128.341456527848), + Boardcore::TrajectoryPoint(290, 127.435353486752), + Boardcore::TrajectoryPoint(300, 126.524830512989), + Boardcore::TrajectoryPoint(310, 125.609741653331), + Boardcore::TrajectoryPoint(320, 124.689732902745), + Boardcore::TrajectoryPoint(330, 123.765176963417), + Boardcore::TrajectoryPoint(340, 122.835341813559), + Boardcore::TrajectoryPoint(350, 121.900572952773), + Boardcore::TrajectoryPoint(360, 120.960792863729), + Boardcore::TrajectoryPoint(370, 120.015305092351), + Boardcore::TrajectoryPoint(380, 119.064579091961), + Boardcore::TrajectoryPoint(390, 118.108435543303), + Boardcore::TrajectoryPoint(400, 117.146151511612), + Boardcore::TrajectoryPoint(410, 116.178157511366), + Boardcore::TrajectoryPoint(420, 115.204280425278), + Boardcore::TrajectoryPoint(430, 114.224081488583), + Boardcore::TrajectoryPoint(440, 113.237381858813), + Boardcore::TrajectoryPoint(450, 112.244260483028), + Boardcore::TrajectoryPoint(460, 111.244518000119), + Boardcore::TrajectoryPoint(470, 110.237947342303), + Boardcore::TrajectoryPoint(480, 109.223954194018), + Boardcore::TrajectoryPoint(490, 108.202715057859), + Boardcore::TrajectoryPoint(500, 107.174007557828), + Boardcore::TrajectoryPoint(510, 106.137592300079), + Boardcore::TrajectoryPoint(520, 105.093220321237), + Boardcore::TrajectoryPoint(530, 104.040632625069), + Boardcore::TrajectoryPoint(540, 102.97955969124), + Boardcore::TrajectoryPoint(550, 101.909720954233), + Boardcore::TrajectoryPoint(560, 100.830824250328), + Boardcore::TrajectoryPoint(570, 99.7425652303751), + Boardcore::TrajectoryPoint(580, 98.6446267359209), + Boardcore::TrajectoryPoint(590, 97.5366781360087), + Boardcore::TrajectoryPoint(600, 96.4183746217864), + Boardcore::TrajectoryPoint(610, 95.2893564557807), + Boardcore::TrajectoryPoint(620, 94.1492481724396), + Boardcore::TrajectoryPoint(630, 92.9976577262422), + Boardcore::TrajectoryPoint(640, 91.8341755833496), + Boardcore::TrajectoryPoint(650, 90.65837375241), + Boardcore::TrajectoryPoint(660, 89.4695738404114), + Boardcore::TrajectoryPoint(670, 88.2670858315855), + Boardcore::TrajectoryPoint(680, 87.0507713600094), + Boardcore::TrajectoryPoint(690, 85.8201107832082), + Boardcore::TrajectoryPoint(700, 84.5737656215671), + Boardcore::TrajectoryPoint(710, 83.3116436583328), + Boardcore::TrajectoryPoint(720, 82.0330906930786), + Boardcore::TrajectoryPoint(730, 80.7365836136397), + Boardcore::TrajectoryPoint(740, 79.4223147445155), + Boardcore::TrajectoryPoint(750, 78.0881299717913), + Boardcore::TrajectoryPoint(760, 76.7341511419843), + Boardcore::TrajectoryPoint(770, 75.3585671296012), + Boardcore::TrajectoryPoint(780, 73.9603142341499), + Boardcore::TrajectoryPoint(790, 72.5384484305309), + Boardcore::TrajectoryPoint(800, 71.091344753525), + Boardcore::TrajectoryPoint(810, 69.6168619135995), + Boardcore::TrajectoryPoint(820, 68.1138261544216), + Boardcore::TrajectoryPoint(830, 66.580237701304), + Boardcore::TrajectoryPoint(840, 65.0139112803265), + Boardcore::TrajectoryPoint(850, 63.4124543503412), + Boardcore::TrajectoryPoint(860, 61.7730481705832), + Boardcore::TrajectoryPoint(870, 60.0920587713138), + Boardcore::TrajectoryPoint(880, 58.3668062954703), + Boardcore::TrajectoryPoint(890, 56.5929115159511), + Boardcore::TrajectoryPoint(900, 54.7654072928502), + Boardcore::TrajectoryPoint(910, 52.8787579056344), + Boardcore::TrajectoryPoint(920, 50.926776040805), + Boardcore::TrajectoryPoint(930, 48.9013305822642), + Boardcore::TrajectoryPoint(940, 46.7929071366803), + Boardcore::TrajectoryPoint(950, 44.5903025655055), + Boardcore::TrajectoryPoint(960, 42.2772984368827), + Boardcore::TrajectoryPoint(970, 39.8362755998336), + Boardcore::TrajectoryPoint(980, 37.2414512018354), + Boardcore::TrajectoryPoint(990, 34.4586053231666), + Boardcore::TrajectoryPoint(1000, 31.4368798833573), + Boardcore::TrajectoryPoint(1010, 28.100809944689), + Boardcore::TrajectoryPoint(1020, 24.3191455450002), + Boardcore::TrajectoryPoint(1030, 19.8422664178905), + Boardcore::TrajectoryPoint(1040, 14.0183383547549), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t3_closed[] = { + Boardcore::TrajectoryPoint(0, 152.175742743635), + Boardcore::TrajectoryPoint(10, 151.364908902743), + Boardcore::TrajectoryPoint(20, 150.551807643526), + Boardcore::TrajectoryPoint(30, 149.736049663359), + Boardcore::TrajectoryPoint(40, 148.917473431018), + Boardcore::TrajectoryPoint(50, 148.096347069949), + Boardcore::TrajectoryPoint(60, 147.272375695397), + Boardcore::TrajectoryPoint(70, 146.445481959129), + Boardcore::TrajectoryPoint(80, 145.615911330684), + Boardcore::TrajectoryPoint(90, 144.783190943419), + Boardcore::TrajectoryPoint(100, 143.947543970178), + Boardcore::TrajectoryPoint(110, 143.108996573506), + Boardcore::TrajectoryPoint(120, 142.267045921899), + Boardcore::TrajectoryPoint(130, 141.422185524735), + Boardcore::TrajectoryPoint(140, 140.573907769392), + Boardcore::TrajectoryPoint(150, 139.722403477161), + Boardcore::TrajectoryPoint(160, 138.867596026598), + Boardcore::TrajectoryPoint(170, 138.00919671641), + Boardcore::TrajectoryPoint(180, 137.147551843121), + Boardcore::TrajectoryPoint(190, 136.281995558196), + Boardcore::TrajectoryPoint(200, 135.413160653025), + Boardcore::TrajectoryPoint(210, 134.540202969109), + Boardcore::TrajectoryPoint(220, 133.66381943345), + Boardcore::TrajectoryPoint(230, 132.783193026494), + Boardcore::TrajectoryPoint(240, 131.898970455045), + Boardcore::TrajectoryPoint(250, 131.010309261439), + Boardcore::TrajectoryPoint(260, 130.117904973303), + Boardcore::TrajectoryPoint(270, 129.22086287533), + Boardcore::TrajectoryPoint(280, 128.319824503446), + Boardcore::TrajectoryPoint(290, 127.414130819542), + Boardcore::TrajectoryPoint(300, 126.504040030746), + Boardcore::TrajectoryPoint(310, 125.589353726765), + Boardcore::TrajectoryPoint(320, 124.669769654394), + Boardcore::TrajectoryPoint(330, 123.745634708138), + Boardcore::TrajectoryPoint(340, 122.816191497802), + Boardcore::TrajectoryPoint(350, 121.88183607696), + Boardcore::TrajectoryPoint(360, 120.942441188767), + Boardcore::TrajectoryPoint(370, 119.997359289112), + Boardcore::TrajectoryPoint(380, 119.047035397413), + Boardcore::TrajectoryPoint(390, 118.091266519636), + Boardcore::TrajectoryPoint(400, 117.12937699335), + Boardcore::TrajectoryPoint(410, 116.161773690232), + Boardcore::TrajectoryPoint(420, 115.188283470785), + Boardcore::TrajectoryPoint(430, 114.208444925174), + Boardcore::TrajectoryPoint(440, 113.222124512079), + Boardcore::TrajectoryPoint(450, 112.229378484405), + Boardcore::TrajectoryPoint(460, 111.230007461829), + Boardcore::TrajectoryPoint(470, 110.223790888272), + Boardcore::TrajectoryPoint(480, 109.210153629929), + Boardcore::TrajectoryPoint(490, 108.189274349688), + Boardcore::TrajectoryPoint(500, 107.160922769454), + Boardcore::TrajectoryPoint(510, 106.124859479576), + Boardcore::TrajectoryPoint(520, 105.080835501611), + Boardcore::TrajectoryPoint(530, 104.02859182496), + Boardcore::TrajectoryPoint(540, 102.967858915615), + Boardcore::TrajectoryPoint(550, 101.89835619505), + Boardcore::TrajectoryPoint(560, 100.819791487184), + Boardcore::TrajectoryPoint(570, 99.7318604311362), + Boardcore::TrajectoryPoint(580, 98.6342458573339), + Boardcore::TrajectoryPoint(590, 97.5266171242993), + Boardcore::TrajectoryPoint(600, 96.4086294132409), + Boardcore::TrajectoryPoint(610, 95.279922977315), + Boardcore::TrajectoryPoint(620, 94.1401223421543), + Boardcore::TrajectoryPoint(630, 92.9888354539658), + Boardcore::TrajectoryPoint(640, 91.8256527711702), + Boardcore::TrajectoryPoint(650, 90.6501462951963), + Boardcore::TrajectoryPoint(660, 89.4616531455046), + Boardcore::TrajectoryPoint(670, 88.2594518023261), + Boardcore::TrajectoryPoint(680, 87.043419860101), + Boardcore::TrajectoryPoint(690, 85.813037671454), + Boardcore::TrajectoryPoint(700, 84.5669808902813), + Boardcore::TrajectoryPoint(710, 83.3051285020036), + Boardcore::TrajectoryPoint(720, 82.0268545408799), + Boardcore::TrajectoryPoint(730, 80.730608155684), + Boardcore::TrajectoryPoint(740, 79.4166087608309), + Boardcore::TrajectoryPoint(750, 78.0826757424302), + Boardcore::TrajectoryPoint(760, 76.7289567169571), + Boardcore::TrajectoryPoint(770, 75.3536154676685), + Boardcore::TrajectoryPoint(780, 73.955612579305), + Boardcore::TrajectoryPoint(790, 72.5339917275971), + Boardcore::TrajectoryPoint(800, 71.087116921422), + Boardcore::TrajectoryPoint(810, 69.6128690445966), + Boardcore::TrajectoryPoint(820, 68.1100630668372), + Boardcore::TrajectoryPoint(830, 66.5766991659472), + Boardcore::TrajectoryPoint(840, 65.0105920230206), + Boardcore::TrajectoryPoint(850, 63.4093490543778), + Boardcore::TrajectoryPoint(860, 61.7701603457035), + Boardcore::TrajectoryPoint(870, 60.0893737159412), + Boardcore::TrajectoryPoint(880, 58.3643185562435), + Boardcore::TrajectoryPoint(890, 56.5906233620528), + Boardcore::TrajectoryPoint(900, 54.7633049351075), + Boardcore::TrajectoryPoint(910, 52.8768427297941), + Boardcore::TrajectoryPoint(920, 50.9250415580696), + Boardcore::TrajectoryPoint(930, 48.8997701918773), + Boardcore::TrajectoryPoint(940, 46.7915141336095), + Boardcore::TrajectoryPoint(950, 44.5890701483595), + Boardcore::TrajectoryPoint(960, 42.2762247842582), + Boardcore::TrajectoryPoint(970, 39.8353528333233), + Boardcore::TrajectoryPoint(980, 37.2406712394317), + Boardcore::TrajectoryPoint(990, 34.457963804137), + Boardcore::TrajectoryPoint(1000, 31.436367401316), + Boardcore::TrajectoryPoint(1010, 28.1004168081889), + Boardcore::TrajectoryPoint(1020, 24.3188667760365), + Boardcore::TrajectoryPoint(1030, 19.8420915912781), + Boardcore::TrajectoryPoint(1040, 14.0182575502518), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t4_closed[] = { + Boardcore::TrajectoryPoint(0, 152.141151720961), + Boardcore::TrajectoryPoint(10, 151.330839441355), + Boardcore::TrajectoryPoint(20, 150.518236461159), + Boardcore::TrajectoryPoint(30, 149.703005624475), + Boardcore::TrajectoryPoint(40, 148.884920606928), + Boardcore::TrajectoryPoint(50, 148.064282318157), + Boardcore::TrajectoryPoint(60, 147.240827228178), + Boardcore::TrajectoryPoint(70, 146.414414534208), + Boardcore::TrajectoryPoint(80, 145.585321832603), + Boardcore::TrajectoryPoint(90, 144.753106811422), + Boardcore::TrajectoryPoint(100, 143.917930770972), + Boardcore::TrajectoryPoint(110, 143.079881658144), + Boardcore::TrajectoryPoint(120, 142.238394965134), + Boardcore::TrajectoryPoint(130, 141.393995455544), + Boardcore::TrajectoryPoint(140, 140.546204964029), + Boardcore::TrajectoryPoint(150, 139.695154623667), + Boardcore::TrajectoryPoint(160, 138.840827252932), + Boardcore::TrajectoryPoint(170, 137.982874980864), + Boardcore::TrajectoryPoint(180, 137.121702958234), + Boardcore::TrajectoryPoint(190, 136.256586820462), + Boardcore::TrajectoryPoint(200, 135.388189052897), + Boardcore::TrajectoryPoint(210, 134.515693091553), + Boardcore::TrajectoryPoint(220, 133.639739843317), + Boardcore::TrajectoryPoint(230, 132.759567858465), + Boardcore::TrajectoryPoint(240, 131.875768748267), + Boardcore::TrajectoryPoint(250, 130.987554643371), + Boardcore::TrajectoryPoint(260, 130.095593916644), + Boardcore::TrajectoryPoint(270, 129.198964642501), + Boardcore::TrajectoryPoint(280, 128.298362433894), + Boardcore::TrajectoryPoint(290, 127.393074805514), + Boardcore::TrajectoryPoint(300, 126.483412757601), + Boardcore::TrajectoryPoint(310, 125.569125766536), + Boardcore::TrajectoryPoint(320, 124.649962991044), + Boardcore::TrajectoryPoint(330, 123.726245686504), + Boardcore::TrajectoryPoint(340, 122.797191264436), + Boardcore::TrajectoryPoint(350, 121.863245995363), + Boardcore::TrajectoryPoint(360, 120.924233214861), + Boardcore::TrajectoryPoint(370, 119.979553962042), + Boardcore::TrajectoryPoint(380, 119.029628984907), + Boardcore::TrajectoryPoint(390, 118.074231775202), + Boardcore::TrajectoryPoint(400, 117.112733623761), + Boardcore::TrajectoryPoint(410, 116.145517918404), + Boardcore::TrajectoryPoint(420, 115.172411497645), + Boardcore::TrajectoryPoint(430, 114.192930462305), + Boardcore::TrajectoryPoint(440, 113.206986261844), + Boardcore::TrajectoryPoint(450, 112.214612609992), + Boardcore::TrajectoryPoint(460, 111.215610107394), + Boardcore::TrajectoryPoint(470, 110.209736972506), + Boardcore::TrajectoryPoint(480, 109.196460613393), + Boardcore::TrajectoryPoint(490, 108.175938345013), + Boardcore::TrajectoryPoint(500, 107.147939872857), + Boardcore::TrajectoryPoint(510, 106.112225771609), + Boardcore::TrajectoryPoint(520, 105.06854704788), + Boardcore::TrajectoryPoint(530, 104.016644676831), + Boardcore::TrajectoryPoint(540, 102.956249110886), + Boardcore::TrajectoryPoint(550, 101.887079758615), + Boardcore::TrajectoryPoint(560, 100.808844431669), + Boardcore::TrajectoryPoint(570, 99.7212387575233), + Boardcore::TrajectoryPoint(580, 98.6239455555655), + Boardcore::TrajectoryPoint(590, 97.5166341738686), + Boardcore::TrajectoryPoint(600, 96.3989597837677), + Boardcore::TrajectoryPoint(610, 95.2705626291071), + Boardcore::TrajectoryPoint(620, 94.1310672267567), + Boardcore::TrajectoryPoint(630, 92.9800815146977), + Boardcore::TrajectoryPoint(640, 91.8171959436505), + Boardcore::TrajectoryPoint(650, 90.6419825078582), + Boardcore::TrajectoryPoint(660, 89.4537937268754), + Boardcore::TrajectoryPoint(670, 88.2518768051171), + Boardcore::TrajectoryPoint(680, 87.0361251817641), + Boardcore::TrajectoryPoint(690, 85.806019204549), + Boardcore::TrajectoryPoint(700, 84.5602485578731), + Boardcore::TrajectoryPoint(710, 83.2986636392305), + Boardcore::TrajectoryPoint(720, 82.0206665112317), + Boardcore::TrajectoryPoint(730, 80.7246787868096), + Boardcore::TrajectoryPoint(740, 79.4109467714264), + Boardcore::TrajectoryPoint(750, 78.0772635460938), + Boardcore::TrajectoryPoint(760, 76.723802307447), + Boardcore::TrajectoryPoint(770, 75.3487019324884), + Boardcore::TrajectoryPoint(780, 73.9509471119494), + Boardcore::TrajectoryPoint(790, 72.5295693130338), + Boardcore::TrajectoryPoint(800, 71.0829216005362), + Boardcore::TrajectoryPoint(810, 69.608906867327), + Boardcore::TrajectoryPoint(820, 68.1063288926016), + Boardcore::TrajectoryPoint(830, 66.5731878070125), + Boardcore::TrajectoryPoint(840, 65.0072982469998), + Boardcore::TrajectoryPoint(850, 63.4062675866591), + Boardcore::TrajectoryPoint(860, 61.7672946721279), + Boardcore::TrajectoryPoint(870, 60.0867092471352), + Boardcore::TrajectoryPoint(880, 58.3618498819148), + Boardcore::TrajectoryPoint(890, 56.5883527365107), + Boardcore::TrajectoryPoint(900, 54.7612186747232), + Boardcore::TrajectoryPoint(910, 52.8749422119245), + Boardcore::TrajectoryPoint(920, 50.9233203445957), + Boardcore::TrajectoryPoint(930, 48.8982217335666), + Boardcore::TrajectoryPoint(940, 46.7901317777266), + Boardcore::TrajectoryPoint(950, 44.5878471465108), + Boardcore::TrajectoryPoint(960, 42.2751593304123), + Boardcore::TrajectoryPoint(970, 39.8344371101021), + Boardcore::TrajectoryPoint(980, 37.239897227397), + Boardcore::TrajectoryPoint(990, 34.4573271769189), + Boardcore::TrajectoryPoint(1000, 31.4358588250603), + Boardcore::TrajectoryPoint(1010, 28.1000266661281), + Boardcore::TrajectoryPoint(1020, 24.3185901290729), + Boardcore::TrajectoryPoint(1030, 19.8419180944302), + Boardcore::TrajectoryPoint(1040, 14.0181773597103), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t5_closed[] = { + Boardcore::TrajectoryPoint(0, 152.106833201762), + Boardcore::TrajectoryPoint(10, 151.297039405745), + Boardcore::TrajectoryPoint(20, 150.48493063729), + Boardcore::TrajectoryPoint(30, 149.670222724698), + Boardcore::TrajectoryPoint(40, 148.852624916669), + Boardcore::TrajectoryPoint(50, 148.032470723006), + Boardcore::TrajectoryPoint(60, 147.209527788255), + Boardcore::TrajectoryPoint(70, 146.38359222102), + Boardcore::TrajectoryPoint(80, 145.554973558463), + Boardcore::TrajectoryPoint(90, 144.723259864863), + Boardcore::TrajectoryPoint(100, 143.888550930633), + Boardcore::TrajectoryPoint(110, 143.050996121095), + Boardcore::TrajectoryPoint(120, 142.209969621106), + Boardcore::TrajectoryPoint(130, 141.366027260764), + Boardcore::TrajectoryPoint(140, 140.51872014465), + Boardcore::TrajectoryPoint(150, 139.668120078376), + Boardcore::TrajectoryPoint(160, 138.814268958225), + Boardcore::TrajectoryPoint(170, 137.95676010678), + Boardcore::TrajectoryPoint(180, 137.096057165278), + Boardcore::TrajectoryPoint(190, 136.23137761717), + Boardcore::TrajectoryPoint(200, 135.363413456239), + Boardcore::TrajectoryPoint(210, 134.491375541323), + Boardcore::TrajectoryPoint(220, 133.61584910905), + Boardcore::TrajectoryPoint(230, 132.736127930665), + Boardcore::TrajectoryPoint(240, 131.852748869473), + Boardcore::TrajectoryPoint(250, 130.964978298512), + Boardcore::TrajectoryPoint(260, 130.073443228631), + Boardcore::TrajectoryPoint(270, 129.177237835939), + Boardcore::TrajectoryPoint(280, 128.277068325644), + Boardcore::TrajectoryPoint(290, 127.372183490842), + Boardcore::TrajectoryPoint(300, 126.462946780625), + Boardcore::TrajectoryPoint(310, 125.549055898689), + Boardcore::TrajectoryPoint(320, 124.630311078864), + Boardcore::TrajectoryPoint(330, 123.707008104449), + Boardcore::TrajectoryPoint(340, 122.778339357199), + Boardcore::TrajectoryPoint(350, 121.844800990708), + Boardcore::TrajectoryPoint(360, 120.906167261799), + Boardcore::TrajectoryPoint(370, 119.961887469133), + Boardcore::TrajectoryPoint(380, 119.012358250267), + Boardcore::TrajectoryPoint(390, 118.057329741742), + Boardcore::TrajectoryPoint(400, 117.096219871632), + Boardcore::TrajectoryPoint(410, 116.12938870134), + Boardcore::TrajectoryPoint(420, 115.156663047603), + Boardcore::TrajectoryPoint(430, 114.177536676095), + Boardcore::TrajectoryPoint(440, 113.191965719725), + Boardcore::TrajectoryPoint(450, 112.199961506518), + Boardcore::TrajectoryPoint(460, 111.201324618268), + Boardcore::TrajectoryPoint(470, 110.195792194847), + Boardcore::TrajectoryPoint(480, 109.182873892645), + Boardcore::TrajectoryPoint(490, 108.162705825608), + Boardcore::TrajectoryPoint(500, 107.135057682957), + Boardcore::TrajectoryPoint(510, 106.099690023847), + Boardcore::TrajectoryPoint(520, 105.056353840069), + Boardcore::TrajectoryPoint(530, 104.004790092659), + Boardcore::TrajectoryPoint(540, 102.944729220586), + Boardcore::TrajectoryPoint(550, 101.875890619612), + Boardcore::TrajectoryPoint(560, 100.797982089216), + Boardcore::TrajectoryPoint(570, 99.7106992453148), + Boardcore::TrajectoryPoint(580, 98.6137248963344), + Boardcore::TrajectoryPoint(590, 97.5067283799706), + Boardcore::TrajectoryPoint(600, 96.3893648577493), + Boardcore::TrajectoryPoint(610, 95.2612745642613), + Boardcore::TrajectoryPoint(620, 94.1220820076657), + Boardcore::TrajectoryPoint(630, 92.9713951177634), + Boardcore::TrajectoryPoint(640, 91.8088043376144), + Boardcore::TrajectoryPoint(650, 90.6338816543104), + Boardcore::TrajectoryPoint(660, 89.4459948763227), + Boardcore::TrajectoryPoint(670, 88.2443601579958), + Boardcore::TrajectoryPoint(680, 87.0288866688643), + Boardcore::TrajectoryPoint(690, 85.7990547517776), + Boardcore::TrajectoryPoint(700, 84.5535680197478), + Boardcore::TrajectoryPoint(710, 83.2922484899762), + Boardcore::TrajectoryPoint(720, 82.0145260493254), + Boardcore::TrajectoryPoint(730, 80.7187949758998), + Boardcore::TrajectoryPoint(740, 79.4053282695061), + Boardcore::TrajectoryPoint(750, 78.071892898809), + Boardcore::TrajectoryPoint(760, 76.7186874528815), + Boardcore::TrajectoryPoint(770, 75.3438260854402), + Boardcore::TrajectoryPoint(780, 73.9463174159319), + Boardcore::TrajectoryPoint(790, 72.5251807926841), + Boardcore::TrajectoryPoint(800, 71.0787584173257), + Boardcore::TrajectoryPoint(810, 69.6049750292964), + Boardcore::TrajectoryPoint(820, 68.1026232997819), + Boardcore::TrajectoryPoint(830, 66.5697033126382), + Boardcore::TrajectoryPoint(840, 65.0040296599796), + Boardcore::TrajectoryPoint(850, 63.4032096739809), + Boardcore::TrajectoryPoint(860, 61.7644508959707), + Boardcore::TrajectoryPoint(870, 60.0840651290504), + Boardcore::TrajectoryPoint(880, 58.3594000541716), + Boardcore::TrajectoryPoint(890, 56.5860994386859), + Boardcore::TrajectoryPoint(900, 54.7591483275259), + Boardcore::TrajectoryPoint(910, 52.873056184392), + Boardcore::TrajectoryPoint(920, 50.9216122486964), + Boardcore::TrajectoryPoint(930, 48.8966850709917), + Boardcore::TrajectoryPoint(940, 46.7887599474286), + Boardcore::TrajectoryPoint(950, 44.5866334524767), + Boardcore::TrajectoryPoint(960, 42.2741019817903), + Boardcore::TrajectoryPoint(970, 39.8335283498377), + Boardcore::TrajectoryPoint(980, 37.2391290978975), + Boardcore::TrajectoryPoint(990, 34.4566953857729), + Boardcore::TrajectoryPoint(1000, 31.4353541101093), + Boardcore::TrajectoryPoint(1010, 28.0996394844248), + Boardcore::TrajectoryPoint(1020, 24.3183155799728), + Boardcore::TrajectoryPoint(1030, 19.8417459122337), + Boardcore::TrajectoryPoint(1040, 14.0180977761605), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t6_closed[] = { + Boardcore::TrajectoryPoint(0, 152.072783987011), + Boardcore::TrajectoryPoint(10, 151.263505617113), + Boardcore::TrajectoryPoint(20, 150.451887042568), + Boardcore::TrajectoryPoint(30, 149.637697884841), + Boardcore::TrajectoryPoint(40, 148.820583329703), + Boardcore::TrajectoryPoint(50, 148.000909302239), + Boardcore::TrajectoryPoint(60, 147.178474442449), + Boardcore::TrajectoryPoint(70, 146.353012133874), + Boardcore::TrajectoryPoint(80, 145.524863669696), + Boardcore::TrajectoryPoint(90, 144.693647313153), + Boardcore::TrajectoryPoint(100, 143.859401704909), + Boardcore::TrajectoryPoint(110, 143.022336542448), + Boardcore::TrajectoryPoint(120, 142.1817672384), + Boardcore::TrajectoryPoint(130, 141.338278334178), + Boardcore::TrajectoryPoint(140, 140.491450751199), + Boardcore::TrajectoryPoint(150, 139.641297325649), + Boardcore::TrajectoryPoint(160, 138.787918672288), + Boardcore::TrajectoryPoint(170, 137.93084966761), + Boardcore::TrajectoryPoint(180, 137.070606615013), + Boardcore::TrajectoryPoint(190, 136.206365609372), + Boardcore::TrajectoryPoint(200, 135.338831566631), + Boardcore::TrajectoryPoint(210, 134.467248065573), + Boardcore::TrajectoryPoint(220, 133.592145019571), + Boardcore::TrajectoryPoint(230, 132.712871074855), + Boardcore::TrajectoryPoint(240, 131.829908691435), + Boardcore::TrajectoryPoint(250, 130.942578141729), + Boardcore::TrajectoryPoint(260, 130.05145306502), + Boardcore::TrajectoryPoint(270, 129.155680452117), + Boardcore::TrajectoryPoint(280, 128.255940216178), + Boardcore::TrajectoryPoint(290, 127.351454952101), + Boardcore::TrajectoryPoint(300, 126.44264021665), + Boardcore::TrajectoryPoint(310, 125.529142278402), + Boardcore::TrajectoryPoint(320, 124.610812112527), + Boardcore::TrajectoryPoint(330, 123.687920195787), + Boardcore::TrajectoryPoint(340, 122.759634047109), + Boardcore::TrajectoryPoint(350, 121.826499372384), + Boardcore::TrajectoryPoint(360, 120.888241675445), + Boardcore::TrajectoryPoint(370, 119.944358193853), + Boardcore::TrajectoryPoint(380, 118.995221614201), + Boardcore::TrajectoryPoint(390, 118.040558875306), + Boardcore::TrajectoryPoint(400, 117.079834229482), + Boardcore::TrajectoryPoint(410, 116.113384567652), + Boardcore::TrajectoryPoint(420, 115.141036684993), + Boardcore::TrajectoryPoint(430, 114.162262164702), + Boardcore::TrajectoryPoint(440, 113.177061518822), + Boardcore::TrajectoryPoint(450, 112.185423841649), + Boardcore::TrajectoryPoint(460, 111.187149696296), + Boardcore::TrajectoryPoint(470, 110.18195528947), + Boardcore::TrajectoryPoint(480, 109.16939223526), + Boardcore::TrajectoryPoint(490, 108.149575592063), + Boardcore::TrajectoryPoint(500, 107.122275032974), + Boardcore::TrajectoryPoint(510, 106.087251101748), + Boardcore::TrajectoryPoint(520, 105.044254775484), + Boardcore::TrajectoryPoint(530, 103.993027001206), + Boardcore::TrajectoryPoint(540, 102.933298204535), + Boardcore::TrajectoryPoint(550, 101.864787768529), + Boardcore::TrajectoryPoint(560, 100.787203480583), + Boardcore::TrajectoryPoint(570, 99.7002409451403), + Boardcore::TrajectoryPoint(580, 98.6035829597442), + Boardcore::TrajectoryPoint(590, 97.4968988517833), + Boardcore::TrajectoryPoint(600, 96.3798437730387), + Boardcore::TrajectoryPoint(610, 95.2520579489053), + Boardcore::TrajectoryPoint(620, 94.1131658788825), + Boardcore::TrajectoryPoint(630, 92.9627754846369), + Boardcore::TrajectoryPoint(640, 91.8004772016072), + Boardcore::TrajectoryPoint(650, 90.6258430097677), + Boardcore::TrajectoryPoint(660, 89.4382558965152), + Boardcore::TrajectoryPoint(670, 88.2369011894616), + Boardcore::TrajectoryPoint(680, 87.0217036753287), + Boardcore::TrajectoryPoint(690, 85.7921436920914), + Boardcore::TrajectoryPoint(700, 84.5469386805745), + Boardcore::TrajectoryPoint(710, 83.2858824830871), + Boardcore::TrajectoryPoint(720, 82.0084326088468), + Boardcore::TrajectoryPoint(730, 80.7129561999655), + Boardcore::TrajectoryPoint(740, 79.3997527560275), + Boardcore::TrajectoryPoint(750, 78.0665633240034), + Boardcore::TrajectoryPoint(760, 76.7136116997284), + Boardcore::TrajectoryPoint(770, 75.3389874946048), + Boardcore::TrajectoryPoint(780, 73.941723081457), + Boardcore::TrajectoryPoint(790, 72.5208257784082), + Boardcore::TrajectoryPoint(800, 71.0746270039484), + Boardcore::TrajectoryPoint(810, 69.601073183387), + Boardcore::TrajectoryPoint(820, 68.098945961506), + Boardcore::TrajectoryPoint(830, 66.5662453757153), + Boardcore::TrajectoryPoint(840, 65.0007859741282), + Boardcore::TrajectoryPoint(850, 63.4001750472988), + Boardcore::TrajectoryPoint(860, 61.7616287672105), + Boardcore::TrajectoryPoint(870, 60.0814411294299), + Boardcore::TrajectoryPoint(880, 58.3569688580214), + Boardcore::TrajectoryPoint(890, 56.5838632709901), + Boardcore::TrajectoryPoint(900, 54.757093712143), + Boardcore::TrajectoryPoint(910, 52.8711844821095), + Boardcore::TrajectoryPoint(920, 50.9199171209883), + Boardcore::TrajectoryPoint(930, 48.8951600698814), + Boardcore::TrajectoryPoint(940, 46.7873985229568), + Boardcore::TrajectoryPoint(950, 44.5854289604041), + Boardcore::TrajectoryPoint(960, 42.2730526462557), + Boardcore::TrajectoryPoint(970, 39.8326264734145), + Boardcore::TrajectoryPoint(980, 37.2383667841264), + Boardcore::TrajectoryPoint(990, 34.4560683758031), + Boardcore::TrajectoryPoint(1000, 31.4348532126553), + Boardcore::TrajectoryPoint(1010, 28.0992552295128), + Boardcore::TrajectoryPoint(1020, 24.3180431049642), + Boardcore::TrajectoryPoint(1030, 19.8415750298029), + Boardcore::TrajectoryPoint(1040, 14.0180187927368), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t7_closed[] = { + Boardcore::TrajectoryPoint(0, 152.039000927416), + Boardcore::TrajectoryPoint(10, 151.23023494639), + Boardcore::TrajectoryPoint(20, 150.419102596577), + Boardcore::TrajectoryPoint(30, 149.605428073866), + Boardcore::TrajectoryPoint(40, 148.78879286286), + Boardcore::TrajectoryPoint(50, 147.969595120189), + Boardcore::TrajectoryPoint(60, 147.147664303406), + Boardcore::TrajectoryPoint(70, 146.322671432139), + Boardcore::TrajectoryPoint(80, 145.49498937204), + Boardcore::TrajectoryPoint(90, 144.664266409254), + Boardcore::TrajectoryPoint(100, 143.830480392353), + Boardcore::TrajectoryPoint(110, 142.993871814025), + Boardcore::TrajectoryPoint(120, 142.153785206939), + Boardcore::TrajectoryPoint(130, 141.310746110176), + Boardcore::TrajectoryPoint(140, 140.464394263504), + Boardcore::TrajectoryPoint(150, 139.614683889021), + Boardcore::TrajectoryPoint(160, 138.761773963392), + Boardcore::TrajectoryPoint(170, 137.90514127457), + Boardcore::TrajectoryPoint(180, 137.045331590065), + Boardcore::TrajectoryPoint(190, 136.181548494497), + Boardcore::TrajectoryPoint(200, 135.314441123351), + Boardcore::TrajectoryPoint(210, 134.443308446477), + Boardcore::TrajectoryPoint(220, 133.56862539815), + Boardcore::TrajectoryPoint(230, 132.689795156474), + Boardcore::TrajectoryPoint(240, 131.807246119943), + Boardcore::TrajectoryPoint(250, 130.920352120252), + Boardcore::TrajectoryPoint(260, 130.029633763249), + Boardcore::TrajectoryPoint(270, 129.134290518578), + Boardcore::TrajectoryPoint(280, 128.234976173414), + Boardcore::TrajectoryPoint(290, 127.330887295676), + Boardcore::TrajectoryPoint(300, 126.422491211683), + Boardcore::TrajectoryPoint(310, 125.509383089424), + Boardcore::TrajectoryPoint(320, 124.591464314658), + Boardcore::TrajectoryPoint(330, 123.668980000697), + Boardcore::TrajectoryPoint(340, 122.741073631931), + Boardcore::TrajectoryPoint(350, 121.808339475929), + Boardcore::TrajectoryPoint(360, 120.870454827233), + Boardcore::TrajectoryPoint(370, 119.926964544654), + Boardcore::TrajectoryPoint(380, 118.978217521817), + Boardcore::TrajectoryPoint(390, 118.023917655791), + Boardcore::TrajectoryPoint(400, 117.0635752131), + Boardcore::TrajectoryPoint(410, 116.097504068662), + Boardcore::TrajectoryPoint(420, 115.125530996297), + Boardcore::TrajectoryPoint(430, 114.147105547905), + Boardcore::TrajectoryPoint(440, 113.162272313311), + Boardcore::TrajectoryPoint(450, 112.170998303582), + Boardcore::TrajectoryPoint(460, 111.173084063324), + Boardcore::TrajectoryPoint(470, 110.168225010042), + Boardcore::TrajectoryPoint(480, 109.156014427786), + Boardcore::TrajectoryPoint(490, 108.136546463427), + Boardcore::TrajectoryPoint(500, 107.109590774075), + Boardcore::TrajectoryPoint(510, 106.074907888216), + Boardcore::TrajectoryPoint(520, 105.032248768379), + Boardcore::TrajectoryPoint(530, 103.981354347692), + Boardcore::TrajectoryPoint(540, 102.921955038534), + Boardcore::TrajectoryPoint(550, 101.853770211353), + Boardcore::TrajectoryPoint(560, 100.776507641556), + Boardcore::TrajectoryPoint(570, 99.6898629221948), + Boardcore::TrajectoryPoint(580, 98.5935188400059), + Boardcore::TrajectoryPoint(590, 97.4871447121413), + Boardcore::TrajectoryPoint(600, 96.3703956807007), + Boardcore::TrajectoryPoint(610, 95.24291196194), + Boardcore::TrajectoryPoint(620, 94.1043180467503), + Boardcore::TrajectoryPoint(630, 92.9542218487082), + Boardcore::TrajectoryPoint(640, 91.7922137956711), + Boardcore::TrajectoryPoint(650, 90.6178658605292), + Boardcore::TrajectoryPoint(660, 89.4305761007826), + Boardcore::TrajectoryPoint(670, 88.2294992382759), + Boardcore::TrajectoryPoint(680, 87.0145755649535), + Boardcore::TrajectoryPoint(690, 85.7852854139251), + Boardcore::TrajectoryPoint(700, 84.5403599541099), + Boardcore::TrajectoryPoint(710, 83.2795650561241), + Boardcore::TrajectoryPoint(720, 82.0023856518145), + Boardcore::TrajectoryPoint(730, 80.7071619439913), + Boardcore::TrajectoryPoint(740, 79.3942197395534), + Boardcore::TrajectoryPoint(750, 78.0612743523641), + Boardcore::TrajectoryPoint(760, 76.7085746013617), + Boardcore::TrajectoryPoint(770, 75.3341857346371), + Boardcore::TrajectoryPoint(780, 73.9371637049641), + Boardcore::TrajectoryPoint(790, 72.51650388797), + Boardcore::TrajectoryPoint(800, 71.0705269981542), + Boardcore::TrajectoryPoint(810, 69.5972009877557), + Boardcore::TrajectoryPoint(820, 68.0952965558671), + Boardcore::TrajectoryPoint(830, 66.562813693798), + Boardcore::TrajectoryPoint(840, 64.9975669059823), + Boardcore::TrajectoryPoint(850, 63.3971634416501), + Boardcore::TrajectoryPoint(860, 61.7588280396179), + Boardcore::TrajectoryPoint(870, 60.0788370195371), + Boardcore::TrajectoryPoint(880, 58.354556081729), + Boardcore::TrajectoryPoint(890, 56.5816440388267), + Boardcore::TrajectoryPoint(900, 54.7550546499471), + Boardcore::TrajectoryPoint(910, 52.8693269424873), + Boardcore::TrajectoryPoint(920, 50.9182348143468), + Boardcore::TrajectoryPoint(930, 48.893646597994), + Boardcore::TrajectoryPoint(940, 46.7860473863621), + Boardcore::TrajectoryPoint(950, 44.5842335660382), + Boardcore::TrajectoryPoint(960, 42.2720112330624), + Boardcore::TrajectoryPoint(970, 39.8317314029105), + Boardcore::TrajectoryPoint(980, 37.237610220284), + Boardcore::TrajectoryPoint(990, 34.4554460929404), + Boardcore::TrajectoryPoint(1000, 31.4343560895499), + Boardcore::TrajectoryPoint(1010, 28.0988738683303), + Boardcore::TrajectoryPoint(1020, 24.317772680632), + Boardcore::TrajectoryPoint(1030, 19.8414054324756), + Boardcore::TrajectoryPoint(1040, 14.017940402677), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t8_closed[] = { + Boardcore::TrajectoryPoint(0, 152.005480922458), + Boardcore::TrajectoryPoint(10, 151.197224313278), + Boardcore::TrajectoryPoint(20, 150.38657426689), + Boardcore::TrajectoryPoint(30, 149.573410307954), + Boardcore::TrajectoryPoint(40, 148.757250579419), + Boardcore::TrajectoryPoint(50, 147.938525286881), + Boardcore::TrajectoryPoint(60, 147.117094528702), + Boardcore::TrajectoryPoint(70, 146.292567319369), + Boardcore::TrajectoryPoint(80, 145.465347914672), + Boardcore::TrajectoryPoint(90, 144.635114448831), + Boardcore::TrajectoryPoint(100, 143.801784333495), + Boardcore::TrajectoryPoint(110, 142.965628675919), + Boardcore::TrajectoryPoint(120, 142.126020957175), + Boardcore::TrajectoryPoint(130, 141.283428062975), + Boardcore::TrajectoryPoint(140, 140.437548200509), + Boardcore::TrajectoryPoint(150, 139.588277330443), + Boardcore::TrajectoryPoint(160, 138.735832437525), + Boardcore::TrajectoryPoint(170, 137.879632575911), + Boardcore::TrajectoryPoint(180, 137.020252796546), + Boardcore::TrajectoryPoint(190, 136.156924005649), + Boardcore::TrajectoryPoint(200, 135.290239900686), + Boardcore::TrajectoryPoint(210, 134.419554500541), + Boardcore::TrajectoryPoint(220, 133.545288101743), + Boardcore::TrajectoryPoint(230, 132.666898073987), + Boardcore::TrajectoryPoint(240, 131.784759093177), + Boardcore::TrajectoryPoint(250, 130.898298213049), + Boardcore::TrajectoryPoint(260, 130.007983341272), + Boardcore::TrajectoryPoint(270, 129.113066093345), + Boardcore::TrajectoryPoint(280, 128.21417429511), + Boardcore::TrajectoryPoint(290, 127.310478657188), + Boardcore::TrajectoryPoint(300, 126.402497940352), + Boardcore::TrajectoryPoint(310, 125.489776543526), + Boardcore::TrajectoryPoint(320, 124.572265935296), + Boardcore::TrajectoryPoint(330, 123.650161857034), + Boardcore::TrajectoryPoint(340, 122.722656435667), + Boardcore::TrajectoryPoint(350, 121.79031966253), + Boardcore::TrajectoryPoint(360, 120.852805113679), + Boardcore::TrajectoryPoint(370, 119.909704954489), + Boardcore::TrajectoryPoint(380, 118.961344442155), + Boardcore::TrajectoryPoint(390, 118.007404586476), + Boardcore::TrajectoryPoint(400, 117.047441361105), + Boardcore::TrajectoryPoint(410, 116.081745777964), + Boardcore::TrajectoryPoint(420, 115.110144589725), + Boardcore::TrajectoryPoint(430, 114.132065466685), + Boardcore::TrajectoryPoint(440, 113.147596778033), + Boardcore::TrajectoryPoint(450, 112.156683600653), + Boardcore::TrajectoryPoint(460, 111.159126460812), + Boardcore::TrajectoryPoint(470, 110.154600129346), + Boardcore::TrajectoryPoint(480, 109.142739275375), + Boardcore::TrajectoryPoint(490, 108.12361727685), + Boardcore::TrajectoryPoint(500, 107.097003775031), + Boardcore::TrajectoryPoint(510, 106.062659283267), + Boardcore::TrajectoryPoint(520, 105.020334749637), + Boardcore::TrajectoryPoint(530, 103.969771093486), + Boardcore::TrajectoryPoint(540, 102.910698714053), + Boardcore::TrajectoryPoint(550, 101.842836969278), + Boardcore::TrajectoryPoint(560, 100.765893622667), + Boardcore::TrajectoryPoint(570, 99.6795642559619), + Boardcore::TrajectoryPoint(580, 98.5835316451706), + Boardcore::TrajectoryPoint(590, 97.4774650972766), + Boardcore::TrajectoryPoint(600, 96.361019744761), + Boardcore::TrajectoryPoint(610, 95.2338357947974), + Boardcore::TrajectoryPoint(620, 94.0955377297188), + Boardcore::TrajectoryPoint(630, 92.9457334550571), + Boardcore::TrajectoryPoint(640, 91.7840133911267), + Boardcore::TrajectoryPoint(650, 90.6099495037675), + Boardcore::TrajectoryPoint(660, 89.4229548129136), + Boardcore::TrajectoryPoint(670, 88.2221536532672), + Boardcore::TrajectoryPoint(680, 87.0075017112168), + Boardcore::TrajectoryPoint(690, 85.7784793150158), + Boardcore::TrajectoryPoint(700, 84.5338312630252), + Boardcore::TrajectoryPoint(710, 83.2732956551972), + Boardcore::TrajectoryPoint(720, 81.9963846484215), + Boardcore::TrajectoryPoint(730, 80.7014117007834), + Boardcore::TrajectoryPoint(740, 79.3887287361081), + Boardcore::TrajectoryPoint(750, 78.0560255217003), + Boardcore::TrajectoryPoint(760, 76.7035757179309), + Boardcore::TrajectoryPoint(770, 75.3294203866412), + Boardcore::TrajectoryPoint(780, 73.9326388890096), + Boardcore::TrajectoryPoint(790, 72.5122147449245), + Boardcore::TrajectoryPoint(800, 71.0664580431786), + Boardcore::TrajectoryPoint(810, 69.5933581057342), + Boardcore::TrajectoryPoint(820, 68.0916747658291), + Boardcore::TrajectoryPoint(830, 66.5594079690151), + Boardcore::TrajectoryPoint(840, 64.9943721763642), + Boardcore::TrajectoryPoint(850, 63.3941745960764), + Boardcore::TrajectoryPoint(860, 61.7560484706829), + Boardcore::TrajectoryPoint(870, 60.0762525740897), + Boardcore::TrajectoryPoint(880, 58.3521615167551), + Boardcore::TrajectoryPoint(890, 56.5794415505358), + Boardcore::TrajectoryPoint(900, 54.7530309650049), + Boardcore::TrajectoryPoint(910, 52.8674834053872), + Boardcore::TrajectoryPoint(920, 50.9165651838645), + Boardcore::TrajectoryPoint(930, 48.89214452508), + Boardcore::TrajectoryPoint(940, 46.7847064214711), + Boardcore::TrajectoryPoint(950, 44.5830471666934), + Boardcore::TrajectoryPoint(960, 42.2709776528296), + Boardcore::TrajectoryPoint(970, 39.8308430615754), + Boardcore::TrajectoryPoint(980, 37.2368593415597), + Boardcore::TrajectoryPoint(990, 34.4548284839283), + Boardcore::TrajectoryPoint(1000, 31.4338626982924), + Boardcore::TrajectoryPoint(1010, 28.0984953683119), + Boardcore::TrajectoryPoint(1020, 24.3175042839124), + Boardcore::TrajectoryPoint(1030, 19.841237105809), + Boardcore::TrajectoryPoint(1040, 14.0178625993186), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t9_closed[] = { + Boardcore::TrajectoryPoint(0, 151.972220919451), + Boardcore::TrajectoryPoint(10, 151.1644706853), + Boardcore::TrajectoryPoint(20, 150.354299068137), + Boardcore::TrajectoryPoint(30, 149.541641649582), + Boardcore::TrajectoryPoint(40, 148.725953588209), + Boardcore::TrajectoryPoint(50, 147.907696957141), + Boardcore::TrajectoryPoint(60, 147.086762319979), + Boardcore::TrajectoryPoint(70, 146.26269704245), + Boardcore::TrajectoryPoint(80, 145.435936589378), + Boardcore::TrajectoryPoint(90, 144.606188769432), + Boardcore::TrajectoryPoint(100, 143.773310910031), + Boardcore::TrajectoryPoint(110, 142.937604553633), + Boardcore::TrajectoryPoint(120, 142.098471959315), + Boardcore::TrajectoryPoint(130, 141.256321705844), + Boardcore::TrajectoryPoint(140, 140.410910119514), + Boardcore::TrajectoryPoint(150, 139.562075249543), + Boardcore::TrajectoryPoint(160, 138.710091737666), + Boardcore::TrajectoryPoint(170, 137.854321256199), + Boardcore::TrajectoryPoint(180, 136.995367960286), + Boardcore::TrajectoryPoint(190, 136.132489910916), + Boardcore::TrajectoryPoint(200, 135.266225707255), + Boardcore::TrajectoryPoint(210, 134.395984077953), + Boardcore::TrajectoryPoint(220, 133.52213102034), + Boardcore::TrajectoryPoint(230, 132.644177758248), + Boardcore::TrajectoryPoint(240, 131.762445581076), + Boardcore::TrajectoryPoint(250, 130.876414430218), + Boardcore::TrajectoryPoint(260, 129.986499847555), + Boardcore::TrajectoryPoint(270, 129.092005264325), + Boardcore::TrajectoryPoint(280, 128.193532708298), + Boardcore::TrajectoryPoint(290, 127.29022720093), + Boardcore::TrajectoryPoint(300, 126.382658605351), + Boardcore::TrajectoryPoint(310, 125.47032087996), + Boardcore::TrajectoryPoint(320, 124.553215251368), + Boardcore::TrajectoryPoint(330, 123.631488409761), + Boardcore::TrajectoryPoint(340, 122.704380808049), + Boardcore::TrajectoryPoint(350, 121.772438318524), + Boardcore::TrajectoryPoint(360, 120.8352909559), + Boardcore::TrajectoryPoint(370, 119.892577880348), + Boardcore::TrajectoryPoint(380, 118.944600867729), + Boardcore::TrajectoryPoint(390, 117.991018193578), + Boardcore::TrajectoryPoint(400, 117.031431234504), + Boardcore::TrajectoryPoint(410, 116.066108291001), + Boardcore::TrajectoryPoint(420, 115.094876094798), + Boardcore::TrajectoryPoint(430, 114.117140582819), + Boardcore::TrajectoryPoint(440, 113.133033608106), + Boardcore::TrajectoryPoint(450, 112.142478460955), + Boardcore::TrajectoryPoint(460, 111.145275649464), + Boardcore::TrajectoryPoint(470, 110.141079438919), + Boardcore::TrajectoryPoint(480, 109.129565601433), + Boardcore::TrajectoryPoint(490, 108.110786887239), + Boardcore::TrajectoryPoint(500, 107.084512921882), + Boardcore::TrajectoryPoint(510, 106.050504203703), + Boardcore::TrajectoryPoint(520, 105.008511666449), + Boardcore::TrajectoryPoint(530, 103.958276215791), + Boardcore::TrajectoryPoint(540, 102.899528237938), + Boardcore::TrajectoryPoint(550, 101.831987078412), + Boardcore::TrajectoryPoint(560, 100.755360488908), + Boardcore::TrajectoryPoint(570, 99.6693440399418), + Boardcore::TrajectoryPoint(580, 98.5736204968654), + Boardcore::TrajectoryPoint(590, 97.4678591565632), + Boardcore::TrajectoryPoint(600, 96.3517151419596), + Boardcore::TrajectoryPoint(610, 95.224828651202), + Boardcore::TrajectoryPoint(620, 94.0868241581155), + Boardcore::TrajectoryPoint(630, 92.9373095602312), + Boardcore::TrajectoryPoint(640, 91.7758752703596), + Boardcore::TrajectoryPoint(650, 90.6020932473235), + Boardcore::TrajectoryPoint(660, 89.4153913669583), + Boardcore::TrajectoryPoint(670, 88.2148637931413), + Boardcore::TrajectoryPoint(680, 87.0004814970956), + Boardcore::TrajectoryPoint(690, 85.7717248022281), + Boardcore::TrajectoryPoint(700, 84.5273520387386), + Boardcore::TrajectoryPoint(710, 83.267073734804), + Boardcore::TrajectoryPoint(720, 81.9904290768815), + Boardcore::TrajectoryPoint(730, 80.6957049708232), + Boardcore::TrajectoryPoint(740, 79.3832792690369), + Boardcore::TrajectoryPoint(750, 78.0508163768098), + Boardcore::TrajectoryPoint(760, 76.6986146162335), + Boardcore::TrajectoryPoint(770, 75.3246910380499), + Boardcore::TrajectoryPoint(780, 73.9281482421519), + Boardcore::TrajectoryPoint(790, 72.5079579785098), + Boardcore::TrajectoryPoint(800, 71.0624197876405), + Boardcore::TrajectoryPoint(810, 69.5895442057322), + Boardcore::TrajectoryPoint(820, 68.088080279136), + Boardcore::TrajectoryPoint(830, 66.5560279079846), + Boardcore::TrajectoryPoint(840, 64.9912015103021), + Boardcore::TrajectoryPoint(850, 63.3912082535489), + Boardcore::TrajectoryPoint(860, 61.7532898215466), + Boardcore::TrajectoryPoint(870, 60.0736875711954), + Boardcore::TrajectoryPoint(880, 58.3497849576973), + Boardcore::TrajectoryPoint(890, 56.577255617339), + Boardcore::TrajectoryPoint(900, 54.751022484027), + Boardcore::TrajectoryPoint(910, 52.8656537130767), + Boardcore::TrajectoryPoint(920, 50.9149080868102), + Boardcore::TrajectoryPoint(930, 48.8906537228448), + Boardcore::TrajectoryPoint(940, 46.7833755138536), + Boardcore::TrajectoryPoint(950, 44.5818696612242), + Boardcore::TrajectoryPoint(960, 42.2699518175164), + Boardcore::TrajectoryPoint(970, 39.8299613738092), + Boardcore::TrajectoryPoint(980, 37.2361140841138), + Boardcore::TrajectoryPoint(990, 34.4542154963078), + Boardcore::TrajectoryPoint(1000, 31.4333729970188), + Boardcore::TrajectoryPoint(1010, 28.0981196973795), + Boardcore::TrajectoryPoint(1020, 24.3172378920867), + Boardcore::TrajectoryPoint(1030, 19.8410700355767), + Boardcore::TrajectoryPoint(1040, 14.0177853760999), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t10_closed[] = { + Boardcore::TrajectoryPoint(0, 151.939217912631), + Boardcore::TrajectoryPoint(10, 151.131971076877), + Boardcore::TrajectoryPoint(20, 150.322274061098), + Boardcore::TrajectoryPoint(30, 149.510091542429), + Boardcore::TrajectoryPoint(40, 148.694899042729), + Boardcore::TrajectoryPoint(50, 147.877107329736), + Boardcore::TrajectoryPoint(60, 147.056664922089), + Boardcore::TrajectoryPoint(70, 146.23305789076), + Boardcore::TrajectoryPoint(80, 145.406752729722), + Boardcore::TrajectoryPoint(90, 144.577486749675), + Boardcore::TrajectoryPoint(100, 143.745057544031), + Boardcore::TrajectoryPoint(110, 142.909796912352), + Boardcore::TrajectoryPoint(120, 142.071135722547), + Boardcore::TrajectoryPoint(130, 141.229424590358), + Boardcore::TrajectoryPoint(140, 140.384477615441), + Boardcore::TrajectoryPoint(150, 139.536075282899), + Boardcore::TrajectoryPoint(160, 138.68454954307), + Boardcore::TrajectoryPoint(170, 137.829205035625), + Boardcore::TrajectoryPoint(180, 136.970674842081), + Boardcore::TrajectoryPoint(190, 136.108244012702), + Boardcore::TrajectoryPoint(200, 135.242396385354), + Boardcore::TrajectoryPoint(210, 134.372595061928), + Boardcore::TrajectoryPoint(220, 133.499152076336), + Boardcore::TrajectoryPoint(230, 132.621632171883), + Boardcore::TrajectoryPoint(240, 131.740303584736), + Boardcore::TrajectoryPoint(250, 130.854698812387), + Boardcore::TrajectoryPoint(260, 129.965181360491), + Boardcore::TrajectoryPoint(270, 129.071106148747), + Boardcore::TrajectoryPoint(280, 128.173049568719), + Boardcore::TrajectoryPoint(290, 127.270131119324), + Boardcore::TrajectoryPoint(300, 126.362971436904), + Boardcore::TrajectoryPoint(310, 125.451014364936), + Boardcore::TrajectoryPoint(320, 124.534310566176), + Boardcore::TrajectoryPoint(330, 123.612957997475), + Boardcore::TrajectoryPoint(340, 122.686245124051), + Boardcore::TrajectoryPoint(350, 121.754693854927), + Boardcore::TrajectoryPoint(360, 120.817910799138), + Boardcore::TrajectoryPoint(370, 119.875581802795), + Boardcore::TrajectoryPoint(380, 118.92798531408), + Boardcore::TrajectoryPoint(390, 117.974757025813), + Boardcore::TrajectoryPoint(400, 117.015543416263), + Boardcore::TrajectoryPoint(410, 116.050590224644), + Boardcore::TrajectoryPoint(420, 115.079724161938), + Boardcore::TrajectoryPoint(430, 114.102329578488), + Boardcore::TrajectoryPoint(440, 113.11858151853), + Boardcore::TrajectoryPoint(450, 112.128381631957), + Boardcore::TrajectoryPoint(460, 111.131530408858), + Boardcore::TrajectoryPoint(470, 110.127661748693), + Boardcore::TrajectoryPoint(480, 109.116492247272), + Boardcore::TrajectoryPoint(490, 108.098054166921), + Boardcore::TrajectoryPoint(500, 107.072117117609), + Boardcore::TrajectoryPoint(510, 106.038441582792), + Boardcore::TrajectoryPoint(520, 104.996778482003), + Boardcore::TrajectoryPoint(530, 103.946868707353), + Boardcore::TrajectoryPoint(540, 102.888442632115), + Boardcore::TrajectoryPoint(550, 101.821219589495), + Boardcore::TrajectoryPoint(560, 100.744907319461), + Boardcore::TrajectoryPoint(570, 99.6591936540695), + Boardcore::TrajectoryPoint(580, 98.5637845300357), + Boardcore::TrajectoryPoint(590, 97.4583260522675), + Boardcore::TrajectoryPoint(600, 96.3424810615093), + Boardcore::TrajectoryPoint(610, 95.2158897469375), + Boardcore::TrajectoryPoint(620, 94.078176573919), + Boardcore::TrajectoryPoint(630, 92.928949432028), + Boardcore::TrajectoryPoint(640, 91.7677987266097), + Boardcore::TrajectoryPoint(650, 90.5942964095027), + Boardcore::TrajectoryPoint(660, 89.4078851070323), + Boardcore::TrajectoryPoint(670, 88.2076290262928), + Boardcore::TrajectoryPoint(680, 86.9935143148854), + Boardcore::TrajectoryPoint(690, 85.7650212913802), + Boardcore::TrajectoryPoint(700, 84.5209217212486), + Boardcore::TrajectoryPoint(710, 83.2608987576706), + Boardcore::TrajectoryPoint(720, 81.9845184232764), + Boardcore::TrajectoryPoint(730, 80.6900412621207), + Boardcore::TrajectoryPoint(740, 79.3778708688667), + Boardcore::TrajectoryPoint(750, 78.0456464693453), + Boardcore::TrajectoryPoint(760, 76.6936908695887), + Boardcore::TrajectoryPoint(770, 75.319997282504), + Boardcore::TrajectoryPoint(780, 73.9236913788376), + Boardcore::TrajectoryPoint(790, 72.5037332235386), + Boardcore::TrajectoryPoint(800, 71.0584118854396), + Boardcore::TrajectoryPoint(810, 69.5857589611408), + Boardcore::TrajectoryPoint(820, 68.0845127882209), + Boardcore::TrajectoryPoint(830, 66.5526732217284), + Boardcore::TrajectoryPoint(840, 64.9880546369497), + Boardcore::TrajectoryPoint(850, 63.3882641608939), + Boardcore::TrajectoryPoint(860, 61.7505518569308), + Boardcore::TrajectoryPoint(870, 60.0711417922867), + Boardcore::TrajectoryPoint(880, 58.3474262022295), + Boardcore::TrajectoryPoint(890, 56.5750860532844), + Boardcore::TrajectoryPoint(900, 54.7490290363177), + Boardcore::TrajectoryPoint(910, 52.8638377101825), + Boardcore::TrajectoryPoint(920, 50.9132633825867), + Boardcore::TrajectoryPoint(930, 48.8891740649115), + Boardcore::TrajectoryPoint(940, 46.782054550789), + Boardcore::TrajectoryPoint(950, 44.5807009499952), + Boardcore::TrajectoryPoint(960, 42.2689336403958), + Boardcore::TrajectoryPoint(970, 39.8290862651394), + Boardcore::TrajectoryPoint(980, 37.2353743850582), + Boardcore::TrajectoryPoint(990, 34.4536070784014), + Boardcore::TrajectoryPoint(1000, 31.432886944488), + Boardcore::TrajectoryPoint(1010, 28.0977468239322), + Boardcore::TrajectoryPoint(1020, 24.316973482774), + Boardcore::TrajectoryPoint(1030, 19.8409042077629), + Boardcore::TrajectoryPoint(1040, 14.0177087265563), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t0_open[] = { + Boardcore::TrajectoryPoint(0, 169.360315331402), + Boardcore::TrajectoryPoint(10, 168.246480367201), + Boardcore::TrajectoryPoint(20, 167.134073503613), + Boardcore::TrajectoryPoint(30, 166.022426559672), + Boardcore::TrajectoryPoint(40, 164.911499807547), + Boardcore::TrajectoryPoint(50, 163.801180771595), + Boardcore::TrajectoryPoint(60, 162.691340150731), + Boardcore::TrajectoryPoint(70, 161.581892032395), + Boardcore::TrajectoryPoint(80, 160.472755991675), + Boardcore::TrajectoryPoint(90, 159.363858989209), + Boardcore::TrajectoryPoint(100, 158.255076329991), + Boardcore::TrajectoryPoint(110, 157.146323850155), + Boardcore::TrajectoryPoint(120, 156.037509424379), + Boardcore::TrajectoryPoint(130, 154.928523292648), + Boardcore::TrajectoryPoint(140, 153.819291188769), + Boardcore::TrajectoryPoint(150, 152.709730100639), + Boardcore::TrajectoryPoint(160, 151.599668963086), + Boardcore::TrajectoryPoint(170, 150.489079565894), + Boardcore::TrajectoryPoint(180, 149.377885385811), + Boardcore::TrajectoryPoint(190, 148.265870235037), + Boardcore::TrajectoryPoint(200, 147.153034470015), + Boardcore::TrajectoryPoint(210, 146.039310268352), + Boardcore::TrajectoryPoint(220, 144.924403754144), + Boardcore::TrajectoryPoint(230, 143.808418560157), + Boardcore::TrajectoryPoint(240, 142.691154752502), + Boardcore::TrajectoryPoint(250, 141.572461306612), + Boardcore::TrajectoryPoint(260, 140.452406425859), + Boardcore::TrajectoryPoint(270, 139.330559078558), + Boardcore::TrajectoryPoint(280, 138.207142240544), + Boardcore::TrajectoryPoint(290, 137.081788005055), + Boardcore::TrajectoryPoint(300, 135.954543762304), + Boardcore::TrajectoryPoint(310, 134.825185415819), + Boardcore::TrajectoryPoint(320, 133.693635972874), + Boardcore::TrajectoryPoint(330, 132.559748527893), + Boardcore::TrajectoryPoint(340, 131.423402685643), + Boardcore::TrajectoryPoint(350, 130.284432664314), + Boardcore::TrajectoryPoint(360, 129.142784634321), + Boardcore::TrajectoryPoint(370, 127.998149031994), + Boardcore::TrajectoryPoint(380, 126.850677124078), + Boardcore::TrajectoryPoint(390, 125.699762077224), + Boardcore::TrajectoryPoint(400, 124.545732350525), + Boardcore::TrajectoryPoint(410, 123.388086429243), + Boardcore::TrajectoryPoint(420, 122.22678244767), + Boardcore::TrajectoryPoint(430, 121.061838910406), + Boardcore::TrajectoryPoint(440, 119.892601854044), + Boardcore::TrajectoryPoint(450, 118.719294972083), + Boardcore::TrajectoryPoint(460, 117.541762424381), + Boardcore::TrajectoryPoint(470, 116.359420720709), + Boardcore::TrajectoryPoint(480, 115.172332529927), + Boardcore::TrajectoryPoint(490, 113.980366023733), + Boardcore::TrajectoryPoint(500, 112.78328457612), + Boardcore::TrajectoryPoint(510, 111.58046804476), + Boardcore::TrajectoryPoint(520, 110.372034891077), + Boardcore::TrajectoryPoint(530, 109.157731235397), + Boardcore::TrajectoryPoint(540, 107.937279056964), + Boardcore::TrajectoryPoint(550, 106.710388018682), + Boardcore::TrajectoryPoint(560, 105.476754945406), + Boardcore::TrajectoryPoint(570, 104.23588820423), + Boardcore::TrajectoryPoint(580, 102.987660747081), + Boardcore::TrajectoryPoint(590, 101.731742270957), + Boardcore::TrajectoryPoint(600, 100.467776711107), + Boardcore::TrajectoryPoint(610, 99.1953920596245), + Boardcore::TrajectoryPoint(620, 97.9141995654263), + Boardcore::TrajectoryPoint(630, 96.6237928747764), + Boardcore::TrajectoryPoint(640, 95.3237471084763), + Boardcore::TrajectoryPoint(650, 94.0134755817458), + Boardcore::TrajectoryPoint(660, 92.6924549638871), + Boardcore::TrajectoryPoint(670, 91.3603118747879), + Boardcore::TrajectoryPoint(680, 90.0165297318923), + Boardcore::TrajectoryPoint(690, 88.660567268661), + Boardcore::TrajectoryPoint(700, 87.2916139739664), + Boardcore::TrajectoryPoint(710, 85.908740671119), + Boardcore::TrajectoryPoint(720, 84.5117719408656), + Boardcore::TrajectoryPoint(730, 83.0996650103509), + Boardcore::TrajectoryPoint(740, 81.6712708674768), + Boardcore::TrajectoryPoint(750, 80.2264197090199), + Boardcore::TrajectoryPoint(760, 78.7630162208311), + Boardcore::TrajectoryPoint(770, 77.2811121656465), + Boardcore::TrajectoryPoint(780, 75.7786303881267), + Boardcore::TrajectoryPoint(790, 74.2547147333932), + Boardcore::TrajectoryPoint(800, 72.7081441558916), + Boardcore::TrajectoryPoint(810, 71.1368655836554), + Boardcore::TrajectoryPoint(820, 69.5391547447803), + Boardcore::TrajectoryPoint(830, 67.9133719216858), + Boardcore::TrajectoryPoint(840, 66.257311088182), + Boardcore::TrajectoryPoint(850, 64.5685515855946), + Boardcore::TrajectoryPoint(860, 62.8444325050821), + Boardcore::TrajectoryPoint(870, 61.0818038286117), + Boardcore::TrajectoryPoint(880, 59.2765618187081), + Boardcore::TrajectoryPoint(890, 57.4257151566554), + Boardcore::TrajectoryPoint(900, 55.523715252443), + Boardcore::TrajectoryPoint(910, 53.5660024963227), + Boardcore::TrajectoryPoint(920, 51.5446655475009), + Boardcore::TrajectoryPoint(930, 49.4529070771036), + Boardcore::TrajectoryPoint(940, 47.2813085630852), + Boardcore::TrajectoryPoint(950, 45.0173461901611), + Boardcore::TrajectoryPoint(960, 42.6472163924613), + Boardcore::TrajectoryPoint(970, 40.1512282774346), + Boardcore::TrajectoryPoint(980, 37.5049975932762), + Boardcore::TrajectoryPoint(990, 34.6731004141643), + Boardcore::TrajectoryPoint(1000, 31.6062887300925), + Boardcore::TrajectoryPoint(1010, 28.2286684462889), + Boardcore::TrajectoryPoint(1020, 24.4089685562411), + Boardcore::TrajectoryPoint(1030, 19.8977836278758), + Boardcore::TrajectoryPoint(1040, 14.0435849933506), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t1_open[] = { + Boardcore::TrajectoryPoint(0, 169.245401138596), + Boardcore::TrajectoryPoint(10, 168.13373829021), + Boardcore::TrajectoryPoint(20, 167.0234127897), + Boardcore::TrajectoryPoint(30, 165.913819215268), + Boardcore::TrajectoryPoint(40, 164.804894908062), + Boardcore::TrajectoryPoint(50, 163.696573631899), + Boardcore::TrajectoryPoint(60, 162.588704265396), + Boardcore::TrajectoryPoint(70, 161.481201390453), + Boardcore::TrajectoryPoint(80, 160.373980152089), + Boardcore::TrajectoryPoint(90, 159.266973787375), + Boardcore::TrajectoryPoint(100, 158.160059173274), + Boardcore::TrajectoryPoint(110, 157.053150545625), + Boardcore::TrajectoryPoint(120, 155.946160005792), + Boardcore::TrajectoryPoint(130, 154.838968124123), + Boardcore::TrajectoryPoint(140, 153.731507331512), + Boardcore::TrajectoryPoint(150, 152.623695012999), + Boardcore::TrajectoryPoint(160, 151.515370665724), + Boardcore::TrajectoryPoint(170, 150.406483238506), + Boardcore::TrajectoryPoint(180, 149.296969622668), + Boardcore::TrajectoryPoint(190, 148.186630337033), + Boardcore::TrajectoryPoint(200, 147.075430248138), + Boardcore::TrajectoryPoint(210, 145.963327820648), + Boardcore::TrajectoryPoint(220, 144.850030479483), + Boardcore::TrajectoryPoint(230, 143.735617545146), + Boardcore::TrajectoryPoint(240, 142.619932618686), + Boardcore::TrajectoryPoint(250, 141.502769476635), + Boardcore::TrajectoryPoint(260, 140.384226138456), + Boardcore::TrajectoryPoint(270, 139.26389865617), + Boardcore::TrajectoryPoint(280, 138.141953075543), + Boardcore::TrajectoryPoint(290, 137.018082242506), + Boardcore::TrajectoryPoint(300, 135.892269994398), + Boardcore::TrajectoryPoint(310, 134.764358670881), + Boardcore::TrajectoryPoint(320, 133.634202948614), + Boardcore::TrajectoryPoint(330, 132.501726313433), + Boardcore::TrajectoryPoint(340, 131.36673687076), + Boardcore::TrajectoryPoint(350, 130.229141677191), + Boardcore::TrajectoryPoint(360, 129.088813641437), + Boardcore::TrajectoryPoint(370, 127.945517165827), + Boardcore::TrajectoryPoint(380, 126.799329725144), + Boardcore::TrajectoryPoint(390, 125.649718422777), + Boardcore::TrajectoryPoint(400, 124.496976446168), + Boardcore::TrajectoryPoint(410, 123.34056126544), + Boardcore::TrajectoryPoint(420, 122.180509976902), + Boardcore::TrajectoryPoint(430, 121.016802951695), + Boardcore::TrajectoryPoint(440, 119.848744663861), + Boardcore::TrajectoryPoint(450, 118.676639666519), + Boardcore::TrajectoryPoint(460, 117.500292741596), + Boardcore::TrajectoryPoint(470, 116.319079500839), + Boardcore::TrajectoryPoint(480, 115.133142803974), + Boardcore::TrajectoryPoint(490, 113.942311527574), + Boardcore::TrajectoryPoint(500, 112.746322118768), + Boardcore::TrajectoryPoint(510, 111.544594927024), + Boardcore::TrajectoryPoint(520, 110.33724720946), + Boardcore::TrajectoryPoint(530, 109.124012748825), + Boardcore::TrajectoryPoint(540, 107.904613502543), + Boardcore::TrajectoryPoint(550, 106.678759122208), + Boardcore::TrajectoryPoint(560, 105.446119252229), + Boardcore::TrajectoryPoint(570, 104.20624602044), + Boardcore::TrajectoryPoint(580, 102.959006518586), + Boardcore::TrajectoryPoint(590, 101.70405989436), + Boardcore::TrajectoryPoint(600, 100.441050103474), + Boardcore::TrajectoryPoint(610, 99.1696051650051), + Boardcore::TrajectoryPoint(620, 97.8893363608352), + Boardcore::TrajectoryPoint(630, 96.5998373756894), + Boardcore::TrajectoryPoint(640, 95.3006833738967), + Boardcore::TrajectoryPoint(650, 93.9913215174958), + Boardcore::TrajectoryPoint(660, 92.6711599958102), + Boardcore::TrajectoryPoint(670, 91.3398601407874), + Boardcore::TrajectoryPoint(680, 89.9969054349139), + Boardcore::TrajectoryPoint(690, 88.6417546799249), + Boardcore::TrajectoryPoint(700, 87.2736284235172), + Boardcore::TrajectoryPoint(710, 85.8915342286298), + Boardcore::TrajectoryPoint(720, 84.495329045546), + Boardcore::TrajectoryPoint(730, 83.0839992592729), + Boardcore::TrajectoryPoint(740, 81.6563362139503), + Boardcore::TrajectoryPoint(750, 80.2122288237985), + Boardcore::TrajectoryPoint(760, 78.7495241096846), + Boardcore::TrajectoryPoint(770, 77.2683301540632), + Boardcore::TrajectoryPoint(780, 75.7665150159221), + Boardcore::TrajectoryPoint(790, 74.2432756148519), + Boardcore::TrajectoryPoint(800, 72.697364219074), + Boardcore::TrajectoryPoint(810, 71.1267035461265), + Boardcore::TrajectoryPoint(820, 69.5296178585841), + Boardcore::TrajectoryPoint(830, 67.904442903485), + Boardcore::TrajectoryPoint(840, 66.2489725744482), + Boardcore::TrajectoryPoint(850, 64.5607861529751), + Boardcore::TrajectoryPoint(860, 62.8372226895241), + Boardcore::TrajectoryPoint(870, 61.0751513796656), + Boardcore::TrajectoryPoint(880, 59.2704288396001), + Boardcore::TrajectoryPoint(890, 57.4200840825586), + Boardcore::TrajectoryPoint(900, 55.5185850593929), + Boardcore::TrajectoryPoint(910, 53.5613537488966), + Boardcore::TrajectoryPoint(920, 51.5404632028398), + Boardcore::TrajectoryPoint(930, 49.4491473518355), + Boardcore::TrajectoryPoint(940, 47.2779714775322), + Boardcore::TrajectoryPoint(950, 45.0144239949148), + Boardcore::TrajectoryPoint(960, 42.6446752670154), + Boardcore::TrajectoryPoint(970, 40.1490582186216), + Boardcore::TrajectoryPoint(980, 37.5031758068286), + Boardcore::TrajectoryPoint(990, 34.6716124620735), + Boardcore::TrajectoryPoint(1000, 31.6051090189613), + Boardcore::TrajectoryPoint(1010, 28.2277775815275), + Boardcore::TrajectoryPoint(1020, 24.4083375534924), + Boardcore::TrajectoryPoint(1030, 19.8973916846481), + Boardcore::TrajectoryPoint(1040, 14.0434058201059), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t2_open[] = { + Boardcore::TrajectoryPoint(0, 169.131483352568), + Boardcore::TrajectoryPoint(10, 168.021987670571), + Boardcore::TrajectoryPoint(20, 166.913723128917), + Boardcore::TrajectoryPoint(30, 165.806162864455), + Boardcore::TrajectoryPoint(40, 164.699223769936), + Boardcore::TrajectoryPoint(50, 163.592880806674), + Boardcore::TrajectoryPoint(60, 162.48696357211), + Boardcore::TrajectoryPoint(70, 161.381387135012), + Boardcore::TrajectoryPoint(80, 160.276067113742), + Boardcore::TrajectoryPoint(90, 159.170930177318), + Boardcore::TrajectoryPoint(100, 158.065865683118), + Boardcore::TrajectoryPoint(110, 156.960783273119), + Boardcore::TrajectoryPoint(120, 155.85560006476), + Boardcore::TrajectoryPoint(130, 154.75018605909), + Boardcore::TrajectoryPoint(140, 153.644479765301), + Boardcore::TrajectoryPoint(150, 152.538399670524), + Boardcore::TrajectoryPoint(160, 151.431797030004), + Boardcore::TrajectoryPoint(170, 150.32459554215), + Boardcore::TrajectoryPoint(180, 149.216746710064), + Boardcore::TrajectoryPoint(190, 148.108068726142), + Boardcore::TrajectoryPoint(200, 146.998489024823), + Boardcore::TrajectoryPoint(210, 145.887986867122), + Boardcore::TrajectoryPoint(220, 144.776291119778), + Boardcore::TrajectoryPoint(230, 143.663435860165), + Boardcore::TrajectoryPoint(240, 142.549316088821), + Boardcore::TrajectoryPoint(250, 141.433669120944), + Boardcore::TrajectoryPoint(260, 140.316623407473), + Boardcore::TrajectoryPoint(270, 139.197802610314), + Boardcore::TrajectoryPoint(280, 138.077314802393), + Boardcore::TrajectoryPoint(290, 136.954914511937), + Boardcore::TrajectoryPoint(300, 135.830521193842), + Boardcore::TrajectoryPoint(310, 134.704044358959), + Boardcore::TrajectoryPoint(320, 133.575269699904), + Boardcore::TrajectoryPoint(330, 132.444191668923), + Boardcore::TrajectoryPoint(340, 131.310546362978), + Boardcore::TrajectoryPoint(350, 130.174314121649), + Boardcore::TrajectoryPoint(360, 129.035294199772), + Boardcore::TrajectoryPoint(370, 127.893325305339), + Boardcore::TrajectoryPoint(380, 126.748410822322), + Boardcore::TrajectoryPoint(390, 125.600092047644), + Boardcore::TrajectoryPoint(400, 124.448626738249), + Boardcore::TrajectoryPoint(410, 123.293431340795), + Boardcore::TrajectoryPoint(420, 122.134621991213), + Boardcore::TrajectoryPoint(430, 120.972106862744), + Boardcore::TrajectoryPoint(440, 119.80525091659), + Boardcore::TrajectoryPoint(450, 118.634337515645), + Boardcore::TrajectoryPoint(460, 117.459166068322), + Boardcore::TrajectoryPoint(470, 116.279071370591), + Boardcore::TrajectoryPoint(480, 115.094276344341), + Boardcore::TrajectoryPoint(490, 113.904570619208), + Boardcore::TrajectoryPoint(500, 112.709651512399), + Boardcore::TrajectoryPoint(510, 111.509016602242), + Boardcore::TrajectoryPoint(520, 110.302745103897), + Boardcore::TrajectoryPoint(530, 109.090570767566), + Boardcore::TrajectoryPoint(540, 107.872215529525), + Boardcore::TrajectoryPoint(550, 106.647389030539), + Boardcore::TrajectoryPoint(560, 105.415723322119), + Boardcore::TrajectoryPoint(570, 104.17684569139), + Boardcore::TrajectoryPoint(580, 102.930585819697), + Boardcore::TrajectoryPoint(590, 101.676602868099), + Boardcore::TrajectoryPoint(600, 100.414540812681), + Boardcore::TrajectoryPoint(610, 99.1440276993012), + Boardcore::TrajectoryPoint(620, 97.8646748425003), + Boardcore::TrajectoryPoint(630, 96.5760759650707), + Boardcore::TrajectoryPoint(640, 95.2778062743917), + Boardcore::TrajectoryPoint(650, 93.9693465967026), + Boardcore::TrajectoryPoint(660, 92.6500370079844), + Boardcore::TrajectoryPoint(670, 91.3195733673837), + Boardcore::TrajectoryPoint(680, 89.9774392216188), + Boardcore::TrajectoryPoint(690, 88.623093439841), + Boardcore::TrajectoryPoint(700, 87.2557874432964), + Boardcore::TrajectoryPoint(710, 85.8744659108293), + Boardcore::TrajectoryPoint(720, 84.4790179690958), + Boardcore::TrajectoryPoint(730, 83.0684589788966), + Boardcore::TrajectoryPoint(740, 81.6415210126344), + Boardcore::TrajectoryPoint(750, 80.1981513291283), + Boardcore::TrajectoryPoint(760, 78.736139656041), + Boardcore::TrajectoryPoint(770, 77.2556500277865), + Boardcore::TrajectoryPoint(780, 75.7544960787138), + Boardcore::TrajectoryPoint(790, 74.2319274499954), + Boardcore::TrajectoryPoint(800, 72.6866698993925), + Boardcore::TrajectoryPoint(810, 71.1166221010087), + Boardcore::TrajectoryPoint(820, 69.5201565200394), + Boardcore::TrajectoryPoint(830, 67.8955845346031), + Boardcore::TrajectoryPoint(840, 66.2406999587355), + Boardcore::TrajectoryPoint(850, 64.5530820144768), + Boardcore::TrajectoryPoint(860, 62.8300697117241), + Boardcore::TrajectoryPoint(870, 61.0685513214076), + Boardcore::TrajectoryPoint(880, 59.2643440977881), + Boardcore::TrajectoryPoint(890, 57.4144972399486), + Boardcore::TrajectoryPoint(900, 55.5134951186859), + Boardcore::TrajectoryPoint(910, 53.5567414342047), + Boardcore::TrajectoryPoint(920, 51.5362937467672), + Boardcore::TrajectoryPoint(930, 49.4454170152926), + Boardcore::TrajectoryPoint(940, 47.2746604442557), + Boardcore::TrajectoryPoint(950, 45.0115245871944), + Boardcore::TrajectoryPoint(960, 42.6421539307963), + Boardcore::TrajectoryPoint(970, 40.1469050382637), + Boardcore::TrajectoryPoint(980, 37.5013681713287), + Boardcore::TrajectoryPoint(990, 34.6701360530485), + Boardcore::TrajectoryPoint(1000, 31.6039384468464), + Boardcore::TrajectoryPoint(1010, 28.2268936080629), + Boardcore::TrajectoryPoint(1020, 24.4077114236162), + Boardcore::TrajectoryPoint(1030, 19.8970027623163), + Boardcore::TrajectoryPoint(1040, 14.0432280242359), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t3_open[] = { + Boardcore::TrajectoryPoint(0, 169.018549440047), + Boardcore::TrajectoryPoint(10, 167.911215676248), + Boardcore::TrajectoryPoint(20, 166.804991979826), + Boardcore::TrajectoryPoint(30, 165.69944525064), + Boardcore::TrajectoryPoint(40, 164.594489580683), + Boardcore::TrajectoryPoint(50, 163.4900905106), + Boardcore::TrajectoryPoint(60, 162.386106556436), + Boardcore::TrajectoryPoint(70, 161.282438017206), + Boardcore::TrajectoryPoint(80, 160.179000977387), + Boardcore::TrajectoryPoint(90, 159.075717361825), + Boardcore::TrajectoryPoint(100, 157.972485314413), + Boardcore::TrajectoryPoint(110, 156.869211734768), + Boardcore::TrajectoryPoint(120, 155.765814158809), + Boardcore::TrajectoryPoint(130, 154.662167229108), + Boardcore::TrajectoryPoint(140, 153.558198856204), + Boardcore::TrajectoryPoint(150, 152.453834669369), + Boardcore::TrajectoryPoint(160, 151.34893883593), + Boardcore::TrajectoryPoint(170, 150.243407479148), + Boardcore::TrajectoryPoint(180, 149.137207868512), + Boardcore::TrajectoryPoint(190, 148.030176802289), + Boardcore::TrajectoryPoint(200, 146.92220241072), + Boardcore::TrajectoryPoint(210, 145.813285675937), + Boardcore::TrajectoryPoint(220, 144.703177666616), + Boardcore::TrajectoryPoint(230, 143.591865696552), + Boardcore::TrajectoryPoint(240, 142.479297525719), + Boardcore::TrajectoryPoint(250, 141.365152795154), + Boardcore::TrajectoryPoint(260, 140.249590977913), + Boardcore::TrajectoryPoint(270, 139.132263851042), + Boardcore::TrajectoryPoint(280, 138.013220513944), + Boardcore::TrajectoryPoint(290, 136.892278067816), + Boardcore::TrajectoryPoint(300, 135.769290791518), + Boardcore::TrajectoryPoint(310, 134.644236068848), + Boardcore::TrajectoryPoint(320, 133.516829985779), + Boardcore::TrajectoryPoint(330, 132.387138507394), + Boardcore::TrajectoryPoint(340, 131.254825239606), + Boardcore::TrajectoryPoint(350, 130.119944224944), + Boardcore::TrajectoryPoint(360, 128.982220695094), + Boardcore::TrajectoryPoint(370, 127.841567982095), + Boardcore::TrajectoryPoint(380, 126.697915100134), + Boardcore::TrajectoryPoint(390, 125.550877777939), + Boardcore::TrajectoryPoint(400, 124.400678193037), + Boardcore::TrajectoryPoint(410, 123.246691766367), + Boardcore::TrajectoryPoint(420, 122.089113737498), + Boardcore::TrajectoryPoint(430, 120.927774770742), + Boardcore::TrajectoryPoint(440, 119.762116130352), + Boardcore::TrajectoryPoint(450, 118.592384167447), + Boardcore::TrajectoryPoint(460, 117.418355128934), + Boardcore::TrajectoryPoint(470, 116.239392235664), + Boardcore::TrajectoryPoint(480, 115.055729180526), + Boardcore::TrajectoryPoint(490, 113.867139450122), + Boardcore::TrajectoryPoint(500, 112.673281335156), + Boardcore::TrajectoryPoint(510, 111.473729462122), + Boardcore::TrajectoryPoint(520, 110.268525081955), + Boardcore::TrajectoryPoint(530, 109.05740191317), + Boardcore::TrajectoryPoint(540, 107.840081871549), + Boardcore::TrajectoryPoint(550, 106.616274587488), + Boardcore::TrajectoryPoint(560, 105.38557467042), + Boardcore::TrajectoryPoint(570, 104.147684275799), + Boardcore::TrajectoryPoint(580, 102.902395813242), + Boardcore::TrajectoryPoint(590, 101.649368457204), + Boardcore::TrajectoryPoint(600, 100.388246204042), + Boardcore::TrajectoryPoint(610, 99.1186571261929), + Boardcore::TrajectoryPoint(620, 97.8402125705544), + Boardcore::TrajectoryPoint(630, 96.5525062975941), + Boardcore::TrajectoryPoint(640, 95.2551135572692), + Boardcore::TrajectoryPoint(650, 93.9475486582521), + Boardcore::TrajectoryPoint(660, 92.6290839281749), + Boardcore::TrajectoryPoint(670, 91.2994495693191), + Boardcore::TrajectoryPoint(680, 89.9581291918365), + Boardcore::TrajectoryPoint(690, 88.6045817314495), + Boardcore::TrajectoryPoint(700, 87.2380892989704), + Boardcore::TrajectoryPoint(710, 85.8575340628243), + Boardcore::TrajectoryPoint(720, 84.4628371342171), + Boardcore::TrajectoryPoint(730, 83.0530426691203), + Boardcore::TrajectoryPoint(740, 81.6268238372804), + Boardcore::TrajectoryPoint(750, 80.1841858723471), + Boardcore::TrajectoryPoint(760, 78.7228615773764), + Boardcore::TrajectoryPoint(770, 77.2430705742164), + Boardcore::TrajectoryPoint(780, 75.7425724303685), + Boardcore::TrajectoryPoint(790, 74.2206691589271), + Boardcore::TrajectoryPoint(800, 72.6760601813762), + Boardcore::TrajectoryPoint(810, 71.1066202938045), + Boardcore::TrajectoryPoint(820, 69.510769835374), + Boardcore::TrajectoryPoint(830, 67.886795980158), + Boardcore::TrajectoryPoint(840, 66.232492463208), + Boardcore::TrajectoryPoint(850, 64.5454384474621), + Boardcore::TrajectoryPoint(860, 62.8229729023937), + Boardcore::TrajectoryPoint(870, 61.0620030375042), + Boardcore::TrajectoryPoint(880, 59.2583070265182), + Boardcore::TrajectoryPoint(890, 57.4089541098089), + Boardcore::TrajectoryPoint(900, 55.508444958503), + Boardcore::TrajectoryPoint(910, 53.5521651256781), + Boardcore::TrajectoryPoint(920, 51.5321567947398), + Boardcore::TrajectoryPoint(930, 49.4417157242628), + Boardcore::TrajectoryPoint(940, 47.2713751593836), + Boardcore::TrajectoryPoint(950, 45.0086477015002), + Boardcore::TrajectoryPoint(960, 42.6396521535451), + Boardcore::TrajectoryPoint(970, 40.1447685402118), + Boardcore::TrajectoryPoint(980, 37.4995745225391), + Boardcore::TrajectoryPoint(990, 34.6686710532882), + Boardcore::TrajectoryPoint(1000, 31.6027769079599), + Boardcore::TrajectoryPoint(1010, 28.2260164462406), + Boardcore::TrajectoryPoint(1020, 24.4070901103829), + Boardcore::TrajectoryPoint(1030, 19.8966168260871), + Boardcore::TrajectoryPoint(1040, 14.043051589917), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t4_open[] = { + Boardcore::TrajectoryPoint(0, 168.906587066945), + Boardcore::TrajectoryPoint(10, 167.801409690573), + Boardcore::TrajectoryPoint(20, 166.697207011095), + Boardcore::TrajectoryPoint(30, 165.593654322181), + Boardcore::TrajectoryPoint(40, 164.490666223961), + Boardcore::TrajectoryPoint(50, 163.388191156171), + Boardcore::TrajectoryPoint(60, 162.286121896822), + Boardcore::TrajectoryPoint(70, 161.184342976228), + Boardcore::TrajectoryPoint(80, 160.082770937899), + Boardcore::TrajectoryPoint(90, 158.981324724607), + Boardcore::TrajectoryPoint(100, 157.879907698388), + Boardcore::TrajectoryPoint(110, 156.778425804556), + Boardcore::TrajectoryPoint(120, 155.676796991135), + Boardcore::TrajectoryPoint(130, 154.574901930628), + Boardcore::TrajectoryPoint(140, 153.472655130933), + Boardcore::TrajectoryPoint(150, 152.36999076217), + Boardcore::TrajectoryPoint(160, 151.266787017324), + Boardcore::TrajectoryPoint(170, 150.162910201622), + Boardcore::TrajectoryPoint(180, 149.058344464392), + Boardcore::TrajectoryPoint(190, 147.952946108577), + Boardcore::TrajectoryPoint(200, 146.846562155861), + Boardcore::TrajectoryPoint(210, 145.739216200288), + Boardcore::TrajectoryPoint(220, 144.630682244579), + Boardcore::TrajectoryPoint(230, 143.520899375052), + Boardcore::TrajectoryPoint(240, 142.409869418917), + Boardcore::TrajectoryPoint(250, 141.297213178154), + Boardcore::TrajectoryPoint(260, 140.183121714668), + Boardcore::TrajectoryPoint(270, 139.067275405684), + Boardcore::TrajectoryPoint(280, 137.949663417069), + Boardcore::TrajectoryPoint(290, 136.830166276048), + Boardcore::TrajectoryPoint(300, 135.708572326618), + Boardcore::TrajectoryPoint(310, 134.584927495098), + Boardcore::TrajectoryPoint(320, 133.45887766802), + Boardcore::TrajectoryPoint(330, 132.330560842128), + Boardcore::TrajectoryPoint(340, 131.199567675304), + Boardcore::TrajectoryPoint(350, 130.066026309233), + Boardcore::TrajectoryPoint(360, 128.929587605292), + Boardcore::TrajectoryPoint(370, 127.790239817395), + Boardcore::TrajectoryPoint(380, 126.647837330158), + Boardcore::TrajectoryPoint(390, 125.502070524505), + Boardcore::TrajectoryPoint(400, 124.353125859225), + Boardcore::TrajectoryPoint(410, 123.200337733118), + Boardcore::TrajectoryPoint(420, 122.043980540315), + Boardcore::TrajectoryPoint(430, 120.883807398833), + Boardcore::TrajectoryPoint(440, 119.719335896335), + Boardcore::TrajectoryPoint(450, 118.550775340835), + Boardcore::TrajectoryPoint(460, 117.377862804209), + Boardcore::TrajectoryPoint(470, 116.20003806833), + Boardcore::TrajectoryPoint(480, 115.01749740655), + Boardcore::TrajectoryPoint(490, 113.830014234307), + Boardcore::TrajectoryPoint(500, 112.637207921443), + Boardcore::TrajectoryPoint(510, 111.438729956832), + Boardcore::TrajectoryPoint(520, 110.234583707746), + Boardcore::TrajectoryPoint(530, 109.024502861849), + Boardcore::TrajectoryPoint(540, 107.808209315058), + Boardcore::TrajectoryPoint(550, 106.585412687852), + Boardcore::TrajectoryPoint(560, 105.355670298971), + Boardcore::TrajectoryPoint(570, 104.11875887977), + Boardcore::TrajectoryPoint(580, 102.874433707719), + Boardcore::TrajectoryPoint(590, 101.622353970683), + Boardcore::TrajectoryPoint(600, 100.362163685198), + Boardcore::TrajectoryPoint(610, 99.0934909500701), + Boardcore::TrajectoryPoint(620, 97.8159471442535), + Boardcore::TrajectoryPoint(630, 96.5291260655025), + Boardcore::TrajectoryPoint(640, 95.2326030058845), + Boardcore::TrajectoryPoint(650, 93.9259255756011), + Boardcore::TrajectoryPoint(660, 92.6082987172587), + Boardcore::TrajectoryPoint(670, 91.2794867930224), + Boardcore::TrajectoryPoint(680, 89.9389734756901), + Boardcore::TrajectoryPoint(690, 88.5862177667234), + Boardcore::TrajectoryPoint(700, 87.220532283807), + Boardcore::TrajectoryPoint(710, 85.8407370560265), + Boardcore::TrajectoryPoint(720, 84.4467849886527), + Boardcore::TrajectoryPoint(730, 83.0377488536409), + Boardcore::TrajectoryPoint(740, 81.6122432842374), + Boardcore::TrajectoryPoint(750, 80.1703215296299), + Boardcore::TrajectoryPoint(760, 78.7096886114444), + Boardcore::TrajectoryPoint(770, 77.2305905999085), + Boardcore::TrajectoryPoint(780, 75.7307429428338), + Boardcore::TrajectoryPoint(790, 74.2094996787707), + Boardcore::TrajectoryPoint(800, 72.6655340655436), + Boardcore::TrajectoryPoint(810, 71.0966971850236), + Boardcore::TrajectoryPoint(820, 69.5014569248545), + Boardcore::TrajectoryPoint(830, 67.8780764183671), + Boardcore::TrajectoryPoint(840, 66.2243493222208), + Boardcore::TrajectoryPoint(850, 64.5378547406061), + Boardcore::TrajectoryPoint(860, 62.8159316027102), + Boardcore::TrajectoryPoint(870, 61.0555059212506), + Boardcore::TrajectoryPoint(880, 59.2523170678806), + Boardcore::TrajectoryPoint(890, 57.4034541812121), + Boardcore::TrajectoryPoint(900, 55.503434114371), + Boardcore::TrajectoryPoint(910, 53.5476244033823), + Boardcore::TrajectoryPoint(920, 51.5280519681874), + Boardcore::TrajectoryPoint(930, 49.4380431408588), + Boardcore::TrajectoryPoint(940, 47.2681153237535), + Boardcore::TrajectoryPoint(950, 45.0057930764432), + Boardcore::TrajectoryPoint(960, 42.6371697085639), + Boardcore::TrajectoryPoint(970, 40.1426485313466), + Boardcore::TrajectoryPoint(980, 37.4977946987562), + Boardcore::TrajectoryPoint(990, 34.667217331054), + Boardcore::TrajectoryPoint(1000, 31.6016242981433), + Boardcore::TrajectoryPoint(1010, 28.2251460176322), + Boardcore::TrajectoryPoint(1020, 24.4064735584277), + Boardcore::TrajectoryPoint(1030, 19.8962338417036), + Boardcore::TrajectoryPoint(1040, 14.04287650157), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t5_open[] = { + Boardcore::TrajectoryPoint(0, 168.795584094774), + Boardcore::TrajectoryPoint(10, 167.692557307917), + Boardcore::TrajectoryPoint(20, 166.590356097278), + Boardcore::TrajectoryPoint(30, 165.488778228285), + Boardcore::TrajectoryPoint(40, 164.387738801583), + Boardcore::TrajectoryPoint(50, 163.287171349686), + Boardcore::TrajectoryPoint(60, 162.186998460698), + Boardcore::TrajectoryPoint(70, 161.08709113554), + Boardcore::TrajectoryPoint(80, 159.987366369802), + Boardcore::TrajectoryPoint(90, 158.88774182662), + Boardcore::TrajectoryPoint(100, 157.788122639031), + Boardcore::TrajectoryPoint(110, 156.688415524839), + Boardcore::TrajectoryPoint(120, 155.588538837923), + Boardcore::TrajectoryPoint(130, 154.488380621634), + Boardcore::TrajectoryPoint(140, 153.387839273574), + Boardcore::TrajectoryPoint(150, 152.286858854869), + Boardcore::TrajectoryPoint(160, 151.185332658691), + Boardcore::TrajectoryPoint(170, 150.083095008443), + Boardcore::TrajectoryPoint(180, 148.980148006983), + Boardcore::TrajectoryPoint(190, 147.876368328361), + Boardcore::TrajectoryPoint(200, 146.771560146817), + Boardcore::TrajectoryPoint(210, 145.665770526275), + Boardcore::TrajectoryPoint(220, 144.558797108522), + Boardcore::TrajectoryPoint(230, 143.450529343185), + Boardcore::TrajectoryPoint(240, 142.341005103139), + Boardcore::TrajectoryPoint(250, 141.229843069594), + Boardcore::TrajectoryPoint(260, 140.11720860009), + Boardcore::TrajectoryPoint(270, 139.00283041646), + Boardcore::TrajectoryPoint(280, 137.886636830349), + Boardcore::TrajectoryPoint(290, 136.76857261171), + Boardcore::TrajectoryPoint(300, 135.648359444435), + Boardcore::TrajectoryPoint(310, 134.526112435869), + Boardcore::TrajectoryPoint(320, 133.401406709066), + Boardcore::TrajectoryPoint(330, 132.274452784611), + Boardcore::TrajectoryPoint(340, 131.144767940103), + Boardcore::TrajectoryPoint(350, 130.012554789651), + Boardcore::TrajectoryPoint(360, 128.877389498505), + Boardcore::TrajectoryPoint(370, 127.739335520449), + Boardcore::TrajectoryPoint(380, 126.59817236926), + Boardcore::TrajectoryPoint(390, 125.453665281202), + Boardcore::TrajectoryPoint(400, 124.305964866261), + Boardcore::TrajectoryPoint(410, 123.154364510289), + Boardcore::TrajectoryPoint(420, 121.999217800315), + Boardcore::TrajectoryPoint(430, 120.840200282556), + Boardcore::TrajectoryPoint(440, 119.67690587732), + Boardcore::TrajectoryPoint(450, 118.509506824211), + Boardcore::TrajectoryPoint(460, 117.337701080435), + Boardcore::TrajectoryPoint(470, 116.161004906083), + Boardcore::TrajectoryPoint(480, 114.979577179653), + Boardcore::TrajectoryPoint(490, 113.793191246996), + Boardcore::TrajectoryPoint(500, 112.601427664848), + Boardcore::TrajectoryPoint(510, 111.404014593817), + Boardcore::TrajectoryPoint(520, 110.200917600784), + Boardcore::TrajectoryPoint(530, 108.99187034337), + Boardcore::TrajectoryPoint(540, 107.77659469824), + Boardcore::TrajectoryPoint(550, 106.554800276382), + Boardcore::TrajectoryPoint(560, 105.326007257754), + Boardcore::TrajectoryPoint(570, 104.090066655839), + Boardcore::TrajectoryPoint(580, 102.846696756374), + Boardcore::TrajectoryPoint(590, 101.595556760649), + Boardcore::TrajectoryPoint(600, 100.336290705274), + Boardcore::TrajectoryPoint(610, 99.0685267152186), + Boardcore::TrajectoryPoint(620, 97.7918762011947), + Boardcore::TrajectoryPoint(630, 96.5059329978569), + Boardcore::TrajectoryPoint(640, 95.2102724389206), + Boardcore::TrajectoryPoint(650, 93.9044503648583), + Boardcore::TrajectoryPoint(660, 92.5876793685652), + Boardcore::TrajectoryPoint(670, 91.2596831159777), + Boardcore::TrajectoryPoint(680, 89.9199702329934), + Boardcore::TrajectoryPoint(690, 88.5679997859932), + Boardcore::TrajectoryPoint(700, 87.2031147181269), + Boardcore::TrajectoryPoint(710, 85.8240732876301), + Boardcore::TrajectoryPoint(720, 84.4308600046887), + Boardcore::TrajectoryPoint(730, 83.0225760794811), + Boardcore::TrajectoryPoint(740, 81.5977779720038), + Boardcore::TrajectoryPoint(750, 80.1565488685423), + Boardcore::TrajectoryPoint(760, 78.6966195158749), + Boardcore::TrajectoryPoint(770, 77.2182089301946), + Boardcore::TrajectoryPoint(780, 75.7190065057806), + Boardcore::TrajectoryPoint(790, 74.1984179633334), + Boardcore::TrajectoryPoint(800, 72.6550905680859), + Boardcore::TrajectoryPoint(810, 71.0868518498875), + Boardcore::TrajectoryPoint(820, 69.4922169225084), + Boardcore::TrajectoryPoint(830, 67.869425040289), + Boardcore::TrajectoryPoint(840, 66.2162697820797), + Boardcore::TrajectoryPoint(850, 64.530330193674), + Boardcore::TrajectoryPoint(860, 62.8089451641102), + Boardcore::TrajectoryPoint(870, 61.0490593753814), + Boardcore::TrajectoryPoint(880, 59.2463736726344), + Boardcore::TrajectoryPoint(890, 57.3979969511595), + Boardcore::TrajectoryPoint(900, 55.4984621290173), + Boardcore::TrajectoryPoint(910, 53.5431082425134), + Boardcore::TrajectoryPoint(920, 51.5239788943938), + Boardcore::TrajectoryPoint(930, 49.4343989324123), + Boardcore::TrajectoryPoint(940, 47.2648806428178), + Boardcore::TrajectoryPoint(950, 45.0029604546617), + Boardcore::TrajectoryPoint(960, 42.6347063726429), + Boardcore::TrajectoryPoint(970, 40.1405448215165), + Boardcore::TrajectoryPoint(980, 37.4960285407582), + Boardcore::TrajectoryPoint(990, 34.6657747566259), + Boardcore::TrajectoryPoint(1000, 31.6004805148312), + Boardcore::TrajectoryPoint(1010, 28.2242822450064), + Boardcore::TrajectoryPoint(1020, 24.405861713229), + Boardcore::TrajectoryPoint(1030, 19.8958537754282), + Boardcore::TrajectoryPoint(1040, 14.0427027438515), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t6_open[] = { + Boardcore::TrajectoryPoint(0, 168.685570230743), + Boardcore::TrajectoryPoint(10, 167.584646329466), + Boardcore::TrajectoryPoint(20, 166.484427314696), + Boardcore::TrajectoryPoint(30, 165.384805314988), + Boardcore::TrajectoryPoint(40, 164.28569592402), + Boardcore::TrajectoryPoint(50, 163.187019887333), + Boardcore::TrajectoryPoint(60, 162.088725300665), + Boardcore::TrajectoryPoint(70, 160.990671799155), + Boardcore::TrajectoryPoint(80, 159.892776823657), + Boardcore::TrajectoryPoint(90, 158.794958276955), + Boardcore::TrajectoryPoint(100, 157.697120109588), + Boardcore::TrajectoryPoint(110, 156.599171102938), + Boardcore::TrajectoryPoint(120, 155.501030136122), + Boardcore::TrajectoryPoint(130, 154.402593918367), + Boardcore::TrajectoryPoint(140, 153.303742122402), + Boardcore::TrajectoryPoint(150, 152.204430003618), + Boardcore::TrajectoryPoint(160, 151.104566992153), + Boardcore::TrajectoryPoint(170, 150.003953342255), + Boardcore::TrajectoryPoint(180, 148.902610145574), + Boardcore::TrajectoryPoint(190, 147.800435282399), + Boardcore::TrajectoryPoint(200, 146.697188403928), + Boardcore::TrajectoryPoint(210, 145.592940870211), + Boardcore::TrajectoryPoint(220, 144.48751464093), + Boardcore::TrajectoryPoint(230, 143.380748172669), + Boardcore::TrajectoryPoint(240, 142.272706918421), + Boardcore::TrajectoryPoint(250, 141.163035387437), + Boardcore::TrajectoryPoint(260, 140.051844731605), + Boardcore::TrajectoryPoint(270, 138.938922138147), + Boardcore::TrajectoryPoint(280, 137.824134181808), + Boardcore::TrajectoryPoint(290, 136.707490656827), + Boardcore::TrajectoryPoint(300, 135.588645894221), + Boardcore::TrajectoryPoint(310, 134.467784790821), + Boardcore::TrajectoryPoint(320, 133.34441116998), + Boardcore::TrajectoryPoint(330, 132.218808542547), + Boardcore::TrajectoryPoint(340, 131.090420397478), + Boardcore::TrajectoryPoint(350, 129.959524172423), + Boardcore::TrajectoryPoint(360, 128.825621031295), + Boardcore::TrajectoryPoint(370, 127.6888498866), + Boardcore::TrajectoryPoint(380, 126.548915157874), + Boardcore::TrajectoryPoint(390, 125.405657123228), + Boardcore::TrajectoryPoint(400, 124.259156659437), + Boardcore::TrajectoryPoint(410, 123.10876744383), + Boardcore::TrajectoryPoint(420, 121.954820992701), + Boardcore::TrajectoryPoint(430, 120.796949029684), + Boardcore::TrajectoryPoint(440, 119.63482180624), + Boardcore::TrajectoryPoint(450, 118.468574474068), + Boardcore::TrajectoryPoint(460, 117.297865939559), + Boardcore::TrajectoryPoint(470, 116.122288850337), + Boardcore::TrajectoryPoint(480, 114.941964719029), + Boardcore::TrajectoryPoint(490, 113.756666823441), + Boardcore::TrajectoryPoint(500, 112.565937016961), + Boardcore::TrajectoryPoint(510, 111.369579936659), + Boardcore::TrajectoryPoint(520, 110.16752343488), + Boardcore::TrajectoryPoint(530, 108.959501139991), + Boardcore::TrajectoryPoint(540, 107.745234909991), + Boardcore::TrajectoryPoint(550, 106.524434346794), + Boardcore::TrajectoryPoint(560, 105.296582643942), + Boardcore::TrajectoryPoint(570, 104.061604802052), + Boardcore::TrajectoryPoint(580, 102.819182256317), + Boardcore::TrajectoryPoint(590, 101.568974221457), + Boardcore::TrajectoryPoint(600, 100.310624754052), + Boardcore::TrajectoryPoint(610, 99.043762005029), + Boardcore::TrajectoryPoint(620, 97.7679974165565), + Boardcore::TrajectoryPoint(630, 96.4829248598084), + Boardcore::TrajectoryPoint(640, 95.1881197096899), + Boardcore::TrajectoryPoint(650, 93.8831378521849), + Boardcore::TrajectoryPoint(660, 92.5672239072352), + Boardcore::TrajectoryPoint(670, 91.2400366461118), + Boardcore::TrajectoryPoint(680, 89.9011176526654), + Boardcore::TrajectoryPoint(690, 88.5499260573886), + Boardcore::TrajectoryPoint(700, 87.1858349487717), + Boardcore::TrajectoryPoint(710, 85.8075411801055), + Boardcore::TrajectoryPoint(720, 84.4150606786734), + Boardcore::TrajectoryPoint(730, 83.007522916533), + Boardcore::TrajectoryPoint(740, 81.5834265407946), + Boardcore::TrajectoryPoint(750, 80.1428844882167), + Boardcore::TrajectoryPoint(760, 78.6836530677857), + Boardcore::TrajectoryPoint(770, 77.205924408817), + Boardcore::TrajectoryPoint(780, 75.7073620262579), + Boardcore::TrajectoryPoint(790, 74.1874229827818), + Boardcore::TrajectoryPoint(800, 72.6447287205635), + Boardcore::TrajectoryPoint(810, 71.0770833780436), + Boardcore::TrajectoryPoint(820, 69.4830489758586), + Boardcore::TrajectoryPoint(830, 67.8608410495752), + Boardcore::TrajectoryPoint(840, 66.2082531008103), + Boardcore::TrajectoryPoint(850, 64.5228641173075), + Boardcore::TrajectoryPoint(860, 62.8020129480924), + Boardcore::TrajectoryPoint(870, 61.0426628118896), + Boardcore::TrajectoryPoint(880, 59.2404763000421), + Boardcore::TrajectoryPoint(890, 57.39258192443), + Boardcore::TrajectoryPoint(900, 55.4935285522326), + Boardcore::TrajectoryPoint(910, 53.5386220306471), + Boardcore::TrajectoryPoint(920, 51.5199372063863), + Boardcore::TrajectoryPoint(930, 49.4307827713754), + Boardcore::TrajectoryPoint(940, 47.2616708265571), + Boardcore::TrajectoryPoint(950, 45.000149582747), + Boardcore::TrajectoryPoint(960, 42.6322619259962), + Boardcore::TrajectoryPoint(970, 40.1384572234834), + Boardcore::TrajectoryPoint(980, 37.4942758917594), + Boardcore::TrajectoryPoint(990, 34.6643432022668), + Boardcore::TrajectoryPoint(1000, 31.599345457024), + Boardcore::TrajectoryPoint(1010, 28.2234250523099), + Boardcore::TrajectoryPoint(1020, 24.4052545210955), + Boardcore::TrajectoryPoint(1030, 19.8954765940369), + Boardcore::TrajectoryPoint(1040, 14.0425303016496), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t7_open[] = { + Boardcore::TrajectoryPoint(0, 168.576758674548), + Boardcore::TrajectoryPoint(10, 167.477664759087), + Boardcore::TrajectoryPoint(20, 166.379408937412), + Boardcore::TrajectoryPoint(30, 165.281724121234), + Boardcore::TrajectoryPoint(40, 164.184526389915), + Boardcore::TrajectoryPoint(50, 163.087732411267), + Boardcore::TrajectoryPoint(60, 161.991291650769), + Boardcore::TrajectoryPoint(70, 160.895074448018), + Boardcore::TrajectoryPoint(80, 159.798992022537), + Boardcore::TrajectoryPoint(90, 158.702962703546), + Boardcore::TrajectoryPoint(100, 157.606890249155), + Boardcore::TrajectoryPoint(110, 156.510682907825), + Boardcore::TrajectoryPoint(120, 155.414261480221), + Boardcore::TrajectoryPoint(130, 154.317532592117), + Boardcore::TrajectoryPoint(140, 153.220354666761), + Boardcore::TrajectoryPoint(150, 152.122695411745), + Boardcore::TrajectoryPoint(160, 151.02447466393), + Boardcore::TrajectoryPoint(170, 149.925476786568), + Boardcore::TrajectoryPoint(180, 148.825722666637), + Boardcore::TrajectoryPoint(190, 147.725138906152), + Boardcore::TrajectoryPoint(200, 146.623439078599), + Boardcore::TrajectoryPoint(210, 145.520719575995), + Boardcore::TrajectoryPoint(220, 144.416827349337), + Boardcore::TrajectoryPoint(230, 143.311548556918), + Boardcore::TrajectoryPoint(240, 142.204976870697), + Boardcore::TrajectoryPoint(250, 141.096783165562), + Boardcore::TrajectoryPoint(260, 139.987023319395), + Boardcore::TrajectoryPoint(270, 138.875543935805), + Boardcore::TrajectoryPoint(280, 137.762149006706), + Boardcore::TrajectoryPoint(290, 136.646914098217), + Boardcore::TrajectoryPoint(300, 135.529425527086), + Boardcore::TrajectoryPoint(310, 134.409938559071), + Boardcore::TrajectoryPoint(320, 133.287885208454), + Boardcore::TrajectoryPoint(330, 132.163622417914), + Boardcore::TrajectoryPoint(340, 131.036519502466), + Boardcore::TrajectoryPoint(350, 129.906929053034), + Boardcore::TrajectoryPoint(360, 128.774276946874), + Boardcore::TrajectoryPoint(370, 127.638777795595), + Boardcore::TrajectoryPoint(380, 126.500060718327), + Boardcore::TrajectoryPoint(390, 125.358041205484), + Boardcore::TrajectoryPoint(400, 124.212725786401), + Boardcore::TrajectoryPoint(410, 123.063541954857), + Boardcore::TrajectoryPoint(420, 121.910785665741), + Boardcore::TrajectoryPoint(430, 120.754049318777), + Boardcore::TrajectoryPoint(440, 119.593079484775), + Boardcore::TrajectoryPoint(450, 118.42797421363), + Boardcore::TrajectoryPoint(460, 117.258353428119), + Boardcore::TrajectoryPoint(470, 116.083886065141), + Boardcore::TrajectoryPoint(480, 114.904656304586), + Boardcore::TrajectoryPoint(490, 113.720437357707), + Boardcore::TrajectoryPoint(500, 112.530732486217), + Boardcore::TrajectoryPoint(510, 111.335422603955), + Boardcore::TrajectoryPoint(520, 110.134397937061), + Boardcore::TrajectoryPoint(530, 108.927392085409), + Boardcore::TrajectoryPoint(540, 107.714126888904), + Boardcore::TrajectoryPoint(550, 106.494282983142), + Boardcore::TrajectoryPoint(560, 105.267393600956), + Boardcore::TrajectoryPoint(570, 104.03337056106), + Boardcore::TrajectoryPoint(580, 102.791887547647), + Boardcore::TrajectoryPoint(590, 101.54260378887), + Boardcore::TrajectoryPoint(600, 100.28516336117), + Boardcore::TrajectoryPoint(610, 99.0191944412223), + Boardcore::TrajectoryPoint(620, 97.7443085023561), + Boardcore::TrajectoryPoint(630, 96.4600994518848), + Boardcore::TrajectoryPoint(640, 95.1661427054507), + Boardcore::TrajectoryPoint(650, 93.8619941940291), + Boardcore::TrajectoryPoint(660, 92.546930389593), + Boardcore::TrajectoryPoint(670, 91.2205455211946), + Boardcore::TrajectoryPoint(680, 89.8824139521582), + Boardcore::TrajectoryPoint(690, 88.5319948762927), + Boardcore::TrajectoryPoint(700, 87.1686913485823), + Boardcore::TrajectoryPoint(710, 85.7911391807029), + Boardcore::TrajectoryPoint(720, 84.3993855305449), + Boardcore::TrajectoryPoint(730, 82.9925879571092), + Boardcore::TrajectoryPoint(740, 81.5691876521153), + Boardcore::TrajectoryPoint(750, 80.1293271176714), + Boardcore::TrajectoryPoint(760, 78.6707880634015), + Boardcore::TrajectoryPoint(770, 77.1937358975682), + Boardcore::TrajectoryPoint(780, 75.6958084283538), + Boardcore::TrajectoryPoint(790, 74.1765137233225), + Boardcore::TrajectoryPoint(800, 72.6344475696059), + Boardcore::TrajectoryPoint(810, 71.067390873285), + Boardcore::TrajectoryPoint(820, 69.4739522456603), + Boardcore::TrajectoryPoint(830, 67.8523236622251), + Boardcore::TrajectoryPoint(840, 66.2002985479306), + Boardcore::TrajectoryPoint(850, 64.5154558328133), + Boardcore::TrajectoryPoint(860, 62.7951343260225), + Boardcore::TrajectoryPoint(870, 61.0363156518462), + Boardcore::TrajectoryPoint(880, 59.2346244177048), + Boardcore::TrajectoryPoint(890, 57.3872086134296), + Boardcore::TrajectoryPoint(900, 55.4886329407341), + Boardcore::TrajectoryPoint(910, 53.5341702952857), + Boardcore::TrajectoryPoint(920, 51.5159265428244), + Boardcore::TrajectoryPoint(930, 49.4271943352214), + Boardcore::TrajectoryPoint(940, 47.2584855893927), + Boardcore::TrajectoryPoint(950, 44.9973602111657), + Boardcore::TrajectoryPoint(960, 42.6298361521946), + Boardcore::TrajectoryPoint(970, 40.1363855528654), + Boardcore::TrajectoryPoint(980, 37.4925365973632), + Boardcore::TrajectoryPoint(990, 34.6629225421831), + Boardcore::TrajectoryPoint(1000, 31.598219025257), + Boardcore::TrajectoryPoint(1010, 28.222574364643), + Boardcore::TrajectoryPoint(1020, 24.404651929149), + Boardcore::TrajectoryPoint(1030, 19.8951022648077), + Boardcore::TrajectoryPoint(1040, 14.042359160082), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t8_open[] = { + Boardcore::TrajectoryPoint(0, 168.46886387969), + Boardcore::TrajectoryPoint(10, 167.371600799283), + Boardcore::TrajectoryPoint(20, 166.275289433288), + Boardcore::TrajectoryPoint(30, 165.179523375033), + Boardcore::TrajectoryPoint(40, 164.084219182355), + Boardcore::TrajectoryPoint(50, 162.989293988637), + Boardcore::TrajectoryPoint(60, 161.894686922867), + Boardcore::TrajectoryPoint(70, 160.800288736459), + Boardcore::TrajectoryPoint(80, 159.706001858579), + Boardcore::TrajectoryPoint(90, 158.611745038509), + Boardcore::TrajectoryPoint(100, 157.517423359333), + Boardcore::TrajectoryPoint(110, 156.422941466866), + Boardcore::TrajectoryPoint(120, 155.328223619085), + Boardcore::TrajectoryPoint(130, 154.233187566099), + Boardcore::TrajectoryPoint(140, 153.13766804403), + Boardcore::TrajectoryPoint(150, 152.041646426804), + Boardcore::TrajectoryPoint(160, 150.945042898062), + Boardcore::TrajectoryPoint(170, 149.847657062916), + Boardcore::TrajectoryPoint(180, 148.749477491067), + Boardcore::TrajectoryPoint(190, 147.650448858313), + Boardcore::TrajectoryPoint(200, 146.550304450661), + Boardcore::TrajectoryPoint(210, 145.449099112549), + Boardcore::TrajectoryPoint(220, 144.346727863797), + Boardcore::TrajectoryPoint(230, 143.242923308588), + Boardcore::TrajectoryPoint(240, 142.137807951998), + Boardcore::TrajectoryPoint(250, 141.031079551436), + Boardcore::TrajectoryPoint(260, 139.922737684132), + Boardcore::TrajectoryPoint(270, 138.812689282553), + Boardcore::TrajectoryPoint(280, 137.700674945383), + Boardcore::TrajectoryPoint(290, 136.586836725379), + Boardcore::TrajectoryPoint(300, 135.470692293954), + Boardcore::TrajectoryPoint(310, 134.352557901333), + Boardcore::TrajectoryPoint(320, 133.231823076879), + Boardcore::TrajectoryPoint(330, 132.108880822872), + Boardcore::TrajectoryPoint(340, 130.983059799829), + Boardcore::TrajectoryPoint(350, 129.854764114432), + Boardcore::TrajectoryPoint(360, 128.723352073366), + Boardcore::TrajectoryPoint(370, 127.589114209884), + Boardcore::TrajectoryPoint(380, 126.451604153197), + Boardcore::TrajectoryPoint(390, 125.310812760981), + Boardcore::TrajectoryPoint(400, 124.166672084618), + Boardcore::TrajectoryPoint(410, 123.018683538149), + Boardcore::TrajectoryPoint(420, 121.867107439301), + Boardcore::TrajectoryPoint(430, 120.711496897768), + Boardcore::TrajectoryPoint(440, 119.55167478198), + Boardcore::TrajectoryPoint(450, 118.387702031519), + Boardcore::TrajectoryPoint(460, 117.219159655951), + Boardcore::TrajectoryPoint(470, 116.045792775939), + Boardcore::TrajectoryPoint(480, 114.86764827574), + Boardcore::TrajectoryPoint(490, 113.684499301507), + Boardcore::TrajectoryPoint(500, 112.495810636763), + Boardcore::TrajectoryPoint(510, 111.301539268224), + Boardcore::TrajectoryPoint(520, 110.101537886512), + Boardcore::TrajectoryPoint(530, 108.895540063746), + Boardcore::TrajectoryPoint(540, 107.683267622292), + Boardcore::TrajectoryPoint(550, 106.464364616841), + Boardcore::TrajectoryPoint(560, 105.238437317554), + Boardcore::TrajectoryPoint(570, 104.005361219239), + Boardcore::TrajectoryPoint(580, 102.764810012608), + Boardcore::TrajectoryPoint(590, 101.516442939243), + Boardcore::TrajectoryPoint(600, 100.259904095332), + Boardcore::TrajectoryPoint(610, 98.9948216830954), + Boardcore::TrajectoryPoint(620, 97.7208072067237), + Boardcore::TrajectoryPoint(630, 96.4374546092949), + Boardcore::TrajectoryPoint(640, 95.1443393467403), + Boardcore::TrajectoryPoint(650, 93.8410173945125), + Boardcore::TrajectoryPoint(660, 92.5267969025352), + Boardcore::TrajectoryPoint(670, 91.201207908254), + Boardcore::TrajectoryPoint(680, 89.8638573768977), + Boardcore::TrajectoryPoint(690, 88.5142045648081), + Boardcore::TrajectoryPoint(700, 87.1516823158908), + Boardcore::TrajectoryPoint(710, 85.7748657609686), + Boardcore::TrajectoryPoint(720, 84.3838331033719), + Boardcore::TrajectoryPoint(730, 82.977769815506), + Boardcore::TrajectoryPoint(740, 81.555059988348), + Boardcore::TrajectoryPoint(750, 80.1158755057223), + Boardcore::TrajectoryPoint(760, 78.658023317683), + Boardcore::TrajectoryPoint(770, 77.181642275941), + Boardcore::TrajectoryPoint(780, 75.6843446528651), + Boardcore::TrajectoryPoint(790, 74.1656891868915), + Boardcore::TrajectoryPoint(800, 72.6242461766202), + Boardcore::TrajectoryPoint(810, 71.0577734532764), + Boardcore::TrajectoryPoint(820, 69.4649259056458), + Boardcore::TrajectoryPoint(830, 67.843872106348), + Boardcore::TrajectoryPoint(840, 66.1924054042294), + Boardcore::TrajectoryPoint(850, 64.5081046719584), + Boardcore::TrajectoryPoint(860, 62.7883086789434), + Boardcore::TrajectoryPoint(870, 61.0300173252268), + Boardcore::TrajectoryPoint(880, 59.2288175014017), + Boardcore::TrajectoryPoint(890, 57.381876538045), + Boardcore::TrajectoryPoint(900, 55.4837748580334), + Boardcore::TrajectoryPoint(910, 53.5297526405762), + Boardcore::TrajectoryPoint(920, 51.5119465478925), + Boardcore::TrajectoryPoint(930, 49.4236333063493), + Boardcore::TrajectoryPoint(940, 47.255324650102), + Boardcore::TrajectoryPoint(950, 44.9945920941872), + Boardcore::TrajectoryPoint(960, 42.6274288381029), + Boardcore::TrajectoryPoint(970, 40.1343296280831), + Boardcore::TrajectoryPoint(980, 37.4908105055163), + Boardcore::TrajectoryPoint(990, 34.6615126524883), + Boardcore::TrajectoryPoint(1000, 31.5971011215711), + Boardcore::TrajectoryPoint(1010, 28.2217301082383), + Boardcore::TrajectoryPoint(1020, 24.4040538853095), + Boardcore::TrajectoryPoint(1030, 19.8947307555111), + Boardcore::TrajectoryPoint(1040, 14.0421893044884), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t9_open[] = { + Boardcore::TrajectoryPoint(0, 168.361874651667), + Boardcore::TrajectoryPoint(10, 167.266442847243), + Boardcore::TrajectoryPoint(20, 166.172057460137), + Boardcore::TrajectoryPoint(30, 165.078191989716), + Boardcore::TrajectoryPoint(40, 163.984763465206), + Boardcore::TrajectoryPoint(50, 162.891689478684), + Boardcore::TrajectoryPoint(60, 161.798900703067), + Boardcore::TrajectoryPoint(70, 160.706304488736), + Boardcore::TrajectoryPoint(80, 159.613796389609), + Boardcore::TrajectoryPoint(90, 158.521295568787), + Boardcore::TrajectoryPoint(100, 157.428709900975), + Boardcore::TrajectoryPoint(110, 156.335937462659), + Boardcore::TrajectoryPoint(120, 155.242907452877), + Boardcore::TrajectoryPoint(130, 154.149540072807), + Boardcore::TrajectoryPoint(140, 153.055673536648), + Boardcore::TrajectoryPoint(150, 151.961274537679), + Boardcore::TrajectoryPoint(160, 150.866273440767), + Boardcore::TrajectoryPoint(170, 149.770486028088), + Boardcore::TrajectoryPoint(180, 148.673866671489), + Boardcore::TrajectoryPoint(190, 147.576379051883), + Boardcore::TrajectoryPoint(200, 146.477776925786), + Boardcore::TrajectoryPoint(210, 145.378072071307), + Boardcore::TrajectoryPoint(220, 144.277208934424), + Boardcore::TrajectoryPoint(230, 143.174865357185), + Boardcore::TrajectoryPoint(240, 142.071193268059), + Boardcore::TrajectoryPoint(250, 140.965917803829), + Boardcore::TrajectoryPoint(260, 139.858981254763), + Boardcore::TrajectoryPoint(270, 138.750351757402), + Boardcore::TrajectoryPoint(280, 137.639705741156), + Boardcore::TrajectoryPoint(290, 136.527252428432), + Boardcore::TrajectoryPoint(300, 135.412440243561), + Boardcore::TrajectoryPoint(310, 134.295622180286), + Boardcore::TrajectoryPoint(320, 133.176219120437), + Boardcore::TrajectoryPoint(330, 132.054558471532), + Boardcore::TrajectoryPoint(340, 130.930035922262), + Boardcore::TrajectoryPoint(350, 129.803024125281), + Boardcore::TrajectoryPoint(360, 128.672841322107), + Boardcore::TrajectoryPoint(370, 127.539854172974), + Boardcore::TrajectoryPoint(380, 126.40354064371), + Boardcore::TrajectoryPoint(390, 125.263967099277), + Boardcore::TrajectoryPoint(400, 124.120990996584), + Boardcore::TrajectoryPoint(410, 122.974187760683), + Boardcore::TrajectoryPoint(420, 121.823782003417), + Boardcore::TrajectoryPoint(430, 120.669287582584), + Boardcore::TrajectoryPoint(440, 119.510603632944), + Boardcore::TrajectoryPoint(450, 118.347753980452), + Boardcore::TrajectoryPoint(460, 117.180280794938), + Boardcore::TrajectoryPoint(470, 116.008005268345), + Boardcore::TrajectoryPoint(480, 114.830937030228), + Boardcore::TrajectoryPoint(490, 113.648849163055), + Boardcore::TrajectoryPoint(500, 112.46116808736), + Boardcore::TrajectoryPoint(510, 111.267926654841), + Boardcore::TrajectoryPoint(520, 110.068940113543), + Boardcore::TrajectoryPoint(530, 108.863942008548), + Boardcore::TrajectoryPoint(540, 107.652654145213), + Boardcore::TrajectoryPoint(550, 106.434684140738), + Boardcore::TrajectoryPoint(560, 105.209711026935), + Boardcore::TrajectoryPoint(570, 103.977574105828), + Boardcore::TrajectoryPoint(580, 102.737947074756), + Boardcore::TrajectoryPoint(590, 101.49048918872), + Boardcore::TrajectoryPoint(600, 100.234844563544), + Boardcore::TrajectoryPoint(610, 98.9706414267826), + Boardcore::TrajectoryPoint(620, 97.6974913131941), + Boardcore::TrajectoryPoint(630, 96.4149882012485), + Boardcore::TrajectoryPoint(640, 95.1227075867228), + Boardcore::TrajectoryPoint(650, 93.8202054890391), + Boardcore::TrajectoryPoint(660, 92.5068215629322), + Boardcore::TrajectoryPoint(670, 91.1820220030042), + Boardcore::TrajectoryPoint(680, 89.845446199738), + Boardcore::TrajectoryPoint(690, 88.4965534712364), + Boardcore::TrajectoryPoint(700, 87.1347770292141), + Boardcore::TrajectoryPoint(710, 85.7587194162718), + Boardcore::TrajectoryPoint(720, 84.3684019629031), + Boardcore::TrajectoryPoint(730, 82.9630671275761), + Boardcore::TrajectoryPoint(740, 81.5410422523463), + Boardcore::TrajectoryPoint(750, 80.1025284205998), + Boardcore::TrajectoryPoint(760, 78.6453576639641), + Boardcore::TrajectoryPoint(770, 77.1696424407859), + Boardcore::TrajectoryPoint(780, 75.6729696569747), + Boardcore::TrajectoryPoint(790, 74.154948390851), + Boardcore::TrajectoryPoint(800, 72.6141236175065), + Boardcore::TrajectoryPoint(810, 71.048230249288), + Boardcore::TrajectoryPoint(820, 69.4559691422748), + Boardcore::TrajectoryPoint(830, 67.8354856219309), + Boardcore::TrajectoryPoint(840, 66.1845729615503), + Boardcore::TrajectoryPoint(850, 64.5008099767691), + Boardcore::TrajectoryPoint(860, 62.7815353973901), + Boardcore::TrajectoryPoint(870, 61.0237672707407), + Boardcore::TrajectoryPoint(880, 59.2230550349347), + Boardcore::TrajectoryPoint(890, 57.3765852255011), + Boardcore::TrajectoryPoint(900, 55.4789538743064), + Boardcore::TrajectoryPoint(910, 53.5253686767019), + Boardcore::TrajectoryPoint(920, 51.5079968711945), + Boardcore::TrajectoryPoint(930, 49.4200993719904), + Boardcore::TrajectoryPoint(940, 47.252187731736), + Boardcore::TrajectoryPoint(950, 44.9918449898108), + Boardcore::TrajectoryPoint(960, 42.6250397738168), + Boardcore::TrajectoryPoint(970, 40.132289270307), + Boardcore::TrajectoryPoint(980, 37.4890974664655), + Boardcore::TrajectoryPoint(990, 34.6601134111677), + Boardcore::TrajectoryPoint(1000, 31.5959916494853), + Boardcore::TrajectoryPoint(1010, 28.22089221044), + Boardcore::TrajectoryPoint(1020, 24.40346033828), + Boardcore::TrajectoryPoint(1030, 19.8943620344018), + Boardcore::TrajectoryPoint(1040, 14.0420207204283), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::TrajectoryPoint t10_open[] = { + Boardcore::TrajectoryPoint(0, 168.255779968402), + Boardcore::TrajectoryPoint(10, 167.162192733818), + Boardcore::TrajectoryPoint(20, 166.069701861961), + Boardcore::TrajectoryPoint(30, 164.977719060261), + Boardcore::TrajectoryPoint(40, 163.886148579548), + Boardcore::TrajectoryPoint(50, 162.794908463837), + Boardcore::TrajectoryPoint(60, 161.703922748259), + Boardcore::TrajectoryPoint(70, 160.613111695649), + Boardcore::TrajectoryPoint(80, 159.522365835857), + Boardcore::TrajectoryPoint(90, 158.431604739666), + Boardcore::TrajectoryPoint(100, 157.340740491), + Boardcore::TrajectoryPoint(110, 156.249661729934), + Boardcore::TrajectoryPoint(120, 155.158304030038), + Boardcore::TrajectoryPoint(130, 154.066587958333), + Boardcore::TrajectoryPoint(140, 152.974362569214), + Boardcore::TrajectoryPoint(150, 151.881571371771), + Boardcore::TrajectoryPoint(160, 150.788158118517), + Boardcore::TrajectoryPoint(170, 149.693955671423), + Boardcore::TrajectoryPoint(180, 148.598882389628), + Boardcore::TrajectoryPoint(190, 147.502921857494), + Boardcore::TrajectoryPoint(200, 146.405849032977), + Boardcore::TrajectoryPoint(210, 145.307631163775), + Boardcore::TrajectoryPoint(220, 144.208258302506), + Boardcore::TrajectoryPoint(230, 143.107367746732), + Boardcore::TrajectoryPoint(240, 142.005126036057), + Boardcore::TrajectoryPoint(250, 140.90129129059), + Boardcore::TrajectoryPoint(260, 139.79574756635), + Boardcore::TrajectoryPoint(270, 138.688525043137), + Boardcore::TrajectoryPoint(280, 137.579235238258), + Boardcore::TrajectoryPoint(290, 136.468125989427), + Boardcore::TrajectoryPoint(300, 135.354663520507), + Boardcore::TrajectoryPoint(310, 134.239150218386), + Boardcore::TrajectoryPoint(320, 133.121067775264), + Boardcore::TrajectoryPoint(330, 132.000677517929), + Boardcore::TrajectoryPoint(340, 130.877442588637), + Boardcore::TrajectoryPoint(350, 129.751688190099), + Boardcore::TrajectoryPoint(360, 128.622739685996), + Boardcore::TrajectoryPoint(370, 127.490992807816), + Boardcore::TrajectoryPoint(380, 126.355865448186), + Boardcore::TrajectoryPoint(390, 125.21749960496), + Boardcore::TrajectoryPoint(400, 124.075678037563), + Boardcore::TrajectoryPoint(410, 122.9300502602), + Boardcore::TrajectoryPoint(420, 121.780805116912), + Boardcore::TrajectoryPoint(430, 120.627417255802), + Boardcore::TrajectoryPoint(440, 119.469862037486), + Boardcore::TrajectoryPoint(450, 118.308126175976), + Boardcore::TrajectoryPoint(460, 117.141713077783), + Boardcore::TrajectoryPoint(470, 115.97051988696), + Boardcore::TrajectoryPoint(480, 114.794519022964), + Boardcore::TrajectoryPoint(490, 113.613483505953), + Boardcore::TrajectoryPoint(500, 112.426801510304), + Boardcore::TrajectoryPoint(510, 111.234581540997), + Boardcore::TrajectoryPoint(520, 110.036601498587), + Boardcore::TrajectoryPoint(530, 108.832594901812), + Boardcore::TrajectoryPoint(540, 107.622283539544), + Boardcore::TrajectoryPoint(550, 106.405238735423), + Boardcore::TrajectoryPoint(560, 105.181212005869), + Boardcore::TrajectoryPoint(570, 103.95000659209), + Boardcore::TrajectoryPoint(580, 102.71129619815), + Boardcore::TrajectoryPoint(590, 101.46474009246), + Boardcore::TrajectoryPoint(600, 100.209982410363), + Boardcore::TrajectoryPoint(610, 98.9466514045362), + Boardcore::TrajectoryPoint(620, 97.6743586400159), + Boardcore::TrajectoryPoint(630, 96.3926981302938), + Boardcore::TrajectoryPoint(640, 95.101245410554), + Boardcore::TrajectoryPoint(650, 93.7995565436867), + Boardcore::TrajectoryPoint(660, 92.4870025170449), + Boardcore::TrajectoryPoint(670, 91.162986029288), + Boardcore::TrajectoryPoint(680, 89.8271787204284), + Boardcore::TrajectoryPoint(690, 88.4790399695698), + Boardcore::TrajectoryPoint(700, 87.1180030520955), + Boardcore::TrajectoryPoint(710, 85.7426986653437), + Boardcore::TrajectoryPoint(720, 84.3530906971291), + Boardcore::TrajectoryPoint(730, 82.9484785503123), + Boardcore::TrajectoryPoint(740, 81.5271331670396), + Boardcore::TrajectoryPoint(750, 80.0892846495736), + Boardcore::TrajectoryPoint(760, 78.6327899535974), + Boardcore::TrajectoryPoint(770, 77.1577353059767), + Boardcore::TrajectoryPoint(780, 75.6616824139358), + Boardcore::TrajectoryPoint(790, 74.1442903676924), + Boardcore::TrajectoryPoint(800, 72.6040789823789), + Boardcore::TrajectoryPoint(810, 71.0387604059336), + Boardcore::TrajectoryPoint(820, 69.4470811544908), + Boardcore::TrajectoryPoint(830, 67.8271634606104), + Boardcore::TrajectoryPoint(840, 66.1768005225803), + Boardcore::TrajectoryPoint(850, 64.4935710993356), + Boardcore::TrajectoryPoint(860, 62.7748138812089), + Boardcore::TrajectoryPoint(870, 61.0175649356654), + Boardcore::TrajectoryPoint(880, 59.2173365099757), + Boardcore::TrajectoryPoint(890, 57.3713342102218), + Boardcore::TrajectoryPoint(900, 55.4741695662676), + Boardcore::TrajectoryPoint(910, 53.5210180197682), + Boardcore::TrajectoryPoint(920, 51.5040771676524), + Boardcore::TrajectoryPoint(930, 49.4165922241171), + Boardcore::TrajectoryPoint(940, 47.249074561539), + Boardcore::TrajectoryPoint(950, 44.9891186596966), + Boardcore::TrajectoryPoint(960, 42.6226687526033), + Boardcore::TrajectoryPoint(970, 40.1302643034058), + Boardcore::TrajectoryPoint(980, 37.4873973327142), + Boardcore::TrajectoryPoint(990, 34.658724698043), + Boardcore::TrajectoryPoint(1000, 31.5948905139691), + Boardcore::TrajectoryPoint(1010, 28.2200605996832), + Boardcore::TrajectoryPoint(1020, 24.4028712375333), + Boardcore::TrajectoryPoint(1030, 19.8939960702102), + Boardcore::TrajectoryPoint(1040, 14.0418533936787), + Boardcore::TrajectoryPoint(1050, 0), +}; +Boardcore::Trajectory t_closed[] = { + Boardcore::Trajectory{0.0, t0_closed, 106}, + Boardcore::Trajectory{0.0, t1_closed, 106}, + Boardcore::Trajectory{0.0, t2_closed, 106}, + Boardcore::Trajectory{0.0, t3_closed, 106}, + Boardcore::Trajectory{0.0, t4_closed, 106}, + Boardcore::Trajectory{0.0, t5_closed, 106}, + Boardcore::Trajectory{0.0, t6_closed, 106}, + Boardcore::Trajectory{0.0, t7_closed, 106}, + Boardcore::Trajectory{0.0, t8_closed, 106}, + Boardcore::Trajectory{0.0, t9_closed, 106}, + Boardcore::Trajectory{0.0, t10_closed, 106}, +}; +Boardcore::Trajectory t_open[] = { + Boardcore::Trajectory{0.0, t0_open, 106}, + Boardcore::Trajectory{0.0, t1_open, 106}, + Boardcore::Trajectory{0.0, t2_open, 106}, + Boardcore::Trajectory{0.0, t3_open, 106}, + Boardcore::Trajectory{0.0, t4_open, 106}, + Boardcore::Trajectory{0.0, t5_open, 106}, + Boardcore::Trajectory{0.0, t6_open, 106}, + Boardcore::Trajectory{0.0, t7_open, 106}, + Boardcore::Trajectory{0.0, t8_open, 106}, + Boardcore::Trajectory{0.0, t9_open, 106}, + Boardcore::Trajectory{0.0, t10_open, 106}, +}; +const Boardcore::TrajectorySet CLOSED_TRAJECTORY_SET(t_closed, 11); +const Boardcore::TrajectorySet OPEN_TRAJECTORY_SET(t_open, 11); + +} // namespace ABKTrajectories +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..475f8a22421812912a2c7d49bdcd1f4064ecd94c --- /dev/null +++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp @@ -0,0 +1,451 @@ +/* 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 <Main/Configs/ADAConfig.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <common/Events.h> +#include <common/ReferenceConfig.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <utils/AeroUtils/AeroUtils.h> + +#include <functional> + +using namespace Boardcore; +using namespace Common; +using namespace std; + +namespace Main +{ +ADAController::ADAController(TaskScheduler* sched) + : FSM(&ADAController::state_idle), ada(getADAKalmanConfig()), + scheduler(sched) +{ + // Subscribe the class to the topics + EventBroker::getInstance().subscribe(this, TOPIC_ADA); + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); + + // Set the default reference values + ada.setReferenceValues(ReferenceConfig::defaultReferenceValues); +} + +bool ADAController::start() +{ + // Add the task to the scheduler + size_t result = scheduler->addTask(bind(&ADAController::update, this), + ADAConfig::UPDATE_PERIOD, + TaskScheduler::Policy::RECOVER); + + return ActiveObject::start() && result != 0; +} + +void ADAController::update() +{ + ModuleManager& modules = ModuleManager::getInstance(); + PressureData barometerData = + modules.get<Sensors>()->getStaticPressure1LastSample(); + + // Get a snapshot of the situation. There is no need to synchronize because + // the getter are already thread safe with a PauseKernel + ADAControllerStatus status = getStatus(); + ADAState state = getADAState(); + + // The algorithm changes its actions depending on the FSM state + switch (status.state) + { + case ADAControllerState::ARMED: + case ADAControllerState::ACTIVE_DISARMED: + { + ada.update(barometerData.pressure); + break; + } + case ADAControllerState::SHADOW_MODE: + { + // During shadow-mode no event will be thrown + ada.update(barometerData.pressure); + + // Check for apogees + if (state.verticalSpeed < ADAConfig::APOGEE_VERTICAL_SPEED_TARGET) + { + detectedApogees++; + } + else + { + // Apogees must be consecutive in order to be valid + detectedApogees = 0; + } + + // Log the detected apogees during this phase + logStatus(status.state); + + break; + } + case ADAControllerState::ACTIVE: + { + // During shadow-mode no event will be thrown + ada.update(barometerData.pressure); + + // Check for apogees + if (state.verticalSpeed < ADAConfig::APOGEE_VERTICAL_SPEED_TARGET) + { + detectedApogees++; + } + else + { + // Apogees must be consecutive in order to be valid + detectedApogees = 0; + } + + // Check if the number of apogees has reached the limit + if (detectedApogees > ADAConfig::APOGEE_N_SAMPLES) + { + EventBroker::getInstance().post(ADA_APOGEE_DETECTED, TOPIC_ADA); + } + + // Log the detected apogees during this phase + logStatus(status.state); + + break; + } + default: + { + break; + } + } + Logger::getInstance().log(getADAState()); +} + +void ADAController::calibrate() +{ + Stats pressure; + ModuleManager& modules = ModuleManager::getInstance(); + + for (int i = 0; i < ADAConfig::CALIBRATION_SAMPLES_COUNT; i++) + { + PressureData data = + modules.get<Sensors>()->getStaticPressure1LastSample(); + pressure.add(data.pressure); + + miosix::Thread::sleep(ADAConfig::CALIBRATION_SLEEP_TIME); + } + + // Set the pressure and temperature reference + ReferenceValues reference = ada.getReferenceValues(); + reference.refPressure = pressure.getStats().mean; + + // clang-format off + reference.refAltitude = Aeroutils::relAltitude(reference.refPressure, + reference.mslPressure, + reference.mslTemperature); + // clang-format on + + // Update the algorithm reference values + { + miosix::PauseKernelLock l; + ada.setReferenceValues(reference); + ada.setKalmanConfig(getADAKalmanConfig()); + ada.update(reference.refPressure); + } + + EventBroker::getInstance().post(ADA_READY, TOPIC_ADA); +} + +void ADAController::setReferenceAltitude(float altitude) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + ReferenceValues reference = ada.getReferenceValues(); + reference.refAltitude = altitude; + ada.setReferenceValues(reference); +} + +void ADAController::setReferenceTemperature(float temperature) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + ReferenceValues reference = ada.getReferenceValues(); + + // The temperature is in degrees, converted in kelvin + reference.refTemperature = temperature + 273.15f; + ada.setReferenceValues(reference); +} + +void ADAController::setReferenceValues(const ReferenceValues reference) +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + ada.setReferenceValues(reference); +} + +ADAControllerStatus ADAController::getStatus() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return status; +} + +ADAState ADAController::getADAState() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return ada.getState(); +} + +ReferenceValues ADAController::getReferenceValues() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return ada.getReferenceValues(); +} + +void ADAController::state_idle(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(ADAControllerState::IDLE); + } + case ADA_CALIBRATE: + { + return transition(&ADAController::state_calibrating); + } + } +} + +void ADAController::state_calibrating(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(ADAControllerState::CALIBRATING); + + // Calibrate the ADA + calibrate(); + break; + } + case ADA_READY: + { + return transition(&ADAController::state_ready); + } + } +} + +void ADAController::state_ready(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(ADAControllerState::READY); + } + case ADA_CALIBRATE: + { + return transition(&ADAController::state_calibrating); + } + case ADA_FORCE_START: + { + // Skip directly to the active/shadow mode mode + return transition(&ADAController::state_shadow_mode); + } + case FLIGHT_ARMED: + { + return transition(&ADAController::state_armed); + } + } +} + +void ADAController::state_armed(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(ADAControllerState::ARMED); + } + case FLIGHT_DISARMED: + { + return transition(&ADAController::state_ready); + } + case FLIGHT_LIFTOFF: + { + return transition(&ADAController::state_shadow_mode); + } + } +} + +void ADAController::state_shadow_mode(const Event& event) +{ + static uint16_t shadowModeTimeoutEventId = 0; + + switch (event) + { + case EV_ENTRY: + { + logStatus(ADAControllerState::SHADOW_MODE); + + // Add a delayed event to exit the shadow mode + shadowModeTimeoutEventId = EventBroker::getInstance().postDelayed( + ADA_SHADOW_MODE_TIMEOUT, TOPIC_ADA, + ADAConfig::SHADOW_MODE_TIMEOUT); + break; + } + case EV_EXIT: + { + // Remove the shadow mode event. This works even though the event is + // expired (aka after shadow_mode_timeout) because the event broker + // assigns a progressive number for every delayed event. If and only + // if the number of registered delayed event is less than 2^16, then + // this technique is valid. + return EventBroker::getInstance().removeDelayed( + shadowModeTimeoutEventId); + } + case ADA_SHADOW_MODE_TIMEOUT: + { + return transition(&ADAController::state_active); + } + case ADA_FORCE_STOP: + { + return transition(&ADAController::state_ready); + } + case FLIGHT_LANDING_DETECTED: + { + return transition(&ADAController::state_end); + } + } +} + +void ADAController::state_active(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + // Zero the number of so far detected apogees. It is thread safe due + // to std::atomic variable + detectedApogees = 0; + return logStatus(ADAControllerState::ACTIVE); + } + case ADA_FORCE_STOP: + { + return transition(&ADAController::state_ready); + } + case FLIGHT_APOGEE_DETECTED: + { + return transition(&ADAController::state_active_disarmed); + } + case FLIGHT_LANDING_DETECTED: + { + return transition(&ADAController::state_end); + } + } +} + +void ADAController::state_active_disarmed(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(ADAControllerState::ACTIVE_DISARMED); + } + case ADA_FORCE_STOP: + { + return transition(&ADAController::state_ready); + } + case FLIGHT_LANDING_DETECTED: + { + return transition(&ADAController::state_end); + } + } +} + +void ADAController::state_end(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(ADAControllerState::END); + } + } +} + +void ADAController::logStatus(ADAControllerState state) +{ + { + miosix::PauseKernelLock lock; + // Update the current FSM state + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + status.detectedApogees = detectedApogees; + } + + // Log the status + Logger::getInstance().log(status); +} + +ADA::KalmanFilter::KalmanConfig ADAController::getADAKalmanConfig() +{ + ADA::KalmanFilter::MatrixNN F_INIT; + ADA::KalmanFilter::MatrixPN H_INIT; + ADA::KalmanFilter::MatrixNN P_INIT; + ADA::KalmanFilter::MatrixNN Q_INIT; + ADA::KalmanFilter::MatrixPP R_INIT; + ADA::KalmanFilter::MatrixNM G_INIT; + + // clang-format off + F_INIT = ADA::KalmanFilter::MatrixNN({ + {1.0, ADAConfig::SAMPLING_PERIOD, 0.5f * ADAConfig::SAMPLING_PERIOD * ADAConfig::SAMPLING_PERIOD}, + {0.0, 1.0, ADAConfig::SAMPLING_PERIOD}, + {0.0, 0.0, 1.0}}); + // clang-format on + + H_INIT = {1.0, 0.0, 0.0}; + + P_INIT = ADA::KalmanFilter::MatrixNN( + {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}); + + Q_INIT << ADA::KalmanFilter::MatrixNN( + {{30.0, 0.0, 0.0}, {0.0, 10.0, 0.0}, {0.0, 0.0, 2.5f}}); + + R_INIT[0] = 4000.0f; + + G_INIT = ADA::KalmanFilter::MatrixNM::Zero(); + + return {F_INIT, + H_INIT, + Q_INIT, + R_INIT, + P_INIT, + G_INIT, + ADA::KalmanFilter::CVectorN(ada.getReferenceValues().refPressure, 0, + 0)}; +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h new file mode 100644 index 0000000000000000000000000000000000000000..66474c5c953e4a8856a87a21e195410868e9f9d6 --- /dev/null +++ b/src/boards/Main/StateMachines/ADAController/ADAController.h @@ -0,0 +1,97 @@ +/* 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 <Main/StateMachines/ADAController/ADAControllerData.h> +#include <algorithms/ADA/ADA.h> +#include <diagnostic/PrintLogger.h> +#include <events/FSM.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class ADAController : public Boardcore::Module, + public Boardcore::FSM<ADAController> +{ +public: + ADAController(Boardcore::TaskScheduler* sched); + + /** + * @brief Starts the FSM thread and adds an update function into the + * scheduler + */ + bool start() override; + + /** + * @brief Update function called periodically by the scheduler. It checks + * the current FSM state and checks for apogees. + */ + void update(); + + void calibrate(); + + // ADA setters + void setReferenceAltitude(float altitude); + void setReferenceTemperature(float temperature); + void setReferenceValues(const Boardcore::ReferenceValues reference); + + // ADA getters + ADAControllerStatus getStatus(); + Boardcore::ADAState getADAState(); + Boardcore::ReferenceValues getReferenceValues(); + + // FSM states + void state_idle(const Boardcore::Event& event); + void state_calibrating(const Boardcore::Event& event); + void state_ready(const Boardcore::Event& event); + void state_armed(const Boardcore::Event& event); + void state_shadow_mode(const Boardcore::Event& event); + void state_active(const Boardcore::Event& event); + void state_active_disarmed(const Boardcore::Event& event); + void state_end(const Boardcore::Event& event); + +private: + /** + * @brief Logs the ADA status updating the FSM state + * @param state The current FSM state + */ + void logStatus(ADAControllerState state); + + // Returns the ADA kalman matrices that configure the filter + Boardcore::ADA::KalmanFilter::KalmanConfig getADAKalmanConfig(); + + // Controller state machine status + ADAControllerStatus status; + Boardcore::ADA ada; + + // Scheduler to be used for update function + Boardcore::TaskScheduler* scheduler = nullptr; + + // Thread safe counter for apogees + std::atomic<uint16_t> detectedApogees{0}; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ADA"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/ADAController/ADAControllerData.h b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h new file mode 100644 index 0000000000000000000000000000000000000000..8b19c94467ea8d0436b6441ee064ccc33361578a --- /dev/null +++ b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: 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. + */ + +#pragma once + +#include <stdint.h> + +#include <iostream> +#include <string> + +namespace Main +{ + +enum class ADAControllerState : uint8_t +{ + UNINIT = 0, + IDLE, + CALIBRATING, + READY, + ARMED, + SHADOW_MODE, + ACTIVE, + ACTIVE_DISARMED, + END +}; + +struct ADAControllerStatus +{ + uint64_t timestamp = 0; + uint16_t detectedApogees = 0; + ADAControllerState state = ADAControllerState::UNINIT; + + static std::string header() { return "timestamp,detectedApogees,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)detectedApogees << "," << (int)state + << "\n"; + } +}; + +} // namespace Main diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.cpp b/src/boards/Main/StateMachines/Deployment/Deployment.cpp new file mode 100644 index 0000000000000000000000000000000000000000..235a1819dcf46c8e0e40f27c2a8edaecd87a4d0b --- /dev/null +++ b/src/boards/Main/StateMachines/Deployment/Deployment.cpp @@ -0,0 +1,108 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/Configs/DeploymentConfig.h> +#include <Main/StateMachines/Deployment/Deployment.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> + +using namespace Boardcore; +using namespace Common; + +namespace Main +{ +Deployment::Deployment() : FSM(&Deployment::state_idle) +{ + EventBroker::getInstance().subscribe(this, TOPIC_DPL); + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); +} + +DeploymentStatus Deployment::getStatus() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return status; +} + +void Deployment::state_idle(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + modules.get<Actuators>()->cutterOff(); + return logStatus(DeploymentState::IDLE); + } + case FLIGHT_DPL_ALT_DETECTED: + { + return transition(&Deployment::state_cutting); + } + } +} + +void Deployment::state_cutting(const Event& event) +{ + static uint16_t ncCuttingTimeoutEventId = 0; + + ModuleManager& modules = ModuleManager::getInstance(); + + switch (event) + { + case EV_ENTRY: + { + logStatus(DeploymentState::CUTTING); + modules.get<Actuators>()->cutterOn(); + + ncCuttingTimeoutEventId = EventBroker::getInstance().postDelayed( + DPL_CUT_TIMEOUT, TOPIC_DPL, DeploymentConfig::CUT_DURATION); + + break; + } + case EV_EXIT: + { + return EventBroker::getInstance().removeDelayed( + ncCuttingTimeoutEventId); + } + case DPL_CUT_TIMEOUT: + { + return transition(&Deployment::state_idle); + } + } +} + +void Deployment::logStatus(DeploymentState state) +{ + { + miosix::PauseKernelLock lock; + // Update the current FSM state + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + } + + // Log the status + Logger::getInstance().log(status); +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.h b/src/boards/Main/StateMachines/Deployment/Deployment.h new file mode 100644 index 0000000000000000000000000000000000000000..b928681173a6b8ebf9dd94d1c128b200457a8c3c --- /dev/null +++ b/src/boards/Main/StateMachines/Deployment/Deployment.h @@ -0,0 +1,56 @@ +/* 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 <Main/StateMachines/Deployment/DeploymentData.h> +#include <diagnostic/PrintLogger.h> +#include <events/FSM.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class Deployment : public Boardcore::Module, public Boardcore::FSM<Deployment> +{ +public: + Deployment(); + + DeploymentStatus getStatus(); + + // FSM states + void state_idle(const Boardcore::Event& event); + void state_cutting(const Boardcore::Event& event); + +private: + /** + * @brief Logs the Deployment status updating the FSM state + * @param state The current FSM state + */ + void logStatus(DeploymentState state); + + // State machine status + DeploymentStatus status; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Deployment"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/Deployment/DeploymentData.h b/src/boards/Main/StateMachines/Deployment/DeploymentData.h new file mode 100644 index 0000000000000000000000000000000000000000..edf440bdff0855a47f4c7690bf774d8de0f7c282 --- /dev/null +++ b/src/boards/Main/StateMachines/Deployment/DeploymentData.h @@ -0,0 +1,51 @@ +/* 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 Main +{ +enum class DeploymentState : uint8_t +{ + UNINIT = 0, + IDLE, + CUTTING, +}; + +struct DeploymentStatus +{ + uint64_t timestamp = 0; + DeploymentState state = DeploymentState::UNINIT; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp new file mode 100644 index 0000000000000000000000000000000000000000..52cbd188dcb1ae2e773d67c93d28f1ab47ead009 --- /dev/null +++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -0,0 +1,742 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Angelo Prete, 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 <Main/Actuators/Actuators.h> +#include <Main/CanHandler/CanHandler.h> +#include <Main/Configs/FlightModeManagerConfig.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> + +#include "FlightModeManagerData.h" + +using namespace Boardcore; +using namespace Common; +using namespace miosix; +using namespace Main::FMMConfig; + +namespace Main +{ +FlightModeManager::FlightModeManager() + : HSM(&FlightModeManager::state_on_ground) +{ + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); + EventBroker::getInstance().subscribe(this, TOPIC_FMM); + EventBroker::getInstance().subscribe(this, TOPIC_TMTC); + EventBroker::getInstance().subscribe(this, TOPIC_MOTOR); + EventBroker::getInstance().subscribe(this, TOPIC_CAN); + EventBroker::getInstance().subscribe(this, TOPIC_NAS); + EventBroker::getInstance().subscribe(this, TOPIC_ADA); + EventBroker::getInstance().subscribe(this, TOPIC_ALT); + EventBroker::getInstance().subscribe(this, TOPIC_MEA); +} + +FlightModeManagerStatus FlightModeManager::getStatus() +{ + PauseKernelLock lock; + return status; +} + +State FlightModeManager::state_on_ground(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::ON_GROUND); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + case EV_INIT: + { + return transition(&FlightModeManager::state_init); + } + case TMTC_RESET_BOARD: + { + reboot(); + return HANDLED; + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_init(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::INIT); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_on_ground); + } + case EV_INIT: + { + return HANDLED; + } + case FMM_INIT_ERROR: + { + return transition(&FlightModeManager::state_init_error); + } + case FMM_INIT_OK: + { + return transition(&FlightModeManager::state_init_done); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_init_error(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::INIT_ERROR); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_on_ground); + } + case EV_INIT: + { + return HANDLED; + } + case CAN_FORCE_INIT: + case TMTC_FORCE_INIT: + { + if (event != CAN_FORCE_INIT) + { + modules.get<CanHandler>()->sendEvent( + CanConfig::EventId::FORCE_INIT); + } + return transition(&FlightModeManager::state_init_done); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_init_done(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::INIT_DONE); + EventBroker::getInstance().post(FMM_CALIBRATE, TOPIC_FMM); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_on_ground); + } + case EV_INIT: + { + return HANDLED; + } + case FMM_CALIBRATE: + { + return transition(&FlightModeManager::state_calibrate_sensors); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_calibrate_sensors(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::CALIBRATE_SENSORS); + + ModuleManager::getInstance().get<Sensors>()->calibrate(); + EventBroker::getInstance().post(FMM_SENSORS_CAL_DONE, TOPIC_FMM); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_on_ground); + } + case EV_INIT: + { + return HANDLED; + } + case FMM_SENSORS_CAL_DONE: + { + return transition(&FlightModeManager::state_calibrate_algorithms); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_calibrate_algorithms(const Event& event) +{ + static bool nasReady = false; + static bool adaReady = false; + + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::CALIBRATE_ALGORITHMS); + + // Reset the readiness variables (not doing this could lead to + // mis-calibration in future attempts) + nasReady = false; + adaReady = false; + + // Post the calibration events + EventBroker::getInstance().post(NAS_CALIBRATE, TOPIC_NAS); + EventBroker::getInstance().post(ADA_CALIBRATE, TOPIC_ADA); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_on_ground); + } + case EV_INIT: + { + return HANDLED; + } + case NAS_READY: + { + nasReady = true; + + if (adaReady) + { + return transition(&FlightModeManager::state_disarmed); + } + else + { + return HANDLED; + } + } + case ADA_READY: + { + adaReady = true; + + if (nasReady) + { + return transition(&FlightModeManager::state_disarmed); + } + else + { + return HANDLED; + } + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_disarmed(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::DISARMED); + + // Stop eventual logging + Logger::getInstance().stop(); + modules.get<Actuators>()->camOff(); + modules.get<Actuators>()->setBuzzerOff(); + EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_on_ground); + } + case EV_INIT: + { + return HANDLED; + } + case CAN_CALIBRATE: + case TMTC_CALIBRATE: + { + if (event != CAN_CALIBRATE) + { + modules.get<CanHandler>()->sendEvent( + CanConfig::EventId::CALIBRATE); + } + return transition(&FlightModeManager::state_calibrate_sensors); + } + case CAN_ENTER_TEST_MODE: + case TMTC_ENTER_TEST_MODE: + { + if (event != CAN_ENTER_TEST_MODE) + { + modules.get<CanHandler>()->sendEvent( + CanConfig::EventId::ENTER_TEST_MODE); + } + return transition(&FlightModeManager::state_test_mode); + } + case CAN_ARM: + case TMTC_ARM: + { + if (event != CAN_ARM) + { + modules.get<CanHandler>()->sendEvent(CanConfig::EventId::ARM); + } + return transition(&FlightModeManager::state_armed); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_test_mode(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::TEST_MODE); + + Logger::getInstance().start(); + EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS); + + return HANDLED; + } + case EV_EXIT: + { + Logger::getInstance().stop(); + EventBroker::getInstance().post(NAS_FORCE_STOP, TOPIC_NAS); + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_on_ground); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_START_RECORDING: + { + modules.get<Actuators>()->camOn(); + return HANDLED; + } + case TMTC_STOP_RECORDING: + { + modules.get<Actuators>()->camOff(); + return HANDLED; + } + case CAN_EXIT_TEST_MODE: + case TMTC_EXIT_TEST_MODE: + { + if (event != CAN_EXIT_TEST_MODE) + { + modules.get<CanHandler>()->sendEvent( + CanConfig::EventId::EXIT_TEST_MODE); + } + return transition(&FlightModeManager::state_disarmed); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_armed(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::ARMED); + + Logger::getInstance().start(); + modules.get<Actuators>()->camOn(); + modules.get<Actuators>()->setBuzzerArm(); + EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + case EV_INIT: + { + return HANDLED; + } + case CAN_DISARM: + case TMTC_DISARM: + { + if (event != CAN_DISARM) + { + modules.get<CanHandler>()->sendEvent( + CanConfig::EventId::DISARM); + } + return transition(&FlightModeManager::state_disarmed); + } + case FLIGHT_LAUNCH_PIN_DETACHED: + case TMTC_FORCE_LAUNCH: + { + modules.get<CanHandler>()->sendEvent(CanConfig::EventId::LIFTOFF); + return transition(&FlightModeManager::state_flying); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_flying(const Event& event) +{ + static uint16_t missionTimeoutEventId = 0; + + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::FLYING); + + EventBroker::getInstance().post(FLIGHT_LIFTOFF, TOPIC_FLIGHT); + missionTimeoutEventId = EventBroker::getInstance().postDelayed( + FLIGHT_MISSION_TIMEOUT, TOPIC_FLIGHT, MISSION_TIMEOUT); + + return HANDLED; + } + case EV_EXIT: + { + EventBroker::getInstance().removeDelayed(missionTimeoutEventId); + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + case EV_INIT: + { + return transition(&FlightModeManager::state_powered_ascent); + } + case FLIGHT_MISSION_TIMEOUT: + { + EventBroker::getInstance().post(FLIGHT_LANDING_TIMEOUT, + TOPIC_FLIGHT); + return transition(&FlightModeManager::state_landed); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_powered_ascent(const Event& event) +{ + static uint16_t engineShutdownTimeoutEventId = 0; + ModuleManager& modules = ModuleManager::getInstance(); + + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::POWERED_ASCENT); + + // After a maximum time, the motor is considered shut down + engineShutdownTimeoutEventId = + EventBroker::getInstance().postDelayed( + MOTOR_CLOSE_FEED_VALVE, TOPIC_FMM, ENGINE_SHUTDOWN_TIMEOUT); + + return HANDLED; + } + case EV_EXIT: + { + // Shutdown via can + modules.get<CanHandler>()->sendCanCommand(ServosList::MAIN_VALVE, 0, + ENGINE_SHUTDOWN_TIMEOUT); + + EventBroker::getInstance().removeDelayed( + engineShutdownTimeoutEventId); + + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_flying); + } + case EV_INIT: + { + return HANDLED; + } + case MEA_SHUTDOWN_DETECTED: + case MOTOR_CLOSE_FEED_VALVE: + { + return transition(&FlightModeManager::state_unpowered_ascent); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_unpowered_ascent(const Event& event) +{ + static uint16_t apogeeTimeoutEventId = 0; + ModuleManager& modules = ModuleManager::getInstance(); + + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::UNPOWERED_ASCENT); + EventBroker::getInstance().post(FLIGHT_MOTOR_SHUTDOWN, + TOPIC_FLIGHT); + apogeeTimeoutEventId = EventBroker::getInstance().postDelayed( + TMTC_FORCE_APOGEE, TOPIC_TMTC, APOGEE_EVENT_TIMEOUT); + + return HANDLED; + } + case EV_EXIT: + { + modules.get<Actuators>()->setServoPosition( + ServosList::EXPULSION_SERVO, 1); + + EventBroker::getInstance().removeDelayed(apogeeTimeoutEventId); + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_flying); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_FORCE_APOGEE: + case ADA_APOGEE_DETECTED: + { + modules.get<CanHandler>()->sendEvent( + CanConfig::EventId::APOGEE_DETECTED); + return transition(&FlightModeManager::state_drogue_descent); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_drogue_descent(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::DROGUE_DESCENT); + EventBroker::getInstance().post(FLIGHT_APOGEE_DETECTED, + TOPIC_FLIGHT); + EventBroker::getInstance().post(FLIGHT_DROGUE_DESCENT, + TOPIC_FLIGHT); + + // Open the venting valve + modules.get<CanHandler>()->sendCanCommand(ServosList::VENTING_VALVE, + 1, MISSION_TIMEOUT); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_flying); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_FORCE_DEPLOYMENT: + case ALTITUDE_TRIGGER_ALTITUDE_REACHED: + { + return transition(&FlightModeManager::state_terminal_descent); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_terminal_descent(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::TERMINAL_DESCENT); + EventBroker::getInstance().post(FLIGHT_DPL_ALT_DETECTED, + TOPIC_FLIGHT); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_flying); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_FORCE_LANDING: + { + return transition(&FlightModeManager::state_landed); + } + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::state_landed(const Event& event) +{ + ModuleManager& modules = ModuleManager::getInstance(); + switch (event) + { + case EV_ENTRY: + { + logStatus(FlightModeManagerState::LANDED); + modules.get<Actuators>()->setBuzzerLand(); + EventBroker::getInstance().post(FLIGHT_LANDING_DETECTED, + TOPIC_FLIGHT); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_RESET_BOARD: + { + reboot(); + return HANDLED; + } + default: + { + return UNHANDLED; + } + } +} + +void FlightModeManager::logStatus(FlightModeManagerState state) +{ + { + PauseKernelLock lock; + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + } + + Logger::getInstance().log(status); +} + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h new file mode 100644 index 0000000000000000000000000000000000000000..9ab34b0bed7ad9a55c81315a5e460e0d44e03cd5 --- /dev/null +++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h @@ -0,0 +1,101 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Angelo Prete, 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 <events/EventBroker.h> +#include <events/HSM.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +#include "FlightModeManagerData.h" + +namespace Main +{ + +class FlightModeManager : public Boardcore::Module, + public Boardcore::HSM<FlightModeManager> +{ +public: + FlightModeManager(); + + FlightModeManagerStatus getStatus(); + + // Super state for when the rocket is on ground + Boardcore::State state_on_ground(const Boardcore::Event& event); + + // Initialization state + Boardcore::State state_init(const Boardcore::Event& event); + + // State in which the init has failed + Boardcore::State state_init_error(const Boardcore::Event& event); + + // State in which the init is done and a calibration event is thrown + Boardcore::State state_init_done(const Boardcore::Event& event); + + // Calibration of all the sensors (offsets) + Boardcore::State state_calibrate_sensors(const Boardcore::Event& event); + + // Calibration of all the algorithms (triad for NAS etc..) + Boardcore::State state_calibrate_algorithms(const Boardcore::Event& event); + + // State in which the electronics is ready to be armed + Boardcore::State state_disarmed(const Boardcore::Event& event); + + // State in which some actions (like main deployment, expulsion event etc..) + // are accepted + Boardcore::State state_test_mode(const Boardcore::Event& event); + + // State in which the algorithms start to run (NAS) and the electronics is + // ready to fly + Boardcore::State state_armed(const Boardcore::Event& event); + + // Super state in which the liftoff has been detected + Boardcore::State state_flying(const Boardcore::Event& event); + + // State in which the the rocket is ascending with the motor on. The + // automatic shutdown algorithm runs during this phase + Boardcore::State state_powered_ascent(const Boardcore::Event& event); + + // State in which the motor has been shut down. The airbrakes algorithm is + // running during this phase + Boardcore::State state_unpowered_ascent(const Boardcore::Event& event); + + // State in which the apogee has been detected (triggered by the ADA system) + Boardcore::State state_drogue_descent(const Boardcore::Event& event); + + // State in which the parachute has been opened by an altitude trigger + Boardcore::State state_terminal_descent(const Boardcore::Event& event); + + // State in which n minutes of time have passed after the liftoff event + Boardcore::State state_landed(const Boardcore::Event& event); + +private: + void logStatus(FlightModeManagerState state); + + FlightModeManagerStatus status; + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("FlightModeManager"); +}; + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h new file mode 100644 index 0000000000000000000000000000000000000000..c9ede12b227b1fd0e443f4882e20005213748b26 --- /dev/null +++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h @@ -0,0 +1,67 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Angelo Prete, 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 Main +{ + +enum class FlightModeManagerState : uint8_t +{ + ON_GROUND = 0, + INIT, + INIT_ERROR, + INIT_DONE, + CALIBRATE_SENSORS, + CALIBRATE_ALGORITHMS, + DISARMED, + TEST_MODE, + ARMED, + IGNITION, + FLYING, + POWERED_ASCENT, + UNPOWERED_ASCENT, + DROGUE_DESCENT, + TERMINAL_DESCENT, + LANDED, + INVALID, +}; + +struct FlightModeManagerStatus +{ + uint64_t timestamp = 0; + FlightModeManagerState state = FlightModeManagerState::INVALID; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.cpp b/src/boards/Main/StateMachines/MEAController/MEAController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a59e2de05b50fb66a1dfd0fab26a42d12c485acd --- /dev/null +++ b/src/boards/Main/StateMachines/MEAController/MEAController.cpp @@ -0,0 +1,403 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/Configs/MEAConfig.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/MEAController/MEAController.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> + +#include <functional> + +using namespace Boardcore; +using namespace Common; +using namespace std; + +namespace Main +{ +MEAController::MEAController(TaskScheduler* sched) + : FSM(&MEAController::state_idle), mea(getMEAKalmanConfig()), + scheduler(sched) +{ + // Subscribe the class of the topics + EventBroker::getInstance().subscribe(this, TOPIC_MEA); + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); +} + +bool MEAController::start() +{ + size_t result = scheduler->addTask(bind(&MEAController::update, this), + MEAConfig::UPDATE_PERIOD, + TaskScheduler::Policy::RECOVER); + return ActiveObject::start() && result != 0; +} + +void MEAController::update() +{ + ModuleManager& modules = ModuleManager::getInstance(); + PressureData ccPressure = modules.get<Sensors>()->getCCPressureLastSample(); + + // No need for pause kernel due to its presence inside the getter + MEAControllerStatus status = getStatus(); + NASState nasState = modules.get<NASController>()->getNasState(); + + // Get mach number and estimated CD + float mach = computeMach(nasState); + float CD = computeCD(mach); + float rho = computeRho(nasState); + + // Get MAIN valve state from CAN bus + float valvePosition = + modules.get<Actuators>()->getServoPosition(ServosList::MAIN_VALVE) > 0.3 + ? 1 + : 0; + + // If the state is active (or armed) and the pressure is greater than the + // threshold update the kalman + switch (status.state) + { + case MEAControllerState::ARMED: + { + if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD && + ccPressure.pressureTimestamp > lastUpdateTimestamp) + { + mea.update(valvePosition, ccPressure.pressure); + lastUpdateTimestamp = TimestampTimer::getTimestamp(); + } + break; + } + case MEAControllerState::SHADOW_MODE: + { + if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD && + ccPressure.pressureTimestamp > lastUpdateTimestamp) + { + mea.update(valvePosition, ccPressure.pressure); + lastUpdateTimestamp = TimestampTimer::getTimestamp(); + } + + // Compute the estimated altitude + estimatedAltitude = + computeAltitude(nasState, mea.getState().x2, CD, 0.15f, rho); + estimatedMass = mea.getState().x2; + + if (estimatedAltitude >= MEAConfig::SHUTDOWN_THRESHOLD_ALTITUDE) + { + detectedShutdowns++; + } + else + { + // Shutdowns must be consecutive in order to be valid + detectedShutdowns = 0; + } + + logStatus(status.state); + break; + } + case MEAControllerState::ACTIVE: + { + if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD && + ccPressure.pressureTimestamp > lastUpdateTimestamp) + { + mea.update(valvePosition, ccPressure.pressure); + lastUpdateTimestamp = TimestampTimer::getTimestamp(); + } + + // Compute the estimated altitude + estimatedAltitude = + computeAltitude(nasState, mea.getState().x2, CD, 0.15f, rho); + estimatedMass = mea.getState().x2; + + if (estimatedAltitude >= MEAConfig::SHUTDOWN_THRESHOLD_ALTITUDE) + { + detectedShutdowns++; + } + else + { + // Shutdowns must be consecutive in order to be valid + detectedShutdowns = 0; + } + + if (detectedShutdowns > MEAConfig::SHUTDOWN_N_SAMPLES) + { + EventBroker::getInstance().post(MEA_SHUTDOWN_DETECTED, + TOPIC_MEA); + } + + logStatus(status.state); + break; + } + case MEAControllerState::ACTIVE_DISARMED: + { + if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD && + ccPressure.pressureTimestamp > lastUpdateTimestamp) + { + mea.update(valvePosition, ccPressure.pressure); + lastUpdateTimestamp = TimestampTimer::getTimestamp(); + } + + estimatedMass = mea.getState().x2; + + logStatus(status.state); + break; + } + default: + { + break; + } + } + + Logger::getInstance().log(getMEAState()); +} + +MEAControllerStatus MEAController::getStatus() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return status; +} + +MEAState MEAController::getMEAState() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return mea.getState(); +} + +void MEAController::state_idle(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(MEAControllerState::IDLE); + + return transition(&MEAController::state_ready); + } + } +} + +void MEAController::state_ready(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(MEAControllerState::READY); + } + case FLIGHT_ARMED: + { + return transition(&MEAController::state_armed); + } + } +} + +void MEAController::state_armed(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(MEAControllerState::ARMED); + } + case FLIGHT_DISARMED: + { + return transition(&MEAController::state_ready); + } + case FLIGHT_LIFTOFF: + { + return transition(&MEAController::state_shadow_mode); + } + } +} + +void MEAController::state_shadow_mode(const Event& event) +{ + static uint16_t shadowModeTimeoutEventId = 0; + + switch (event) + { + case EV_ENTRY: + { + logStatus(MEAControllerState::SHADOW_MODE); + + // Add a delayed event to exit the shadow mode + shadowModeTimeoutEventId = EventBroker::getInstance().postDelayed( + MOTOR_SHADOW_MODE_TIMEOUT, TOPIC_MEA, + MEAConfig::SHADOW_MODE_TIMEOUT); + break; + } + case EV_EXIT: + { + // Remove the shadow mode event. This works even though the event is + // expired (aka after shadow_mode_timeout) because the event broker + // assigns a progressive number for every delayed event. If and only + // if the number of registered delayed event is less than 2^16, then + // this technique is valid. + return EventBroker::getInstance().removeDelayed( + shadowModeTimeoutEventId); + } + case MOTOR_SHADOW_MODE_TIMEOUT: + { + return transition(&MEAController::state_active); + } + case FLIGHT_LANDING_DETECTED: + { + return transition(&MEAController::state_end); + } + } +} + +void MEAController::state_active(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + // Zero the number of so far detected shutdowns. It is thread safe + // due to std::atomic variable + detectedShutdowns = 0; + return logStatus(MEAControllerState::ACTIVE); + } + case FLIGHT_MOTOR_SHUTDOWN: + { + return transition(&MEAController::state_active_disarmed); + } + case FLIGHT_APOGEE_DETECTED: + case FLIGHT_LANDING_DETECTED: + { + return transition(&MEAController::state_end); + } + } +} + +void MEAController::state_active_disarmed(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(MEAControllerState::ACTIVE_DISARMED); + } + case FLIGHT_APOGEE_DETECTED: + case FLIGHT_LANDING_DETECTED: + { + return transition(&MEAController::state_end); + } + } +} + +void MEAController::state_end(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(MEAControllerState::END); + } + } +} + +void MEAController::logStatus(MEAControllerState state) +{ + { + miosix::PauseKernelLock lock; + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + status.detectedShutdowns = detectedShutdowns; + status.estimatedApogee = estimatedAltitude; + status.estimatedMass = estimatedMass; + } + + Logger::getInstance().log(status); +} + +float MEAController::computeMach(NASState state) +{ + float T = Constants::MSL_TEMPERATURE + Constants::a * (-state.d); + float c = sqrt(1.4f * Constants::R * T); + return -state.vd / c; +} + +float MEAController::computeCD(float mach) +{ + return MEAConfig::n000 + MEAConfig::n100 * mach + + MEAConfig::n200 * powf(mach, 2) + MEAConfig::n300 * powf(mach, 3) + + MEAConfig::n400 * powf(mach, 4) + MEAConfig::n500 * powf(mach, 5) + + MEAConfig::n600 * powf(mach, 6); +} + +float MEAController::computeAltitude(NASState state, float mass, float CD, + float D, float rho) +{ + // Compute rocket surface + float S = Constants::PI * (D / 2) * (D / 2); + + return -state.d + + 1 / (2 * (0.5 * rho * CD * S / mass)) * + log1p(((-state.vd) * (-state.vd) * (0.5 * rho * CD * S) / mass) / + Constants::g); +} + +float MEAController::computeRho(NASState state) +{ + return Constants::RHO_0 * + exp(state.d / 11000.f); // 11000 is the troposphere height +} + +MEA::KalmanFilter::KalmanConfig MEAController::getMEAKalmanConfig() +{ + MEA::KalmanFilter::MatrixNN F_INIT; + MEA::KalmanFilter::MatrixPN H_INIT; + MEA::KalmanFilter::MatrixNN P_INIT; + MEA::KalmanFilter::MatrixNN Q_INIT; + MEA::KalmanFilter::MatrixPP R_INIT; + MEA::KalmanFilter::MatrixNM G_INIT; + + // clang-format off + F_INIT = MEA::KalmanFilter::MatrixNN({ + {1.435871191228868, -0.469001276508780, 0.f}, + {1.f, 0.f, 0.f}, + {-0.002045309260755, 0.001867496708935, 1.f}}); + + H_INIT = {1.780138883879285,-1.625379384370081,0.f}; + + P_INIT = MEA::KalmanFilter::MatrixNN::Zero(); + Q_INIT = MEAConfig::MODEL_NOISE_VARIANCE * MEA::KalmanFilter::CVectorN({1, 1, 1}).asDiagonal(); + R_INIT[0] = MEAConfig::SENSOR_NOISE_VARIANCE; + G_INIT = MEA::KalmanFilter::MatrixNM{{4}, {0}, {0}}; + // clang-format on + + return {F_INIT, + H_INIT, + Q_INIT, + R_INIT, + P_INIT, + G_INIT, + MEA::KalmanFilter::CVectorN{ + 0, 0, MEAConfig::DEFAULT_INITIAL_ROCKET_MASS}}; +} + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.h b/src/boards/Main/StateMachines/MEAController/MEAController.h new file mode 100644 index 0000000000000000000000000000000000000000..c1e1ae6310efa3b3c1824713ab862fb2cc01fa8d --- /dev/null +++ b/src/boards/Main/StateMachines/MEAController/MEAController.h @@ -0,0 +1,121 @@ +/* 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 <Main/StateMachines/MEAController/MEAControllerData.h> +#include <algorithms/MEA/MEA.h> +#include <algorithms/NAS/NASState.h> +#include <diagnostic/PrintLogger.h> +#include <events/FSM.h> +#include <scheduler/TaskScheduler.h> +#include <utils/Constants.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class MEAController : public Boardcore::Module, + public Boardcore::FSM<MEAController> +{ +public: + MEAController(Boardcore::TaskScheduler* sched); + + /** + * @brief Starts the FSM thread and adds an update function into the + * scheduler. + */ + bool start() override; + + /** + * @brief Update function called periodically by the scheduler. It checks + * the current FSM state and checks for engine shutdown. + */ + void update(); + + // MEA getters + MEAControllerStatus getStatus(); + Boardcore::MEAState getMEAState(); + + // 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_shadow_mode(const Boardcore::Event& event); + void state_active(const Boardcore::Event& event); + void state_active_disarmed(const Boardcore::Event& event); + void state_end(const Boardcore::Event& event); + +private: + /** + * @brief Logs the MEA status updating the FSM state + * @param state The current FSM state + */ + void logStatus(MEAControllerState state); + + /** + * @brief Computes the mach speed using the NASState + */ + float computeMach(Boardcore::NASState state); + + /** + * @brief Computes the rocket CD based on current speed and the coefficients + */ + float computeCD(float mach); + + /** + * @brief Estimates the altitude based on the estimated mass and current CD + * + * @param state NASState for estimated velocity and height + * @param mass Estimated mass + * @param CD estimated CD from velocity and height + * @param D diameter of the rocket + * @param rho air density at altitude h + */ + float computeAltitude(Boardcore::NASState state, float mass, float CD, + float D, float rho); + + /** + * @brief Computes the RHO air density for standard isa air model + */ + float computeRho(Boardcore::NASState state); + + // Returns the MEA kalman matrices to configure the filter + Boardcore::MEA::KalmanFilter::KalmanConfig getMEAKalmanConfig(); + + // Controller state machine status + MEAControllerStatus status; + Boardcore::MEA mea; + + // Scheduler to be used for update function + Boardcore::TaskScheduler* scheduler = nullptr; + + // Thread safe counter for engine shutdown + std::atomic<uint16_t> detectedShutdowns{0}; + std::atomic<float> estimatedAltitude{0}; + std::atomic<float> estimatedMass{0}; + + // Last update timestamp of mea + uint64_t lastUpdateTimestamp = 0; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("MEA"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/MEAController/MEAControllerData.h b/src/boards/Main/StateMachines/MEAController/MEAControllerData.h new file mode 100644 index 0000000000000000000000000000000000000000..a531d8d99295f7b99567331ccb86dfb6d76ae6fc --- /dev/null +++ b/src/boards/Main/StateMachines/MEAController/MEAControllerData.h @@ -0,0 +1,63 @@ +/* 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 Main +{ +enum class MEAControllerState : uint8_t +{ + UNINIT = 0, + IDLE, + READY, + ARMED, + SHADOW_MODE, + ACTIVE, + ACTIVE_DISARMED, + END +}; + +struct MEAControllerStatus +{ + uint64_t timestamp = 0; + float estimatedMass = 0; + float estimatedApogee = 0; + uint16_t detectedShutdowns = 0; + MEAControllerState state = MEAControllerState::UNINIT; + + static std::string header() + { + return "timestamp,estimatedMass,estimatedApogee,detectedShutdowns," + "state\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << estimatedMass << "," << estimatedApogee << "," + << (int)detectedShutdowns << "," << (int)state << "\n"; + } +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1e63149930c70f42e66191bc16de4bb63ff1c8b5 --- /dev/null +++ b/src/boards/Main/StateMachines/NASController/NASController.cpp @@ -0,0 +1,384 @@ +/* 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 <Main/Configs/NASConfig.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <algorithms/NAS/StateInitializer.h> +#include <common/Events.h> +#include <common/ReferenceConfig.h> +#include <events/EventBroker.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +using namespace Boardcore; +using namespace Eigen; +using namespace std; +using namespace Common; +namespace Main +{ +NASController::NASController(TaskScheduler* sched) + : FSM(&NASController::state_idle), nas(NASConfig::config), scheduler(sched) +{ + // Subscribe the class to the topics + EventBroker::getInstance().subscribe(this, TOPIC_NAS); + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); + + // Setup the NAS + Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero(); + + // Create the initial quaternion + Vector4f q = SkyQuaternion::eul2quat({0, 0, 0}); + + // Set the initial quaternion inside the matrix + x(6) = q(0); + x(7) = q(1); + x(8) = q(2); + x(9) = q(3); + + // Set the NAS x matrix + nas.setX(x); + + // Set the referenced values for the correct place on earth + nas.setReferenceValues(ReferenceConfig::defaultReferenceValues); +} + +bool NASController::start() +{ + // Add the task to the scheduler + size_t result = scheduler->addTask(bind(&NASController::update, this), + NASConfig::UPDATE_PERIOD, + TaskScheduler::Policy::RECOVER); + + return ActiveObject::start() && result != 0; +} + +void NASController::update() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Update the NAS state only if the FSM is active + if (this->testState(&NASController::state_active)) + { + // Get the IMU data + RotatedIMUData imuData = modules.get<Sensors>()->getIMULastSample(); + UBXGPSData gpsData = modules.get<Sensors>()->getGPSLastSample(); + + // Get barometer data + PressureData staticPressure = + modules.get<Sensors>()->getStaticPressure1LastSample(); + + // Compute the norm for acceleration validity + Vector3f accVector = static_cast<AccelerometerData>(imuData); + float accelerationNorm = accVector.norm(); + + // NAS prediction + nas.predictGyro(imuData); + nas.predictAcc(imuData); + + // NAS correction + nas.correctMag(imuData); + nas.correctGPS(gpsData); + nas.correctBaro(staticPressure.pressure); + + // Correct with accelerometer if the acceleration is in specs + if (accelerationValid) + { + nas.correctAcc(imuData); + } + + // Check boundaries + if ((accelerationNorm < + (Constants::g + (NASConfig::ACCELERATION_THRESHOLD) / 2) && + accelerationNorm > + (Constants::g - (NASConfig::ACCELERATION_THRESHOLD) / 2))) + { + if (!accelerationValid) + { + accSampleAfterSpike++; + } + } + else + { + accelerationValid = false; + accSampleAfterSpike = 0; + } + + // Enable the accelerometer after a fixed amount of good samples + if (accSampleAfterSpike > NASConfig::ACCELERATION_THRESHOLD_SAMPLE) + { + accSampleAfterSpike = 0; + accelerationValid = true; + } + + Logger::getInstance().log(nas.getState()); + + // printf("/w%fwa%fab%fbc%fc/\n", getNasState().qw, getNasState().qx, + // getNasState().qy, getNasState().qz); + } +} + +void NASController::calibrate() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + Vector3f acceleration = Vector3f::Zero(); + Vector3f magneticField = Vector3f::Zero(); + Stats pressure; + + for (int i = 0; i < NASConfig::CALIBRATION_SAMPLES_COUNT; i++) + { + // IMU + LSM6DSRXData imuData = modules.get<Sensors>()->getLSM6DSRXLastSample(); + acceleration += Vector3f(imuData.accelerationX, imuData.accelerationY, + imuData.accelerationZ); + + // Magnetometer + LIS2MDLData magData = modules.get<Sensors>()->getLIS2MDLLastSample(); + magneticField += + Vector3f(magData.magneticFieldX, magData.magneticFieldY, + magData.magneticFieldZ); + + // Static pressure barometer + PressureData barometerData = + modules.get<Sensors>()->getStaticPressure1LastSample(); + pressure.add(barometerData.pressure); + + miosix::Thread::sleep(NASConfig::CALIBRATION_SLEEP_TIME); + } + + // Normalize the data + acceleration /= NASConfig::CALIBRATION_SAMPLES_COUNT; + magneticField /= NASConfig::CALIBRATION_SAMPLES_COUNT; + acceleration.normalize(); + magneticField.normalize(); + + // Use the triad algorithm to estimate the initial orientation + StateInitializer state; + state.triad(acceleration, magneticField, ReferenceConfig::nedMag); + + // Set the pressure reference using an already existing reference values + ReferenceValues reference = nas.getReferenceValues(); + reference.refPressure = pressure.getStats().mean; + + // Set reference altitude using barometric measure + reference.refAltitude = Aeroutils::relAltitude(pressure.getStats().mean); + + // If in this moment the GPS has fix i use that position as starting + UBXGPSData gps = modules.get<Sensors>()->getGPSLastSample(); + if (gps.fix != 0) + { + // We don't set the altitude with the GPS because of not precise + // measurements + reference.refLatitude = gps.latitude; + reference.refLongitude = gps.longitude; + } + + // Update the algorithm reference values + { + miosix::PauseKernelLock lock; + nas.setX(state.getInitX()); + nas.setReferenceValues(reference); + } + + EventBroker::getInstance().post(NAS_READY, TOPIC_NAS); +} + +void NASController::setCoordinates(Vector2f position) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + ReferenceValues reference = nas.getReferenceValues(); + reference.refLatitude = position[0]; + reference.refLongitude = position[1]; + nas.setReferenceValues(reference); +} + +void NASController::setOrientation(float yaw, float pitch, float roll) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + Matrix<float, 13, 1> x = nas.getX(); + + // Set quaternions + Vector4f q = SkyQuaternion::eul2quat({yaw, pitch, roll}); + x(6) = q(0); + x(7) = q(1); + x(8) = q(2); + x(9) = q(3); + + nas.setX(x); +} + +void NASController::setReferenceAltitude(float altitude) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + ReferenceValues reference = nas.getReferenceValues(); + reference.refAltitude = altitude; + nas.setReferenceValues(reference); +} + +void NASController::setReferenceTemperature(float temperature) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + ReferenceValues reference = nas.getReferenceValues(); + + // The temperature is in degrees, converted in kelvin + reference.refTemperature = temperature + 273.15f; + nas.setReferenceValues(reference); +} + +void NASController::setReferenceValues(const ReferenceValues reference) +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + nas.setReferenceValues(reference); +} + +NASControllerStatus NASController::getStatus() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return status; +} + +NASState NASController::getNasState() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return nas.getState(); +} + +ReferenceValues NASController::getReferenceValues() +{ + // Need to pause the kernel + miosix::PauseKernelLock l; + return nas.getReferenceValues(); +} + +void NASController::state_idle(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(NASControllerState::IDLE); + } + case NAS_CALIBRATE: + { + return transition(&NASController::state_calibrating); + } + } +} + +void NASController::state_calibrating(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(NASControllerState::CALIBRATING); + + // Calibrate the NAS + calibrate(); + break; + } + case NAS_READY: + { + return transition(&NASController::state_ready); + } + } +} + +void NASController::state_ready(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(NASControllerState::READY); + } + case NAS_CALIBRATE: + { + return transition(&NASController::state_calibrating); + } + case NAS_FORCE_START: + case FLIGHT_ARMED: + { + return transition(&NASController::state_active); + } + } +} + +void NASController::state_active(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(NASControllerState::ACTIVE); + } + case FLIGHT_LANDING_DETECTED: + { + return transition(&NASController::state_end); + } + case NAS_FORCE_STOP: + case FLIGHT_DISARMED: + { + return transition(&NASController::state_ready); + } + } +} + +void NASController::state_end(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return logStatus(NASControllerState::END); + } + } +} + +void NASController::logStatus(NASControllerState state) +{ + { + miosix::PauseKernelLock lock; + // Update the current FSM state + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + } + + // Log the status + Logger::getInstance().log(status); +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h new file mode 100644 index 0000000000000000000000000000000000000000..01658f4122ec16ae1acf892d377e6581747c41c3 --- /dev/null +++ b/src/boards/Main/StateMachines/NASController/NASController.h @@ -0,0 +1,94 @@ +/* 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 <algorithms/NAS/NAS.h> +#include <diagnostic/PrintLogger.h> +#include <events/FSM.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +#include "NASControllerData.h" + +namespace Main +{ +class NASController : public Boardcore::FSM<NASController>, + public Boardcore::Module +{ +public: + NASController(Boardcore::TaskScheduler* sched); + + /** + * @brief Starts the FSM thread and adds an update function into the + * scheduler + */ + bool start() override; + + // NAS FSM called methods + void update(); + void calibrate(); + + // NAS setters + void setCoordinates(Eigen::Vector2f position); + void setOrientation(float yaw, float pitch, float roll); + void setReferenceAltitude(float altitude); + void setReferenceTemperature(float temperature); + void setReferenceValues(const Boardcore::ReferenceValues reference); + + // NAS Getters + NASControllerStatus getStatus(); + Boardcore::NASState getNasState(); + Boardcore::ReferenceValues getReferenceValues(); + + // FSM states + void state_idle(const Boardcore::Event& event); + void state_calibrating(const Boardcore::Event& event); + void state_ready(const Boardcore::Event& event); + void state_active(const Boardcore::Event& event); + void state_end(const Boardcore::Event& event); + +private: + /** + * @brief Logs the NAS status updating the FSM state + * @param state The current FSM state + */ + void logStatus(NASControllerState state); + + // Controller state machine status + NASControllerStatus status; + Boardcore::NAS nas; + + // Scheduler to be used for update function + Boardcore::TaskScheduler* scheduler; + + // User set (or triac set) initial orientation + Eigen::Vector3f initialOrientation; + + // Acceleration correction parameters + bool accelerationValid = true; + unsigned int accSampleAfterSpike = 0; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("NAS"); +}; +} // namespace Main diff --git a/src/boards/Main/StateMachines/NASController/NASControllerData.h b/src/boards/Main/StateMachines/NASController/NASControllerData.h new file mode 100644 index 0000000000000000000000000000000000000000..559722a552f163863d269ca81edc517b9d07b5d8 --- /dev/null +++ b/src/boards/Main/StateMachines/NASController/NASControllerData.h @@ -0,0 +1,56 @@ +/* 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 Main +{ + +enum class NASControllerState : uint8_t +{ + UNINIT = 0, + IDLE, + CALIBRATING, + READY, + ACTIVE, + END +}; + +struct NASControllerStatus +{ + long long timestamp = 0; + NASControllerState state = NASControllerState::UNINIT; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +} // namespace Main diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main/TMRepository/TMRepository.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fc612e4056814826b735d1358b189efade719d45 --- /dev/null +++ b/src/boards/Main/TMRepository/TMRepository.cpp @@ -0,0 +1,736 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/BoardScheduler.h> +#include <Main/Configs/RadioConfig.h> +#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> +#include <Main/PinHandler/PinHandler.h> +#include <Main/Radio/Radio.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/ABKController/ABKController.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <Main/StateMachines/Deployment/Deployment.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/MEAController/MEAController.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <Main/TMRepository/TMRepository.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <interfaces-impl/hwmapping.h> + +using namespace miosix; +using namespace Boardcore; + +namespace Main +{ +mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId, + uint8_t seq) +{ + ModuleManager& modules = ModuleManager::getInstance(); + mavlink_message_t msg; + + switch (tmId) + { + case SystemTMList::MAV_SYS_ID: + { + mavlink_sys_tm_t tm; + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.logger = Logger::getInstance().isStarted(); + tm.board_scheduler = modules.get<BoardScheduler>()->isStarted(); + tm.event_broker = EventBroker::getInstance().isRunning(); + tm.radio = modules.get<Radio>()->isStarted(); + tm.sensors = modules.get<Sensors>()->isStarted(); + tm.pin_observer = modules.get<PinHandler>()->isStarted(); + + mavlink_msg_sys_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case SystemTMList::MAV_LOGGER_ID: + { + mavlink_logger_tm_t tm; + + // Get the logger stats + LoggerStats stats = Logger::getInstance().getStats(); + + tm.timestamp = stats.timestamp; + tm.average_write_time = stats.averageWriteTime; + tm.buffers_filled = stats.buffersFilled; + tm.buffers_written = stats.buffersWritten; + tm.dropped_samples = stats.droppedSamples; + tm.last_write_error = stats.lastWriteError; + tm.log_number = stats.logNumber; + tm.max_write_time = stats.maxWriteTime; + tm.queued_samples = stats.queuedSamples; + tm.too_large_samples = stats.tooLargeSamples; + tm.writes_failed = stats.writesFailed; + + mavlink_msg_logger_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case SystemTMList::MAV_MAVLINK_STATS: + { + mavlink_mavlink_stats_tm_t tm; + + // Get the mavlink stats + 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(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, + &tm); + break; + } + case SystemTMList::MAV_NAS_ID: + { + mavlink_nas_tm_t tm; + + // Get the current NAS state + NASState state = modules.get<NASController>()->getNasState(); + ReferenceValues ref = + modules.get<NASController>()->getReferenceValues(); + + tm.timestamp = state.timestamp; + tm.state = static_cast<uint8_t>( + modules.get<NASController>()->getStatus().state); + tm.nas_n = state.n; + tm.nas_e = state.e; + tm.nas_d = state.d; + tm.nas_vn = state.vn; + tm.nas_ve = state.ve; + tm.nas_vd = state.vd; + tm.nas_qx = state.qx; + tm.nas_qy = state.qy; + tm.nas_qz = state.qz; + tm.nas_qw = state.qw; + tm.nas_bias_x = state.bx; + tm.nas_bias_y = state.by; + tm.nas_bias_z = state.bz; + tm.ref_pressure = ref.refPressure; + tm.ref_temperature = ref.refTemperature; + tm.ref_latitude = ref.refLatitude; + tm.ref_longitude = ref.refLongitude; + + mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case SystemTMList::MAV_ADA_ID: + { + mavlink_ada_tm_t tm; + + // Get the current ADA state + ADAState state = modules.get<ADAController>()->getADAState(); + ReferenceValues ref = + modules.get<ADAController>()->getReferenceValues(); + + tm.timestamp = state.timestamp; + tm.state = static_cast<uint8_t>( + modules.get<ADAController>()->getStatus().state); + tm.kalman_x0 = state.x0; + tm.kalman_x1 = state.x1; + tm.kalman_x2 = state.x2; + tm.vertical_speed = state.verticalSpeed; + tm.msl_altitude = state.mslAltitude; + tm.msl_pressure = ref.mslPressure; + tm.msl_temperature = ref.mslTemperature - 273.15f; + tm.ref_altitude = ref.refAltitude; + tm.ref_temperature = ref.refTemperature - 273.15f; + tm.ref_pressure = ref.refPressure; + tm.dpl_altitude = 0; + + mavlink_msg_ada_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case SystemTMList::MAV_FLIGHT_ID: + { + mavlink_rocket_flight_tm_t tm; + + tm.timestamp = TimestampTimer::getTimestamp(); + + // Last samples + LSM6DSRXData lsm6dsrx = + modules.get<Sensors>()->getLSM6DSRXLastSample(); + LPS28DFWData lps28dfw1 = + modules.get<Sensors>()->getLPS28DFW_1LastSample(); + LIS2MDLData lis2mdl = + modules.get<Sensors>()->getLIS2MDLLastSample(); + PressureData staticPressure = + modules.get<Sensors>()->getStaticPressure1LastSample(); + PressureData deploymentPressure = + modules.get<Sensors>()->getDeploymentPressureLastSample(); + UBXGPSData gps = modules.get<Sensors>()->getGPSLastSample(); + + // NAS state + NASState nasState = modules.get<NASController>()->getNasState(); + + // ADA state + ADAState adaState = modules.get<ADAController>()->getADAState(); + + // State machines + tm.mea_state = static_cast<uint8_t>( + modules.get<MEAController>()->getStatus().state); + tm.ada_state = static_cast<uint8_t>( + modules.get<ADAController>()->getStatus().state); + tm.nas_state = static_cast<uint8_t>( + modules.get<NASController>()->getStatus().state); + tm.abk_state = static_cast<uint8_t>( + modules.get<ABKController>()->getStatus().state); + tm.fmm_state = static_cast<uint8_t>( + modules.get<FlightModeManager>()->getStatus().state); + tm.dpl_state = static_cast<uint8_t>( + modules.get<Deployment>()->getStatus().state); + + // Pressures + tm.pressure_ada = adaState.x0; + tm.pressure_digi = lps28dfw1.pressure; + tm.pressure_dpl = deploymentPressure.pressure; + tm.pressure_static = staticPressure.pressure; + + // Altitude agl + tm.altitude_agl = -nasState.d; + + // IMU + tm.acc_x = lsm6dsrx.accelerationX; + tm.acc_y = lsm6dsrx.accelerationY; + tm.acc_z = lsm6dsrx.accelerationZ; + tm.gyro_x = lsm6dsrx.angularSpeedX; + tm.gyro_y = lsm6dsrx.angularSpeedY; + tm.gyro_z = lsm6dsrx.angularSpeedZ; + + // Magnetometer + tm.mag_x = lis2mdl.magneticFieldX; + tm.mag_y = lis2mdl.magneticFieldY; + tm.mag_z = lis2mdl.magneticFieldZ; + + // GPS + tm.gps_fix = gps.fix; + tm.gps_lat = gps.latitude; + tm.gps_lon = gps.longitude; + tm.gps_alt = gps.height; + + // NAS + tm.nas_n = nasState.n; + tm.nas_e = nasState.e; + tm.nas_d = nasState.d; + tm.nas_vn = nasState.vn; + tm.nas_ve = nasState.ve; + tm.nas_vd = nasState.vd; + tm.nas_qx = nasState.qx; + tm.nas_qy = nasState.qy; + tm.nas_qz = nasState.qz; + tm.nas_qw = nasState.qw; + tm.nas_bias_x = nasState.bx; + tm.nas_bias_y = nasState.by; + tm.nas_bias_z = nasState.bz; + + // ADA + tm.ada_vert_speed = adaState.verticalSpeed; + + // ABK + tm.abk_angle = modules.get<Actuators>()->getServoPosition( + ServosList::AIR_BRAKES_SERVO); + + // Pins + tm.pin_launch = modules.get<PinHandler>() + ->getPinData(PinHandler::PinList::LAUNCH_PIN) + .lastState; + tm.pin_nosecone = + modules.get<PinHandler>() + ->getPinData(PinHandler::PinList::NOSECONE_PIN) + .lastState; + tm.pin_expulsion = + modules.get<PinHandler>() + ->getPinData(PinHandler::PinList::PIN_EXPULSION) + .lastState; + + // Board status + tm.battery_voltage = modules.get<Sensors>() + ->getBatteryVoltageLastSample() + .batVoltage; + tm.cam_battery_voltage = modules.get<Sensors>() + ->getCamBatteryVoltageLastSample() + .batVoltage; + tm.current_consumption = + modules.get<Sensors>()->getCurrentLastSample().current; + tm.temperature = lps28dfw1.temperature; + tm.logger_error = Logger::getInstance().getStats().lastWriteError; + tm.cutter_presence = + modules.get<PinHandler>() + ->getPinData(PinHandler::PinList::CUTTER_PRESENCE) + .lastState; + + // Pitot CAN sensor + tm.airspeed_pitot = + modules.get<Sensors>()->getPitotLastSample().airspeed; + + mavlink_msg_rocket_flight_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, + &tm); + + break; + } + case SystemTMList::MAV_STATS_ID: + { + mavlink_rocket_stats_tm_t tm = + modules.get<FlightStatsRecorder>()->getStats(); + + tm.cpu_load = CpuMeter::getCpuStats().mean; + tm.free_heap = CpuMeter::getCpuStats().freeHeap; + + mavlink_msg_rocket_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, + &tm); + break; + } + case SystemTMList::MAV_FSM_ID: + { + mavlink_fsm_tm_t tm; + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.abk_state = static_cast<uint8_t>( + modules.get<ABKController>()->getStatus().state); + tm.ada_state = static_cast<uint8_t>( + modules.get<ADAController>()->getStatus().state); + tm.dpl_state = static_cast<uint8_t>( + modules.get<Deployment>()->getStatus().state); + tm.fmm_state = static_cast<uint8_t>( + modules.get<FlightModeManager>()->getStatus().state); + tm.nas_state = static_cast<uint8_t>( + modules.get<NASController>()->getStatus().state); + + mavlink_msg_fsm_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case SystemTMList::MAV_MOTOR_ID: + { + mavlink_motor_tm_t tm; + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.bottom_tank_pressure = modules.get<Sensors>() + ->getBottomTankPressureLastSample() + .pressure; + tm.combustion_chamber_pressure = + modules.get<Sensors>()->getCCPressureLastSample().pressure; + tm.tank_temperature = modules.get<Sensors>() + ->getTankTemperatureLastSample() + .temperature; + // clang-format off + tm.main_valve_state = modules.get<Actuators>()->getServoPosition( + ServosList::MAIN_VALVE) > 0.3 ? 1 : 0; + tm.venting_valve_state = modules.get<Actuators>()->getServoPosition( + ServosList::VENTING_VALVE) > 0.3 ? 1 : 0; + // clang-format on + + tm.top_tank_pressure = + modules.get<Sensors>()->getTopTankPressureLastSample().pressure; + tm.battery_voltage = + modules.get<Sensors>()->getMotorBatteryVoltage().batVoltage; + tm.current_consumption = + modules.get<Sensors>()->getMotorCurrent().current; + + mavlink_msg_motor_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + break; + } + default: + { + // Send by default the nack message + mavlink_nack_tm_t nack; + + nack.recv_msgid = msgId; + nack.seq_ack = seq; + + LOG_ERR(logger, "Unknown message id: {}", tmId); + mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &nack); + + break; + } + } + return msg; +} + +mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId, + uint8_t msgId, uint8_t seq) +{ + ModuleManager& modules = ModuleManager::getInstance(); + mavlink_message_t msg; + + switch (sensorId) + { + case MAV_GPS_ID: + { + mavlink_gps_tm_t tm; + + UBXGPSData data = modules.get<Sensors>()->getGPSLastSample(); + + tm.fix = data.fix; + tm.height = data.height; + tm.latitude = data.latitude; + tm.longitude = data.longitude; + tm.n_satellites = data.satellites; + strcpy(tm.sensor_name, "UBXGPS"); + tm.speed = data.speed; + tm.timestamp = data.gpsTimestamp; + tm.track = data.track; + tm.vel_down = data.velocityDown; + tm.vel_east = data.velocityEast; + tm.vel_north = data.velocityNorth; + + mavlink_msg_gps_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_ADS_ID: + { + mavlink_adc_tm_t tm; + + ADS131M08Data data = + modules.get<Sensors>()->getADS131M08LastSample(); + + tm.channel_0 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage; + tm.channel_1 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage; + tm.channel_2 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage; + tm.channel_3 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage; + tm.channel_4 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage; + tm.channel_5 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage; + tm.channel_6 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage; + tm.channel_7 = + data.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage; + tm.timestamp = data.timestamp; + + strcpy(tm.sensor_name, "ADS131M08"); + + mavlink_msg_adc_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_CURRENT_SENSE_ID: + { + mavlink_current_tm_t tm; + + CurrentData data = modules.get<Sensors>()->getCurrentLastSample(); + + tm.current = data.current; + tm.timestamp = data.currentTimestamp; + strcpy(tm.sensor_name, "CURRENT"); + + mavlink_msg_current_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_DPL_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData data = + modules.get<Sensors>()->getDeploymentPressureLastSample(); + + tm.pressure = data.pressure; + tm.timestamp = data.pressureTimestamp; + strcpy(tm.sensor_name, "DPL_PRESSURE"); + + mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_STATIC_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData data = + modules.get<Sensors>()->getStaticPressure1LastSample(); + + tm.pressure = data.pressure; + tm.timestamp = data.pressureTimestamp; + strcpy(tm.sensor_name, "STATIC_PRESSURE"); + + mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_BATTERY_VOLTAGE_ID: + { + mavlink_voltage_tm_t tm; + + BatteryVoltageSensorData data = + modules.get<Sensors>()->getBatteryVoltageLastSample(); + + tm.voltage = data.batVoltage; + tm.timestamp = data.voltageTimestamp; + strcpy(tm.sensor_name, "BATTERY_VOLTAGE"); + + mavlink_msg_voltage_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_TANK_TOP_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData data = + modules.get<Sensors>()->getTopTankPressureLastSample(); + + tm.pressure = data.pressure; + tm.timestamp = data.pressureTimestamp; + strcpy(tm.sensor_name, "TOP_TANK"); + + mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_TANK_BOTTOM_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData data = + modules.get<Sensors>()->getBottomTankPressureLastSample(); + + tm.pressure = data.pressure; + tm.timestamp = data.pressureTimestamp; + strcpy(tm.sensor_name, "BOTTOM_TANK"); + + mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_COMBUSTION_PRESS_ID: + { + mavlink_pressure_tm_t tm; + + PressureData data = + modules.get<Sensors>()->getCCPressureLastSample(); + + tm.pressure = data.pressure; + tm.timestamp = data.pressureTimestamp; + strcpy(tm.sensor_name, "COMBUSTION_CHAMBER"); + + mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_TANK_TEMP_ID: + { + mavlink_temp_tm_t tm; + + TemperatureData data = + modules.get<Sensors>()->getTankTemperatureLastSample(); + + tm.temperature = data.temperature; + tm.timestamp = data.temperatureTimestamp; + strcpy(tm.sensor_name, "TANK_TEMPERATURE"); + + mavlink_msg_temp_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_LIS2MDL_ID: + { + mavlink_imu_tm_t tm; + + LIS2MDLData data = modules.get<Sensors>()->getLIS2MDLLastSample(); + + tm.acc_x = 0; + tm.acc_y = 0; + tm.acc_z = 0; + tm.gyro_x = 0; + tm.gyro_y = 0; + tm.gyro_z = 0; + + tm.mag_x = data.magneticFieldX; + tm.mag_y = data.magneticFieldY; + tm.mag_z = data.magneticFieldZ; + tm.timestamp = data.magneticFieldTimestamp; + strcpy(tm.sensor_name, "LIS2MDL"); + + mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_LPS28DFW_ID: + { + mavlink_pressure_tm_t tm; + + LPS28DFWData data = + modules.get<Sensors>()->getLPS28DFW_1LastSample(); + + tm.pressure = data.pressure; + tm.timestamp = data.pressureTimestamp; + strcpy(tm.sensor_name, "LPS28DFW"); + + mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_LSM6DSRX_ID: + { + mavlink_imu_tm_t tm; + + LSM6DSRXData data = modules.get<Sensors>()->getLSM6DSRXLastSample(); + + tm.mag_x = 0; + tm.mag_y = 0; + tm.mag_z = 0; + tm.acc_x = data.accelerationX; + tm.acc_y = data.accelerationY; + tm.acc_z = data.accelerationZ; + tm.gyro_x = data.angularSpeedX; + tm.gyro_y = data.angularSpeedY; + tm.gyro_z = data.angularSpeedZ; + tm.timestamp = data.accelerationTimestamp; + strcpy(tm.sensor_name, "LSM6DSRX"); + + mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_H3LIS331DL_ID: + { + mavlink_imu_tm_t tm; + + H3LIS331DLData data = + modules.get<Sensors>()->getH3LIS331DLLastSample(); + + tm.mag_x = 0; + tm.mag_y = 0; + tm.mag_z = 0; + tm.gyro_x = 0; + tm.gyro_y = 0; + tm.gyro_z = 0; + tm.acc_x = data.accelerationX; + tm.acc_y = data.accelerationY; + tm.acc_z = data.accelerationZ; + tm.timestamp = data.accelerationTimestamp; + strcpy(tm.sensor_name, "H3LIS331DL"); + + mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + case MAV_LPS22DF_ID: + { + mavlink_pressure_tm_t tm; + + LPS22DFData data = modules.get<Sensors>()->getLPS22DFLastSample(); + + tm.pressure = data.pressure; + tm.timestamp = data.pressureTimestamp; + strcpy(tm.sensor_name, "LPS22DF"); + + mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + + break; + } + default: + { + // Send by default the nack message + mavlink_nack_tm_t nack; + + nack.recv_msgid = msgId; + nack.seq_ack = seq; + + LOG_ERR(logger, "Unknown sensor id: {}", sensorId); + mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &nack); + + break; + } + } + + return msg; +} + +mavlink_message_t TMRepository::packServoTm(ServosList servoId, uint8_t msgId, + uint8_t seq) +{ + ModuleManager& modules = ModuleManager::getInstance(); + mavlink_message_t msg; + + if (servoId == AIR_BRAKES_SERVO || servoId == EXPULSION_SERVO || + servoId == MAIN_VALVE || servoId == VENTING_VALVE) + { + mavlink_servo_tm_t tm; + tm.servo_id = servoId; + tm.servo_position = modules.get<Actuators>()->getServoPosition(servoId); + + mavlink_msg_servo_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &tm); + } + else + { + mavlink_nack_tm_t nack; + nack.recv_msgid = msgId; + nack.seq_ack = seq; + + mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, &nack); + } + + return msg; +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/TMRepository/TMRepository.h b/src/boards/Main/TMRepository/TMRepository.h new file mode 100644 index 0000000000000000000000000000000000000000..9cb72f0acb6185aa1ffc34744864929837c50e18 --- /dev/null +++ b/src/boards/Main/TMRepository/TMRepository.h @@ -0,0 +1,52 @@ +/* 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 <common/Mavlink.h> +#include <diagnostic/PrintLogger.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +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 Main \ No newline at end of file diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..aa575195a0d393861dfcf3bacd6f0c9dbeed70c5 --- /dev/null +++ b/src/entrypoints/Main/main-entry.cpp @@ -0,0 +1,319 @@ +/* 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 <Main/Actuators/Actuators.h> +#include <Main/AltitudeTrigger/AltitudeTrigger.h> +#include <Main/BoardScheduler.h> +#include <Main/Buses.h> +#include <Main/CanHandler/CanHandler.h> +#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> +#include <Main/PinHandler/PinHandler.h> +#include <Main/Radio/Radio.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/ABKController/ABKController.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <Main/StateMachines/Deployment/Deployment.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/MEAController/MEAController.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <Main/TMRepository/TMRepository.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <diagnostic/PrintLogger.h> +#include <diagnostic/StackLogger.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <events/EventData.h> +#include <events/utils/EventSniffer.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +using namespace Boardcore; +using namespace Main; +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 + BoardScheduler* scheduler = new BoardScheduler(); + Buses* buses = new Buses(); + Sensors* sensors = + new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1)); + NASController* nas = + new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + ADAController* ada = + new ADAController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); + TMRepository* tmRepo = new TMRepository(); + CanHandler* canHandler = + new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); + FlightModeManager* fmm = new FlightModeManager(); + Actuators* actuators = + new Actuators(scheduler->getScheduler(miosix::MAIN_PRIORITY)); + Deployment* dpl = new Deployment(); + PinHandler* pinHandler = new PinHandler(); + AltitudeTrigger* altitudeTrigger = + new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX)); + MEAController* mea = + new MEAController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + ABKController* abk = + new ABKController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + FlightStatsRecorder* recorder = + new FlightStatsRecorder(scheduler->getScheduler(miosix::MAIN_PRIORITY)); + + // Insert modules + if (!modules.insert<BoardScheduler>(scheduler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Board Scheduler module"); + } + + if (!modules.insert<Buses>(buses)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Buses module"); + } + + if (!modules.insert<Sensors>(sensors)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Sensor module"); + } + + if (!modules.insert<Actuators>(actuators)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Actuators module"); + } + + if (!modules.insert<Deployment>(dpl)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the DPL module"); + } + + if (!modules.insert<NASController>(nas)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the NAS module"); + } + + if (!modules.insert<ADAController>(ada)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the ADA module"); + } + + if (!modules.insert<MEAController>(mea)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the MEA module"); + } + + if (!modules.insert<ABKController>(abk)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the ABK controller module"); + } + + if (!modules.insert<Radio>(radio)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Radio module"); + } + + if (!modules.insert<FlightModeManager>(fmm)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the FMM module"); + } + + if (!modules.insert<TMRepository>(tmRepo)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the TMRepository module"); + } + + if (!modules.insert<CanHandler>(canHandler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the CanHandler module"); + } + + if (!modules.insert<PinHandler>(pinHandler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the PinHandler module"); + } + + if (!modules.insert<AltitudeTrigger>(altitudeTrigger)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Altitude Trigger module"); + } + + if (!modules.insert<FlightStatsRecorder>(recorder)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Flight Stats Recorder module"); + } + + // Start modules + if (!Logger::getInstance().testSDCard()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Logger module"); + } + + if (!EventBroker::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the EventBroker module"); + } + + if (!modules.get<BoardScheduler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Board Scheduler module"); + } + + if (!modules.get<Actuators>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Actuators module"); + } + + if (!modules.get<Deployment>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Deployment module"); + } + + if (!modules.get<Sensors>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Sensors module"); + } + + if (!modules.get<NASController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the NAS module"); + } + + if (!modules.get<ADAController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the ADA module"); + } + + if (!modules.get<MEAController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the MEA module"); + } + + if (!modules.get<ABKController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the ABK controller module"); + } + + if (!modules.get<FlightModeManager>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the FMM module"); + } + + if (!modules.get<Radio>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Radio module"); + } + + if (!modules.get<CanHandler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the CanHandler module"); + } + + if (!modules.get<PinHandler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the PinHandler module"); + } + + if (!modules.get<AltitudeTrigger>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Altitude Trigger module"); + } + + if (!modules.get<FlightStatsRecorder>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Flight Stats Recorder module"); + } + + // Log all the events + EventSniffer sniffer( + EventBroker::getInstance(), TOPICS_LIST, + [](uint8_t event, uint8_t topic) + { + EventData ev{TimestampTimer::getTimestamp(), event, topic}; + Logger::getInstance().log(ev); + }); + + // Check the init result and launch an event + if (initResult) + { + // Post OK + EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM); + + // Set the LED status + miosix::led1On(); + } + else + { + EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM); + LOG_ERR(logger, "Failed to initialize"); + } + + // 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 new file mode 100644 index 0000000000000000000000000000000000000000..4fb6d768353e40f6f25ed01cf497920e8b3713b5 --- /dev/null +++ b/src/scripts/EventDumper/EventDumper.cpp @@ -0,0 +1,42 @@ +/* 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 <common/Events.h> + +#include <iostream> +#include <string> + +using namespace std; + +int main() +{ + // Scan all the indices and print their correspondent name + for (int i = Boardcore::EV_FIRST_CUSTOM; i < 256; i++) + { + if (Common::getEventString(i).compare("EV_UNKNOWN") == 0) + { + break; + } + cout << Common::getEventString(i) << "," << i << endl; + } + return 0; +} diff --git a/src/scripts/EventDumper/Makefile b/src/scripts/EventDumper/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..61729aa2876d8cb8188f38b05fe68bbce4dc065e --- /dev/null +++ b/src/scripts/EventDumper/Makefile @@ -0,0 +1,16 @@ +BOARDCORE := ../../../skyward-boardcore/ +OBSW := ../../../src/boards/ + +all: + g++ -std=c++17 -o eventdumper EventDumper.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/eventgen.sh b/src/scripts/eventgen.sh new file mode 100755 index 0000000000000000000000000000000000000000..6763addd5072948bc8e191ff4e61e4c31c20494a --- /dev/null +++ b/src/scripts/eventgen.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright (c) 2021 Skyward Experimental Rocketry +# Authors: 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. + +DIRNAME="$(dirname $0)" +$DIRNAME/../skyward-boardcore/scripts/generators/eventgen.py $@ diff --git a/src/scripts/fsmgen.sh b/src/scripts/fsmgen.sh new file mode 100755 index 0000000000000000000000000000000000000000..2bae83d7f938ec78624784bfbb93aaa0f17e4431 --- /dev/null +++ b/src/scripts/fsmgen.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright (c) 2021 Skyward Experimental Rocketry +# Authors: 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. + +DIRNAME="$(dirname $0)" +$DIRNAME/../skyward-boardcore/scripts/generators/fsmgen.py $@ diff --git a/src/scripts/logdecoder/Main/Makefile b/src/scripts/logdecoder/Main/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..01b0967730c29d85054d1eb3e3df4b1cc605d255 --- /dev/null +++ b/src/scripts/logdecoder/Main/Makefile @@ -0,0 +1,18 @@ +BOARDCORE := ../../../../skyward-boardcore/ +OBSW := ../../../../src/boards/ +HIL := ../../../../src/ + +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) \ + -I$(HIL) +clean: + rm logdecoder diff --git a/src/scripts/logdecoder/Main/logdecoder.cpp b/src/scripts/logdecoder/Main/logdecoder.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e656031c7fe16b3ea8d98bb2fc15c57f4cbb040f --- /dev/null +++ b/src/scripts/logdecoder/Main/logdecoder.cpp @@ -0,0 +1,158 @@ +/* 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 <Main/Sensors/SensorsData.h> +#include <Main/CanHandler/CanHandlerData.h> +#include <Main/PinHandler/PinData.h> +#include <Main/Sensors/RotatedIMU/RotatedIMUData.h> +#include <Main/Sensors/SensorsData.h> +#include <Main/StateMachines/ABKController/ABKControllerData.h> +#include <Main/StateMachines/ADAController/ADAControllerData.h> +#include <Main/StateMachines/Deployment/DeploymentData.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManagerData.h> +#include <Main/StateMachines/MEAController/MEAControllerData.h> +#include <Main/StateMachines/NASController/NASControllerData.h> +#include <algorithms/MEA/MEAData.h> +#include <hardware_in_the_loop/HIL_sensors/HILSensorsData.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 Main; + +void registerTypes(Deserializer& ds) +{ + // Register all Boardcore types + LogTypes::registerTypes(ds); + + // Custom types + ds.registerType<FlightModeManagerStatus>(); + ds.registerType<NASControllerStatus>(); + ds.registerType<MEAControllerStatus>(); + ds.registerType<MEAState>(); + ds.registerType<ADAControllerStatus>(); + ds.registerType<ABKControllerStatus>(); + ds.registerType<DeploymentStatus>(); + ds.registerType<LPS28DFW_1Data>(); + ds.registerType<LPS28DFW_2Data>(); + ds.registerType<HSCMRNN015PA_1Data>(); + ds.registerType<HSCMRNN015PA_2Data>(); + ds.registerType<RotatedIMUData>(); + ds.registerType<CanPressureSensor>(); + ds.registerType<CanTemperatureSensor>(); + ds.registerType<CanCurrentSensor>(); + ds.registerType<CanVoltageSensor>(); + ds.registerType<SensorsCalibrationParameter>(); + ds.registerType<PinChangeData>(); + ds.registerType<HILAccelerometerData>(); + ds.registerType<HILBarometerData>(); + ds.registerType<HILGpsData>(); + ds.registerType<HILGyroscopeData>(); + ds.registerType<HILImuData>(); + ds.registerType<HILMagnetometerData>(); + ds.registerType<HILPitotData>(); + ds.registerType<HILTempData>(); +} + +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; +}