diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 4b020343a7afcbcdf7f2ad77dadb3ca22c89088c..78fa0a5bf72b56797516944e56d65f09ad4c7863 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -74,6 +74,7 @@ "HSE_VALUE=8000000", "SYSCLK_FREQ_168MHz=168000000", "_MIOSIX", + "HILSimulation", "__cplusplus=201103L" ], "includePath": [ @@ -185,6 +186,70 @@ }, "configurationProvider": "ms-vscode.cmake-tools" }, + { + "name": "stm32f429zi_skyward_death_stack_x_maker_faire", + "cStandard": "c11", + "cppStandard": "c++11", + "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", + "defines": [ + "DEBUG", + "HILSimulation", + "_ARCH_CORTEXM4_STM32F4", + "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X_MAKER_FAIRE", + "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x_maker_faire", + "HSE_VALUE=8000000", + "SYSCLK_FREQ_168MHz=168000000", + "_MIOSIX", + "__cplusplus=201103L" + ], + "includePath": [ + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix", + "${workspaceFolder}/skyward-boardcore/libs/Catch2/single_include", + "${workspaceFolder}/skyward-boardcore/libs/mavlink-skyward-lib", + "${workspaceFolder}/skyward-boardcore/libs/fmt/include", + "${workspaceFolder}/skyward-boardcore/libs/eigen", + "${workspaceFolder}/skyward-boardcore/libs/tscpp", + "${workspaceFolder}/skyward-boardcore/libs", + "${workspaceFolder}/skyward-boardcore/src/shared", + "${workspaceFolder}/skyward-boardcore/src/tests", + "${workspaceFolder}/src/hardware_in_the_loop", + "${workspaceFolder}/src/boards", + "${workspaceFolder}/src/tests", + "${workspaceFolder}/src" + ], + "browse": { + "path": [ + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/stdlib_integration", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/interfaces", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/filesystem", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/kernel", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/util", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/e20", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/*", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix", + "${workspaceFolder}/skyward-boardcore/libs/mavlink-skyward-lib", + "${workspaceFolder}/skyward-boardcore/libs/eigen", + "${workspaceFolder}/skyward-boardcore/libs/tscpp", + "${workspaceFolder}/skyward-boardcore/libs/mxgui", + "${workspaceFolder}/skyward-boardcore/libs/fmt", + "${workspaceFolder}/skyward-boardcore/src/shared", + "${workspaceFolder}/skyward-boardcore/src/tests", + "${workspaceFolder}/src/hardware_in_the_loop", + "${workspaceFolder}/src/boards", + "${workspaceFolder}/src/tests" + ], + "limitSymbolsToIncludedHeaders": true + }, + "configurationProvider": "ms-vscode.cmake-tools" + }, { "name": "stm32f429zi_skyward_death_stack_v3", "cStandard": "c11", @@ -683,4 +748,4 @@ } ], "version": 4 -} \ No newline at end of file +} diff --git a/.vscode/settings.json b/.vscode/settings.json index d64d9495c135a6f23bd27faa31950c80082b3441..743392d0cbad610129652fe8bcf299b7831f3e7f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,10 +1,7 @@ { - "cmake.configureSettings": { - "CMAKE_C_COMPILER_LAUNCHER": "ccache", - "CMAKE_CXX_COMPILER_LAUNCHER": "ccache" - }, "cmake.parallelJobs": 1, "files.associations": { + "**/miosix-kernel/**": "c", "sstream": "cpp", "format": "cpp", "any": "cpp", @@ -92,7 +89,38 @@ "specialfunctions": "cpp", "splines": "cpp", "matrixfunctions": "cpp", - "tensorsymmetry": "cpp" + "tensorsymmetry": "cpp", + "ios": "cpp", + "charconv": "cpp", + "compare": "cpp", + "concepts": "cpp", + "forward_list": "cpp", + "locale": "cpp", + "queue": "cpp", + "stop_token": "cpp", + "xfacet": "cpp", + "xhash": "cpp", + "xiosbase": "cpp", + "xlocale": "cpp", + "xlocbuf": "cpp", + "xlocinfo": "cpp", + "xlocmes": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp", + "xmemory": "cpp", + "xstddef": "cpp", + "xstring": "cpp", + "xtr1common": "cpp", + "xtree": "cpp", + "xutility": "cpp" + }, + "editor.formatOnSave": true, + "[c]": { + "editor.formatOnSave": false + }, + "[cmake]": { + "editor.formatOnSave": false }, "cSpell.words": [ "Aeroutils", @@ -138,5 +166,9 @@ "VREF", "Xbee" ], - "C_Cpp.errorSquiggles": "Enabled" -} + "C_Cpp.errorSquiggles": "Enabled", + "cmake.configureSettings": { + "CMAKE_C_COMPILER_LAUNCHER": "ccache", + "CMAKE_CXX_COMPILER_LAUNCHER": "ccache" + } +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index e85e8d7a62ed79213cfd0bd77d90b63fb3457103..1bee016e7317b5bd31b5b4051942f14b80ec3036 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,19 +54,26 @@ sbs_target(main-entry-milano stm32f429zi_skyward_death_stack_v3) add_executable(main-entry-hil-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL}) target_include_directories(main-entry-hil-euroc PRIVATE ${OBSW_INCLUDE_DIRS}) -target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW) +target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP) sbs_target(main-entry-hil-euroc stm32f429zi_skyward_death_stack_v3) add_executable(main-entry-hil-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL}) target_include_directories(main-entry-hil-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS}) -target_compile_definitions(main-entry-hil-roccaraso PRIVATE HILSimulation ROCCARASO BUZZER_LOW) +target_compile_definitions(main-entry-hil-roccaraso PRIVATE HILSimulation ROCCARASO BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP) sbs_target(main-entry-hil-roccaraso stm32f429zi_skyward_death_stack_v3) add_executable(main-entry-hil-milano src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL}) target_include_directories(main-entry-hil-milano PRIVATE ${OBSW_INCLUDE_DIRS}) -target_compile_definitions(main-entry-hil-milano PRIVATE HILSimulation MILANO BUZZER_LOW) +target_compile_definitions(main-entry-hil-milano PRIVATE HILSimulation MILANO BUZZER_LOW USE_SERIAL_TRANSCEIVER) sbs_target(main-entry-hil-milano stm32f429zi_skyward_death_stack_v3) +add_executable(main-entry-hil-maker-faire src/entrypoints/Main/main-entry-maker-faire.cpp ${MAIN_COMPUTER} ${HIL}) +# add_executable(main-entry-hil-maker-faire src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL}) +target_include_directories(main-entry-hil-maker-faire PRIVATE ${OBSW_INCLUDE_DIRS}) +target_compile_definitions(main-entry-hil-maker-faire PRIVATE HILSimulation EUROC BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP DEATHSTACK_V2) +sbs_target(main-entry-hil-maker-faire stm32f429zi_skyward_death_stack_x_maker_faire) +# sbs_target(main-entry-hil-maker-faire stm32f429zi_stm32f4discovery) + add_executable(payload-entry-milano src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER}) target_include_directories(payload-entry-milano PRIVATE ${OBSW_INCLUDE_DIRS}) target_compile_definitions(payload-entry-milano PRIVATE MILANO) @@ -86,6 +93,11 @@ add_executable(ciuti-entry src/entrypoints/Ciuti/ciuti-entry.cpp ${CIUTI_COMPUTE target_include_directories(ciuti-entry PRIVATE ${OBSW_INCLUDE_DIRS}) sbs_target(ciuti-entry stm32f205rc_skyward_ciuti) +add_executable(payload-entry-hil src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER} ${HIL}) +target_include_directories(payload-entry-hil PRIVATE ${OBSW_INCLUDE_DIRS}) +target_compile_definitions(payload-entry-hil PRIVATE PAYLOAD_ENTRY HILSimulation EUROC USE_SERIAL_TRANSCEIVER) +sbs_target(payload-entry-hil stm32f429zi_skyward_death_stack_x) + add_executable(groundstation-entry src/entrypoints/Groundstation/groundstation-entry.cpp) sbs_target(groundstation-entry stm32f429zi_skyward_groundstation_v2) @@ -118,7 +130,12 @@ add_executable(main-test-cutter src/tests/Main/actuators/test-cutter.cpp) sbs_target(main-test-cutter stm32f429zi_skyward_death_stack_v3) add_executable(main-test-servos src/tests/Main/actuators/test-servos.cpp) -sbs_target(main-test-servos stm32f429zi_skyward_death_stack_v3) +sbs_target(main-test-servos stm32f429zi_skyward_death_stack_x_maker_faire) + +add_executable(test-airbrakes-st src/tests/Main/actuators/test-airbrakes-st.cpp ${MAIN_COMPUTER} ${HIL}) +target_include_directories(test-airbrakes-st PRIVATE ${OBSW_INCLUDE_DIRS}) +target_compile_definitions(test-airbrakes-st PRIVATE HILSimulation) +sbs_target(test-airbrakes-st stm32f429zi_skyward_death_stack_x_maker_faire) add_executable(main-test-bmx160-calibration src/tests/Main/calibration/test-bmx160-calibration.cpp) sbs_target(main-test-bmx160-calibration stm32f429zi_skyward_death_stack_v3) diff --git a/skyward-boardcore b/skyward-boardcore index 0030a2bb5e8e62d56d28bd5bdcb34a9e58ebd499..7638dc18607fc870a7550a0691b0edb47592a2c0 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit 0030a2bb5e8e62d56d28bd5bdcb34a9e58ebd499 +Subproject commit 7638dc18607fc870a7550a0691b0edb47592a2c0 diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main/Actuators/Actuators.cpp index 5d76d1adad3320f292e99fa4940133575264fa07..7871c6a4a6ebc731d43c25168b62f960058dd4e6 100644 --- a/src/boards/Main/Actuators/Actuators.cpp +++ b/src/boards/Main/Actuators/Actuators.cpp @@ -121,10 +121,10 @@ bool Actuators::disableServo(ServosList servoId) switch (servoId) { case AIR_BRAKES_SERVO: - servoAirbrakes.enable(); + servoAirbrakes.disable(); break; case EXPULSION_SERVO: - servoAirbrakes.enable(); + servoAirbrakes.disable(); break; default: return false; diff --git a/src/boards/Main/Buses.h b/src/boards/Main/Buses.h index d586ffac3517f015a9be6da4f7d0e1f20566c065..4d845b69180f07a347813d71b061d82cfa4d50e1 100644 --- a/src/boards/Main/Buses.h +++ b/src/boards/Main/Buses.h @@ -67,8 +67,9 @@ private: uart4(UART4, Boardcore::USARTInterface::Baudrate::B115200), spi1({}), spi2({}), spi4({}), spi5({}), spi6({}) { - // usart2.init(); + usart2.init(); usart3.init(); + uart4.init(); } #endif }; diff --git a/src/boards/Main/Configs/ActuatorsConfigs.h b/src/boards/Main/Configs/ActuatorsConfigs.h index 2a8bd2e7b6ca9ba3dabb870de48c6864ed55a464..dc6008213fabab0c58e7716de02d8163a4130381 100644 --- a/src/boards/Main/Configs/ActuatorsConfigs.h +++ b/src/boards/Main/Configs/ActuatorsConfigs.h @@ -32,6 +32,20 @@ namespace ActuatorsConfigs { // Airbrakes servo +#ifdef DEATHSTACK_V2 +// calibrated at +// constexpr float ABK_SERVO_MIN_PULSE = 1080; // [us] +// constexpr float ABK_SERVO_MAX_PULSE = 1600; // [us] + +static TIM_TypeDef* const ABK_SERVO_TIMER = TIM8; +constexpr Boardcore::TimerUtils::Channel ABK_SERVO_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_2; + +constexpr float ABK_SERVO_ROTATION = 53; // [deg] AirBrakes without end stop +constexpr float ABK_SERVO_MIN_PULSE = 1080; // [us] +constexpr float ABK_SERVO_MAX_PULSE = 1500; // [us] +#else +// TODO: Fix rotation value static TIM_TypeDef* const ABK_SERVO_TIMER = TIM10; constexpr Boardcore::TimerUtils::Channel ABK_SERVO_PWM_CH = Boardcore::TimerUtils::Channel::CHANNEL_1; @@ -40,7 +54,9 @@ constexpr float ABK_SERVO_ROTATION = 66.4; // [deg] AirBrakes without end stop constexpr float ABK_SERVO_MIN_PULSE = 1405; // [deg] constexpr float ABK_SERVO_MAX_PULSE = ABK_SERVO_MIN_PULSE - 10 * ABK_SERVO_ROTATION; // [us] -constexpr float ABK_WIGGLE_TIME = 2 * 1000; // [ms] +#endif + +constexpr float ABK_WIGGLE_TIME = 2 * 1000; // [ms] // Deployment servo static TIM_TypeDef* const DPL_SERVO_TIMER = TIM4; @@ -55,9 +71,16 @@ constexpr float DPL_SERVO_RESET_POS = DPL_SERVO_ROTATION; // [deg] constexpr float DPL_WIGGLE_TIME = 5 * 1000; // [ms] // Buzzer + +#ifdef DEATHSTACK_V2 +TIM_TypeDef* const BUZZER_TIMER = TIM1; +constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL = + Boardcore::TimerUtils::Channel::CHANNEL_1; +#else TIM_TypeDef* const BUZZER_TIMER = TIM8; constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL = Boardcore::TimerUtils::Channel::CHANNEL_1; +#endif #ifdef BUZZER_LOW constexpr float BUZZER_DUTY_CYCLE = 0.01; diff --git a/src/boards/Main/Configs/AirBrakesControllerConfig.h b/src/boards/Main/Configs/AirBrakesControllerConfig.h index 4480be7084de1e32db6ead281cb3e467a58e1d6b..29b0d580d7cc467afac80376ea171331ae8bbffe 100644 --- a/src/boards/Main/Configs/AirBrakesControllerConfig.h +++ b/src/boards/Main/Configs/AirBrakesControllerConfig.h @@ -74,9 +74,6 @@ static const Boardcore::AirBrakesConfig ABK_CONFIG{ .S0 = 0.017671458676443, .SURFACE = 0.009564 * Main::ActuatorsConfigs::ABK_SERVO_ROTATION * EIGEN_PI / 180.0, - .KP = 20, - .KI = 5, - .TS = UPDATE_PERIOD / 1000.0, }; } // namespace AirBrakesControllerConfig diff --git a/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h new file mode 100644 index 0000000000000000000000000000000000000000..ae0d3aa5234c950f3ac56ff583f161928311ca55 --- /dev/null +++ b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h @@ -0,0 +1,41 @@ +/* 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 <Main/Configs/AirBrakesControllerConfig.h> +#include <algorithms/AirBrakes/AirBrakesInterpConfig.h> + +namespace Main +{ + +namespace AirBrakesControllerConfig +{ + +static const Boardcore::AirBrakesInterpConfig ABK_CONFIG_INTERP{ + .INITIAL_FILTER_COEFF = 0.3, + .INITIAL_T_FILTER = 12, + .DELTA_T_FILTER = 2.5, + .FILTER_RATIO = 2}; +} // namespace AirBrakesControllerConfig + +} // namespace Main diff --git a/src/boards/Main/Configs/AirBrakesControllerConfigPI.h b/src/boards/Main/Configs/AirBrakesControllerConfigPI.h new file mode 100644 index 0000000000000000000000000000000000000000..e2696756f531b63b310d053a598bee81640d6540 --- /dev/null +++ b/src/boards/Main/Configs/AirBrakesControllerConfigPI.h @@ -0,0 +1,39 @@ +/* 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 <Main/Configs/AirBrakesControllerConfig.h> +#include <algorithms/AirBrakes/AirBrakesPIConfig.h> + +namespace Main +{ + +namespace AirBrakesControllerConfig +{ + +static const Boardcore::AirBrakesPIConfig ABK_CONFIG_PI{ + .KP = 20, .KI = 5, .TS = UPDATE_PERIOD / 1000.0}; + +} // namespace AirBrakesControllerConfig + +} // namespace Main diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h index 43c7d5d802e022a8fb9e4e858f1999d8339d0838..3b5cf64b9518fe37d833d9d003feb607345036b1 100644 --- a/src/boards/Main/Configs/SensorsConfig.h +++ b/src/boards/Main/Configs/SensorsConfig.h @@ -24,7 +24,6 @@ #include <drivers/adc/InternalADC.h> #include <drivers/usart/USART.h> -#include <interfaces-impl/hwmapping.h> #include <sensors/ADS131M04/ADS131M04.h> #include <sensors/BMX160/BMX160Config.h> #include <sensors/LIS3MDL/LIS3MDL.h> diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp index fff8510746d0d0d28ccdda6c13df8b9f8ceb3a38..d04033c35fcb56e3bcddd6082b312629074101fe 100644 --- a/src/boards/Main/Radio/Radio.cpp +++ b/src/boards/Main/Radio/Radio.cpp @@ -34,6 +34,7 @@ #include <Main/TMRepository/TMRepository.h> #include <common/events/Events.h> #include <drivers/interrupt/external_interrupts.h> +#include <interfaces-impl/hwmapping.h> #include <functional> @@ -94,7 +95,7 @@ Radio::Radio() { #if defined(USE_SERIAL_TRANSCEIVER) Boardcore::SerialTransceiver* transceiver; - transceiver = new SerialTransceiver(Buses::getInstance().usart1); + transceiver = new SerialTransceiver(Buses::getInstance().usart2); #elif defined(USE_XBEE_TRANSCEIVER) SPIBusConfig config; config.clockDivider = SPI::ClockDivider::DIV_16; diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp index 0bcbb26135a5549f8d3995854165ce27b6ad2532..21a98a1cd79b7f5b44e717f142913daf3f50a255 100644 --- a/src/boards/Main/Sensors/Sensors.cpp +++ b/src/boards/Main/Sensors/Sensors.cpp @@ -28,6 +28,7 @@ #include <common/events/Events.h> #include <drivers/interrupt/external_interrupts.h> #include <events/EventBroker.h> +#include <interfaces-impl/hwmapping.h> using namespace std; using namespace miosix; diff --git a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp index ff64e2e484392a5855be473053c3eca797a9e13e..8996ae207a1e9b2a3f0a8f1aeb94d6cf6e78bfea 100644 --- a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp +++ b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp @@ -25,8 +25,9 @@ #include <Main/Actuators/Actuators.h> #include <Main/BoardScheduler.h> #include <Main/Configs/ActuatorsConfigs.h> -#include <Main/Configs/AirBrakesControllerConfig.h> #include <Main/StateMachines/NASController/NASController.h> +#include <algorithms/AirBrakes/AirBrakesInterp.h> +#include <algorithms/AirBrakes/AirBrakesPI.h> #include <common/events/Events.h> #include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> @@ -36,8 +37,10 @@ #endif #ifdef INTERP +#include "Main/Configs/AirBrakesControllerConfigInterp.h" #include "TrajectorySetInterp.h" #else +#include "Main/Configs/AirBrakesControllerConfigPI.h" #include "TrajectorySet.h" #endif @@ -125,6 +128,10 @@ void AirBrakesController::state_idle(const Event& event) } case FLIGHT_LIFTOFF: { +#ifdef INTERP + abk.setLiftoffTimestamp(); +#endif + return transition(&AirBrakesController::state_shadow_mode); } } @@ -190,6 +197,7 @@ void AirBrakesController::state_end(const Event& event) AirBrakesController::AirBrakesController() : FSM(&AirBrakesController::state_init), abk( +#ifndef INTERP #ifndef HILMockNAS []() { return TimedTrajectoryPoint{ @@ -199,10 +207,28 @@ AirBrakesController::AirBrakesController() []() { return Sensors::getInstance().state.kalman->getLastSample(); }, #endif // HILMockNAS TRAJECTORY_SET, AirBrakesControllerConfig::ABK_CONFIG, + AirBrakesControllerConfig::ABK_CONFIG_PI, [](float position) { Actuators::getInstance().setServo(ServosList::AIR_BRAKES_SERVO, position); }) +#else // INTERP +#ifndef HILMockNAS + []() { + return TimedTrajectoryPoint{ + NASController::getInstance().getNasState()}; + }, +#else // HILMockNAS + []() { return Sensors::getInstance().state.kalman->getLastSample(); }, +#endif // HILMockNAS + TRAJECTORY_SET, AirBrakesControllerConfig::ABK_CONFIG, + AirBrakesControllerConfig::ABK_CONFIG_INTERP, + [](float position) { + Actuators::getInstance().setServo(ServosList::AIR_BRAKES_SERVO, + position); + }, + Main::dz) +#endif // INTERP { EventBroker::getInstance().subscribe(this, TOPIC_ABK); EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); @@ -225,10 +251,7 @@ void AirBrakesController::wiggleServo() { for (int i = 0; i < 2; i++) { - Actuators::getInstance().setServoAngle(AIR_BRAKES_SERVO, - ABK_SERVO_ROTATION); - miosix::Thread::sleep(500); - Actuators::getInstance().setServoAngle(AIR_BRAKES_SERVO, 0); + Actuators::getInstance().wiggleServo(AIR_BRAKES_SERVO); miosix::Thread::sleep(500); } } diff --git a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h index 0ad5f9a05cf8f02aba88bf932bf3a20c041c4841..cfb67a1862469fa7bb8842e2934ff1f1f85fd573 100644 --- a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h +++ b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h @@ -23,12 +23,17 @@ #pragma once #include <Singleton.h> -#include <algorithms/AirBrakes/AirBrakes.h> #include <diagnostic/PrintLogger.h> #include <events/FSM.h> #include "AirBrakesControllerData.h" +#ifndef INTERP +#include <algorithms/AirBrakes/AirBrakesPI.h> +#else +#include <algorithms/AirBrakes/AirBrakesInterp.h> +#endif + namespace Main { @@ -60,8 +65,11 @@ private: void wiggleServo(); - Boardcore::AirBrakes abk; - +#ifndef INTERP + Boardcore::AirBrakesPI abk; +#else + Boardcore::AirBrakesInterp abk; +#endif Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("main.abk"); }; diff --git a/src/boards/Payload/Buses.h b/src/boards/Payload/Buses.h index 4e1009b3d7333028f2696a3983b34fcac2fa4f7f..130f4a6ec464be99bd673ba095787d1e1533ef77 100644 --- a/src/boards/Payload/Buses.h +++ b/src/boards/Payload/Buses.h @@ -64,6 +64,8 @@ private: uart4(UART4, Boardcore::USARTInterface::Baudrate::B115200), spi1({}), spi2({}) { + usart2.init(); + usart3.init(); } #endif }; diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp index 7ab7a6a12ee4fd04af110869680695ae3b6ba605..4967a79c0e0e4e5661a9a716063fc389807dec51 100644 --- a/src/boards/Payload/Radio/Radio.cpp +++ b/src/boards/Payload/Radio/Radio.cpp @@ -48,11 +48,13 @@ using namespace Payload::RadioConfig; using namespace Common; // XBee interrupt +#ifndef USE_SERIAL_TRANSCEIVER void __attribute__((used)) EXTI10_IRQHandlerImpl() { - if (Payload::Radio::getInstance().xbee != nullptr) - Payload::Radio::getInstance().xbee->handleATTNInterrupt(); + if (Payload::Radio::getInstance().transceiver != nullptr) + Payload::Radio::getInstance().transceiver->handleATTNInterrupt(); } +#endif namespace Payload { @@ -87,26 +89,31 @@ void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); } Radio::Radio() { +#if defined(USE_SERIAL_TRANSCEIVER) + Boardcore::SerialTransceiver* transceiver; + transceiver = new SerialTransceiver(Buses::getInstance().usart2); +#else // Create the SPI bus configuration SPIBusConfig config{}; config.clockDivider = SPI::ClockDivider::DIV_16; // Create the xbee object - xbee = new Xbee::Xbee( + transceiver = new Xbee::Xbee( Buses::getInstance().spi2, config, miosix::xbee::cs::getPin(), miosix::xbee::attn::getPin(), miosix::xbee::reset::getPin()); - xbee->setOnFrameReceivedListener( + transceiver->setOnFrameReceivedListener( bind(&Radio::onXbeeFrameReceived, this, _1)); - Xbee::setDataRate(*xbee, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT); - - mavDriver = - new MavDriver(xbee, bind(&Radio::handleMavlinkMessage, this, _1, _2), 0, - MAV_OUT_BUFFER_MAX_AGE); + Xbee::setDataRate(*transceiver, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT); enableExternalInterrupt(miosix::xbee::attn::getPin().getPort(), miosix::xbee::attn::getPin().getNumber(), InterruptTrigger::FALLING_EDGE); +#endif + + mavDriver = new MavDriver(transceiver, + bind(&Radio::handleMavlinkMessage, this, _1, _2), + 0, MAV_OUT_BUFFER_MAX_AGE); // Add to the scheduler the periodic telemetries BoardScheduler::getInstance().getScheduler().addTask( diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h index f33e32eefada109e854eee62d90a8878ebbea07b..dbac47db8f12022960faabf8839622a41956605b 100644 --- a/src/boards/Payload/Radio/Radio.h +++ b/src/boards/Payload/Radio/Radio.h @@ -28,6 +28,14 @@ #include <radio/Xbee/Xbee.h> #include <scheduler/TaskScheduler.h> +#if defined(USE_SERIAL_TRANSCEIVER) +#include <radio/SerialTransceiver/SerialTransceiver.h> +#elif defined(USE_XBEE_TRANSCEIVER) +#include <radio/Xbee/ATCommands.h> +#else +#include <radio/SX1278/SX1278.h> +#endif + namespace Payload { @@ -40,7 +48,12 @@ class Radio : public Boardcore::Singleton<Radio> friend class Boardcore::Singleton<Radio>; public: - Boardcore::Xbee::Xbee* xbee; +#if defined(USE_SERIAL_TRANSCEIVER) + Boardcore::SerialTransceiver* transceiver; +#else + Boardcore::Xbee::Xbee* transceiver; +#endif + MavDriver* mavDriver; /** diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp index b98e956ea87cfdccd6a1fd599a7ed4f82583b8bb..fc476a5892740fe3c2e4ae61a2e11a94b7ff3d44 100644 --- a/src/boards/Payload/Sensors/Sensors.cpp +++ b/src/boards/Payload/Sensors/Sensors.cpp @@ -69,27 +69,81 @@ BMX160Data Sensors::getBMX160LastSample() BMX160WithCorrectionData Sensors::getBMX160WithCorrectionLastSample() { miosix::PauseKernelLock lock; +#ifndef HILSimulation return bmx160WithCorrection != nullptr ? bmx160WithCorrection->getLastSample() : BMX160WithCorrectionData{}; +#else + auto imuData = state.imu->getLastSample(); + BMX160WithCorrectionData data; + + data.accelerationTimestamp = imuData.accelerationTimestamp; + data.accelerationX = imuData.accelerationX; + data.accelerationY = imuData.accelerationY; + data.accelerationZ = imuData.accelerationZ; + data.angularVelocityTimestamp = imuData.angularVelocityTimestamp; + data.angularVelocityX = imuData.angularVelocityX; + data.angularVelocityY = imuData.angularVelocityY; + data.angularVelocityZ = imuData.angularVelocityZ; + data.magneticFieldTimestamp = imuData.magneticFieldTimestamp; + data.magneticFieldX = imuData.magneticFieldX; + data.magneticFieldY = imuData.magneticFieldY; + data.magneticFieldZ = imuData.magneticFieldZ; + + return data; +#endif } LIS3MDLData Sensors::getMagnetometerLIS3MDLLastSample() { miosix::PauseKernelLock lock; +#ifndef HILSimulation return lis3mdl != nullptr ? lis3mdl->getLastSample() : LIS3MDLData{}; +#else + return LIS3MDLData(state.magnetometer->getLastSample(), + state.temperature->getLastSample()); +#endif } MS5803Data Sensors::getMS5803LastSample() { miosix::PauseKernelLock lock; + +#ifndef HILSimulation return ms5803 != nullptr ? ms5803->getLastSample() : MS5803Data{}; +#else + auto baroData = state.barometer->getLastSample(); + auto tempData = state.temperature->getLastSample(); + + return MS5803Data(baroData.pressureTimestamp, baroData.pressure, + tempData.temperatureTimestamp, tempData.temperature); +#endif } UBXGPSData Sensors::getUbxGpsLastSample() { miosix::PauseKernelLock lock; +#ifndef HILSimulation return ubxGps != nullptr ? ubxGps->getLastSample() : UBXGPSData{}; +#else + auto data = state.gps->getLastSample(); + UBXGPSData ubxData; + + ubxData.gpsTimestamp = data.gpsTimestamp; + ubxData.latitude = data.latitude; + ubxData.longitude = data.longitude; + ubxData.height = data.height; + ubxData.velocityNorth = data.velocityNorth; + ubxData.velocityEast = data.velocityEast; + ubxData.velocityDown = data.velocityDown; + ubxData.speed = data.speed; + ubxData.track = data.track; + ubxData.positionDOP = data.positionDOP; + ubxData.satellites = data.satellites; + ubxData.fix = data.fix; + + return ubxData; +#endif } ADS1118Data Sensors::getADS1118LastSample() @@ -115,14 +169,26 @@ SSCDANN030PAAData Sensors::getDplPressureLastSample() SSCDRRN015PDAData Sensors::getPitotPressureLastSample() { miosix::PauseKernelLock lock; +#ifndef HILSimulation return pitotPressure != nullptr ? pitotPressure->getLastSample() : SSCDRRN015PDAData{}; +#else + auto pitotData = state.pitot->getLastSample(); + SSCDRRN015PDAData data; + data.pressureTimestamp = pitotData.timestamp; + data.pressure = pitotData.deltaP; + return data; +#endif } PitotData Sensors::getPitotLastSample() { miosix::PauseKernelLock lock; +#ifndef HILSimulation return pitot != nullptr ? pitot->getLastSample() : PitotData{}; +#else + return state.pitot->getLastSample(); +#endif } InternalADCData Sensors::getInternalADCLastSample() @@ -216,6 +282,29 @@ Sensors::Sensors() bmx160Init(); bmx160WithCorrectionInit(); +#ifdef HILSimulation + // Definition of the fake sensors for the simulation + state.accelerometer = new HILAccelerometer(N_DATA_ACCEL); + state.barometer = new HILBarometer(N_DATA_BARO); + state.pitot = new HILPitot(N_DATA_PITOT); + state.gps = new HILGps(N_DATA_GPS); + state.gyro = new HILGyroscope(N_DATA_GYRO); + state.magnetometer = new HILMagnetometer(N_DATA_MAGN); + state.imu = new HILImu(N_DATA_IMU); + state.temperature = new HILTemperature(N_DATA_TEMP); + state.kalman = new HILKalman(N_DATA_KALM); + + sensorsMap = {{state.accelerometer, accelConfig}, + {state.barometer, baroConfig}, + {state.pitot, pitotConfig}, + {state.magnetometer, magnConfig}, + {state.imu, imuConfig}, + {state.gps, gpsConfig}, + {state.gyro, gyroConfig}, + {state.temperature, tempConfig}, + {state.kalman, kalmConfig}}; +#endif + // Create the sensor manager sensorManager = new SensorManager(sensorsMap); } @@ -230,6 +319,21 @@ Sensors::~Sensors() delete staticPressure; delete dplPressure; delete pitotPressure; + +#ifdef HILSimulation + delete state.accelerometer; + delete state.barometer; + delete state.pitot; + delete state.gps; + delete state.gyro; + delete state.magnetometer; + delete state.imu; + delete state.temperature; + delete state.kalman; +#endif + + sensorManager->stop(); + delete sensorManager; } void Sensors::bmx160Init() diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h index 07c1c8e12dde695a8af8801cf85b24058eeac0d3..ce0f67f9f3e0ec1768fa41b2a78d27ebbb9fd6a7 100644 --- a/src/boards/Payload/Sensors/Sensors.h +++ b/src/boards/Payload/Sensors/Sensors.h @@ -37,6 +37,10 @@ #include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> #include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h> #include <sensors/analog/pressure/nxp/MPXHZ6130A.h> +#ifdef HILSimulation +#include <HIL_algorithms/HILMockKalman.h> +#include <HIL_sensors/HILSensors.h> +#endif // HILSimulation namespace Payload { @@ -50,6 +54,25 @@ public: bool isStarted(); +#ifdef HILSimulation +public: + /** + * structure that contains all the sensors used in the simulation + */ + struct StateComplete + { + HILAccelerometer* accelerometer; + HILBarometer* barometer; + HILPitot* pitot; + HILGps* gps; + HILGyroscope* gyro; + HILMagnetometer* magnetometer; + HILTemperature* temperature; + HILImu* imu; + HILKalman* kalman; + } state; +#endif // HILSimulation + Boardcore::BMX160* bmx160; Boardcore::BMX160Data getBMX160LastSample(); diff --git a/src/entrypoints/Main/main-entry-maker-faire.cpp b/src/entrypoints/Main/main-entry-maker-faire.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6c594a7789a25c766e623fcf730e043ccd0e2bc7 --- /dev/null +++ b/src/entrypoints/Main/main-entry-maker-faire.cpp @@ -0,0 +1,215 @@ +/* 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. + */ + +#include <Main/Actuators/Actuators.h> +#include <Main/BoardScheduler.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/ADAController/ADAController.h> +#include <Main/StateMachines/AirBrakesController/AirBrakesController.h> +#include <Main/StateMachines/Deployment/Deployment.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <common/events/Events.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <events/EventBroker.h> +#include <events/EventData.h> +#include <events/utils/EventSniffer.h> +#include <miosix.h> +#include <utils/PinObserver/PinObserver.h> + +#include "kernel/scheduler/priority/priority_scheduler.h" + +#ifdef HILSimulation +#include <HIL.h> +#include <HIL_algorithms/HILMockAerobrakeAlgorithm.h> +#include <HIL_algorithms/HILMockKalman.h> +#include <HIL_sensors/HILSensors.h> +#endif + +using namespace miosix; +using namespace Boardcore; +using namespace Main; +using namespace Common; + +int main() +{ + bool initResult = true; + PrintLogger logger = Logging::getLogger("main"); + +#ifdef HILSimulation + auto flightPhasesManager = HIL::getInstance().flightPhasesManager; + + flightPhasesManager->setCurrentPositionSource( + []() + { + return TimedTrajectoryPoint{ + Main::NASController::getInstance().getNasState()}; + }); + + HIL::getInstance().start(); + + BoardScheduler::getInstance().getScheduler().addTask( + []() { Actuators::getInstance().sendToSimulator(); }, 100); +#endif + + if (!Logger::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting SD logger"); + } + + if (!EventBroker::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the EventBroker"); + } + + // Start the radio + if (!Radio::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the radio"); + } + + // Start the can interface + // if (!CanHandler::getInstance().start()) + // { + // initResult = false; + // LOG_ERR(logger, "Error starting the CAN interface"); + // } + + // Start the state machines +#ifdef HILSimulation + ADAController::getInstance().setUpdateDataFunction( + [](Boardcore::ADAState state) + { HIL::getInstance().getElaboratedData()->addADAState(state); }); +#endif + if (!ADAController::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the ADAController"); + } + if (!AirBrakesController::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the AirBrakesController"); + } + if (!Deployment::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Deployment"); + } + if (!FlightModeManager::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the FlightModeManager"); + } +#ifdef HILSimulation + NASController::getInstance().setUpdateDataFunction( + [](Boardcore::NASState state) + { HIL::getInstance().getElaboratedData()->addNASState(state); }); +#endif + if (!NASController::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the NASController"); + } + + // Start the sensors sampling + if (!Sensors::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the sensors"); + } + + // Start the pin handler and observer + if (!PinObserver::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the PinObserver"); + } + + // Start the board task scheduler + if (!BoardScheduler::getInstance().getScheduler().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the BoardScheduler"); + } + + // If all is correctly set up i publish the init ok + if (initResult) + EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM); + else + EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM); + + // Log all events + EventSniffer sniffer( + EventBroker::getInstance(), TOPICS_LIST, + [](uint8_t event, uint8_t topic) + { + EventData ev{TimestampTimer::getTimestamp(), event, topic}; + Logger::getInstance().log(ev); + }); + + // Set the clock divider for the analog circuitry (/8) + ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1; + InternalADC internalADC = InternalADC(ADC3, 3.3); + internalADC.enableChannel(InternalADC::CH5); + internalADC.init(); + + std::function<ADCData()> get_batVoltage_function = + std::bind(&InternalADC::getVoltage, &internalADC, InternalADC::CH5); + + BatteryVoltageSensor batterySensor(get_batVoltage_function, 5.98); + + bool servoEnabled = true; + + // Periodical statistics + while (true) + { + // checking for battery charge. if too low for the actuator (< 10.5 V), + // disable the real actuation of the servo + internalADC.sample(); + batterySensor.sample(); + float vbat = batterySensor.getLastSample().batVoltage - + 0.4; // subtracting 0.4 as offset + + printf("vbat: %f\n", vbat); + + if (servoEnabled && vbat < 10.5) + { + printf("*** AIRBRAKES SERVO DISABLED ***"); + servoEnabled = false; + Actuators::getInstance().disableServo(AIR_BRAKES_SERVO); + } + + Thread::sleep(1000); + Logger::getInstance().log(CpuMeter::getCpuStats()); + CpuMeter::resetCpuStats(); + Logger::getInstance().logStats(); + Radio::getInstance().logStatus(); + } +} diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp index cac0120858542a519887e34f3a43b46d26a15951..44b2f9dab57f4b9e6b3834e3d10527ccab4fd959 100644 --- a/src/entrypoints/Payload/payload-entry.cpp +++ b/src/entrypoints/Payload/payload-entry.cpp @@ -42,6 +42,13 @@ #include <events/utils/EventSniffer.h> #include <miosix.h> +#ifdef HILSimulation +#include <HIL.h> +#include <HIL_algorithms/HILMockAerobrakeAlgorithm.h> +#include <HIL_algorithms/HILMockKalman.h> +#include <HIL_sensors/HILSensors.h> +#endif + using namespace miosix; using namespace Boardcore; using namespace Payload; @@ -52,6 +59,23 @@ int main() bool initResult = true; PrintLogger logger = Logging::getLogger("main"); +#ifdef HILSimulation + auto flightPhasesManager = HIL::getInstance().flightPhasesManager; + + flightPhasesManager->setCurrentPositionSource( + []() { + return TimedTrajectoryPoint{ + NASController::getInstance().getNasState()}; + }); + + HIL::getInstance().start(); + + BoardScheduler::getInstance().getScheduler().addTask( + []() { HIL::getInstance().send(0.0f); }, 100); + + // flightPhasesManager->registerToFlightPhase(FlightPhases::FLYING, ) +#endif + if (!Logger::getInstance().start()) { initResult = false; diff --git a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp index dacffb383f4ff9f6819519a354e96b11aa728b1a..8bcadd31aa1090b9f343d9bea4085761d19089bc 100644 --- a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp +++ b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp @@ -33,21 +33,23 @@ using namespace Common; HILFlightPhasesManager::HILFlightPhasesManager() : Boardcore::EventHandler(), - flagsFlightPhases({{FlightPhases::SIMULATION_STARTED, false}, + flagsFlightPhases({{FlightPhases::SIM_FLYING, false}, + {FlightPhases::SIM_ASCENT, false}, + {FlightPhases::SIM_BURNING, false}, + {FlightPhases::SIM_AEROBRAKES, false}, + {FlightPhases::SIM_PARA1, false}, + {FlightPhases::SIM_PARA2, false}, + {FlightPhases::SIMULATION_STARTED, false}, {FlightPhases::CALIBRATION, false}, {FlightPhases::CALIBRATION_OK, false}, {FlightPhases::ARMED, false}, {FlightPhases::LIFTOFF_PIN_DETACHED, false}, - {FlightPhases::FLYING, false}, - {FlightPhases::ASCENT, false}, - {FlightPhases::BURNING, false}, + {FlightPhases::LIFTOFF, false}, {FlightPhases::AEROBRAKES, false}, - {FlightPhases::SIM_AEROBRAKES, false}, {FlightPhases::APOGEE, false}, {FlightPhases::PARA1, false}, {FlightPhases::PARA2, false}, - {FlightPhases::SIMULATION_STOPPED, false}}), - last_event(0) + {FlightPhases::SIMULATION_STOPPED, false}}) { prev_flagsFlightPhases = flagsFlightPhases; @@ -84,7 +86,7 @@ void HILFlightPhasesManager::setFlagFlightPhase(FlightPhases flag, void HILFlightPhasesManager::processFlags(FlightPhasesFlags hil_flags) { - updateFlags(hil_flags); + updateSimulatorFlags(hil_flags); std::vector<FlightPhases> changed_flags; @@ -97,35 +99,36 @@ void HILFlightPhasesManager::processFlags(FlightPhasesFlags hil_flags) changed_flags.push_back(FlightPhases::SIMULATION_STARTED); } - if (flagsFlightPhases[FlightPhases::FLYING]) + if (flagsFlightPhases[FlightPhases::SIM_FLYING]) { - if (isSetTrue(FlightPhases::FLYING)) + if (isSetTrue(FlightPhases::SIM_FLYING)) { TRACE("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n"); sEventBroker.post(FLIGHT_LAUNCH_PIN_DETACHED, TOPIC_FLIGHT); + changed_flags.push_back(FlightPhases::SIM_FLYING); } - if (isSetFalse(FlightPhases::BURNING)) + if (isSetFalse(FlightPhases::SIM_BURNING)) { - registerOutcomes(FlightPhases::BURNING); + registerOutcomes(FlightPhases::SIM_BURNING); TRACE("[HIL] ------- STOPPED BURNING ! ------- \n"); - changed_flags.push_back(FlightPhases::BURNING); + changed_flags.push_back(FlightPhases::SIM_BURNING); } if (isSetTrue(FlightPhases::SIM_AEROBRAKES)) { registerOutcomes(FlightPhases::SIM_AEROBRAKES); changed_flags.push_back(FlightPhases::SIM_AEROBRAKES); } - if (isSetTrue(FlightPhases::PARA1)) + if (isSetTrue(FlightPhases::SIM_PARA1)) { - registerOutcomes(FlightPhases::PARA1); + registerOutcomes(FlightPhases::SIM_PARA1); TRACE("[HIL] ------- PARACHUTE 1 ! ------- \n"); - changed_flags.push_back(FlightPhases::PARA1); + changed_flags.push_back(FlightPhases::SIM_PARA1); } - if (isSetTrue(FlightPhases::PARA2)) + if (isSetTrue(FlightPhases::SIM_PARA2)) { - registerOutcomes(FlightPhases::PARA2); + registerOutcomes(FlightPhases::SIM_PARA2); TRACE("[HIL] ------- PARACHUTE 2 ! ------- \n"); - changed_flags.push_back(FlightPhases::PARA2); + changed_flags.push_back(FlightPhases::SIM_PARA2); } } @@ -155,7 +158,7 @@ void HILFlightPhasesManager::printOutcomes() (double)(t_stop - t_start) / 1000000.0f); printf("Motor stopped burning (simulation flag): \n"); - outcomes[FlightPhases::BURNING].print(t_liftoff); + outcomes[FlightPhases::SIM_BURNING].print(t_liftoff); printf("Airbrakes exit shadowmode: \n"); outcomes[FlightPhases::AEROBRAKES].print(t_liftoff); @@ -183,20 +186,17 @@ void HILFlightPhasesManager::printOutcomes() * @brief Updates the flags of the object with the flags sent from matlab * and checks for the apogee */ -void HILFlightPhasesManager::updateFlags(FlightPhasesFlags hil_flags) +void HILFlightPhasesManager::updateSimulatorFlags(FlightPhasesFlags hil_flags) { - // TODO: Add ifdefs in order to select which flags to take from matlab - flagsFlightPhases[FlightPhases::ASCENT] = hil_flags.flag_ascent; - flagsFlightPhases[FlightPhases::FLYING] = hil_flags.flag_flight; - flagsFlightPhases[FlightPhases::BURNING] = hil_flags.flag_burning; - - // Flags PARA1, PARA2 and SIM_AEROBRAKES ignored from matlab - // flagsFlightPhases[SIM_AEROBRAKES] = hil_flags.flag_airbrakes; - // flagsFlightPhases[PARA1] = hil_flags.flag_para1; - // flagsFlightPhases[PARA2] = hil_flags.flag_para2; + flagsFlightPhases[FlightPhases::SIM_ASCENT] = hil_flags.flag_ascent; + flagsFlightPhases[FlightPhases::SIM_FLYING] = hil_flags.flag_flight; + flagsFlightPhases[FlightPhases::SIM_BURNING] = hil_flags.flag_burning; + flagsFlightPhases[FlightPhases::SIM_AEROBRAKES] = hil_flags.flag_airbrakes; + flagsFlightPhases[FlightPhases::SIM_PARA1] = hil_flags.flag_para1; + flagsFlightPhases[FlightPhases::SIM_PARA2] = hil_flags.flag_para2; flagsFlightPhases[FlightPhases::SIMULATION_STOPPED] = - isSetFalse(FlightPhases::FLYING) || + isSetFalse(FlightPhases::SIM_FLYING) || prev_flagsFlightPhases[FlightPhases::SIMULATION_STOPPED]; } @@ -233,13 +233,13 @@ void HILFlightPhasesManager::handleEvent(const Boardcore::Event& e) t_liftoff = Boardcore::TimestampTimer::getTimestamp(); TRACE("[HIL] ------- LIFTOFF -------: %f, %f \n", getCurrentPosition().z, getCurrentPosition().vz); - changed_flags.push_back(FlightPhases::FLYING); - changed_flags.push_back(FlightPhases::ASCENT); + changed_flags.push_back(FlightPhases::LIFTOFF); break; case ABK_SHADOW_MODE_TIMEOUT: setFlagFlightPhase(FlightPhases::AEROBRAKES, true); registerOutcomes(FlightPhases::AEROBRAKES); - TRACE("[HIL] ------- AEROBRAKES ENABLED ! ------- \n"); + TRACE("[HIL] ABK shadow mode timeout\n"); + // TRACE("[HIL] ------- AEROBRAKES ENABLED ! ------- \n"); changed_flags.push_back(FlightPhases::AEROBRAKES); break; case ADA_SHADOW_MODE_TIMEOUT: @@ -298,8 +298,6 @@ void HILFlightPhasesManager::handleEvent(const Boardcore::Event& e) } prev_flagsFlightPhases = flagsFlightPhases; - - last_event = e; } bool HILFlightPhasesManager::isSetTrue(FlightPhases phase) diff --git a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h index c18d26b002e9b6eb07916e6579efb747df5e4e3d..44c53b8e86d4415ac509f5a33259534f346d9524 100644 --- a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h +++ b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h @@ -41,16 +41,22 @@ class HILTransceiver; enum class FlightPhases { + // simulator flags + SIM_FLYING, + SIM_ASCENT, + SIM_BURNING, + SIM_AEROBRAKES, + SIM_PARA1, + SIM_PARA2, + + // flight flags SIMULATION_STARTED, CALIBRATION, CALIBRATION_OK, ARMED, LIFTOFF_PIN_DETACHED, - FLYING, - ASCENT, - BURNING, + LIFTOFF, AEROBRAKES, - SIM_AEROBRAKES, APOGEE, PARA1, PARA2, @@ -112,7 +118,7 @@ private: * @brief Updates the flags of the object with the flags sent from matlab * and checks for the apogee */ - void updateFlags(FlightPhasesFlags hil_flags); + void updateSimulatorFlags(FlightPhasesFlags hil_flags); bool isSetTrue(FlightPhases phase); @@ -126,6 +132,4 @@ private: std::map<FlightPhases, vector<TCallback>> callbacks; std::map<FlightPhases, Outcomes> outcomes; std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition; - - uint8_t last_event; }; diff --git a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp index c59c16dc9f94e575e445e994d910ed897c6c5d58..f5e23848123a92fadb8c29974db5fc6abe64eaa2 100644 --- a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp +++ b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp @@ -20,15 +20,21 @@ * THE SOFTWARE. */ -#include <Main/Buses.h> - #include "HIL.h" +#ifdef PAYLOAD_ENTRY +#include <Payload/Buses.h> +using namespace Payload; +#else +#include <Main/Buses.h> +using namespace Main; +#endif + /** * @brief Construct a serial connection attached to a control algorithm */ HILTransceiver::HILTransceiver() - : hilSerial(Main::Buses::getInstance().usart3), actuatorData(ActuatorData{}) + : hilSerial(Buses::getInstance().uart4), actuatorData(ActuatorData{}) { } diff --git a/src/hardware_in_the_loop/HILConfig.h b/src/hardware_in_the_loop/HILConfig.h index b3f70973efdcaa294950806e5c498e69a9325815..18834996c2fc9413487ad8d57a3e477e7e9b7e28 100644 --- a/src/hardware_in_the_loop/HILConfig.h +++ b/src/hardware_in_the_loop/HILConfig.h @@ -40,21 +40,8 @@ */ /* Hardware in the loop entrypoint */ -#if defined(HARDWARE_IN_THE_LOOP) -#include "entrypoints/hardware_in_the_loop/HILSimulationConfig.h" -/* serial simulation with sample manager */ -#elif defined(HILSimulation) +#if defined(HILSimulation) #include <HILSimulationConfig.h> -#elif defined(HIL_ADA) -#include "test-HIL+ADA/HILSimulationConfig.h" -#elif defined(HIL_AEROBRAKE) -#include "test-HIL+Airbrake/HILSimulationConfig.h" -#elif defined(HIL_ADA_AEROBRAKE) -#include "test-HIL+ADA+Airbrake/HILSimulationConfig.h" -#elif defined(HIL_ADA_AEROBRAKECONTROLLER) -#include "test-HIL+ADA+AirbrakeController/HILSimulationConfig.h" -#elif defined(HIL_ADA_AEROBRAKECONTROLLER_NAS) -#include "test-HIL+ADA+AirbrakeController+nas/HILSimulationConfig.h" /* #elif defined(HIL_<tuoFlag>) #include "<test-directory>/HILSimulationConfig.h" diff --git a/src/hardware_in_the_loop/HIL_actuators/HILServo.h b/src/hardware_in_the_loop/HIL_actuators/HILServo.h index 8ab6b707288d02a1911bd1c82ba7695bed8f821a..947bd8ae87e0f30d7b69306ebfc6087dfafd87bd 100644 --- a/src/hardware_in_the_loop/HIL_actuators/HILServo.h +++ b/src/hardware_in_the_loop/HIL_actuators/HILServo.h @@ -25,26 +25,17 @@ #include <actuators/Servo/Servo.h> #include "HIL.h" -#include "HILConfig.h" class HILServo : public Boardcore::Servo { public: explicit HILServo(TIM_TypeDef* const timer, Boardcore::TimerUtils::Channel pwmChannel, - unsigned int minPulse = 1000, - unsigned int maxPulse = 2000, unsigned int frequency = 50) + unsigned int minPulse, unsigned int maxPulse, + unsigned int frequency = 50) : Servo(timer, pwmChannel, minPulse, maxPulse, frequency) { } - void setPosition(float position, bool limited = true) - { - Servo::setPosition(position, limited); - - // Send the position to MatLab - HIL::getInstance().send(position); - } - void sendToSimulator() { HIL::getInstance().send(getPosition()); } }; diff --git a/src/tests/Main/actuators/test-airbrakes-st.cpp b/src/tests/Main/actuators/test-airbrakes-st.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4650121baec6bb9dd210c62d7f626de9fa5e271f --- /dev/null +++ b/src/tests/Main/actuators/test-airbrakes-st.cpp @@ -0,0 +1,86 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Authors: Luca Erbetta, 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. + */ + +#include <Main/Actuators/Actuators.h> +#include <Main/Sensors/Sensors.h> +#include <actuators/Servo/Servo.h> +#include <miosix.h> +#include <scheduler/TaskScheduler.h> +#include <utils/ClockUtils.h> + +#include <iostream> + +#include "HIL_actuators/HILServo.h" + +using namespace Boardcore; +using namespace miosix; + +HILServo airbrakesServo(TIM8, TimerUtils::Channel::CHANNEL_2, 1080, 1600); + +// Position to cycle through for the servo 1, 2 and 3 + +void moveServo() +{ + airbrakesServo.setPosition(1); + Thread::sleep(1000); + airbrakesServo.setPosition(0); +} + +int main() +{ + char c[10]; + + // Set the clock divider for the analog circuitry (/8) + ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1; + InternalADC internalADC = InternalADC(ADC3, 3.3); + internalADC.enableChannel(InternalADC::CH5); + internalADC.init(); + + std::function<ADCData()> get_batVoltage_function = + std::bind(&InternalADC::getVoltage, &internalADC, InternalADC::CH5); + + BatteryVoltageSensor batterySensor(get_batVoltage_function, 5.98); + + // Enable the timers + airbrakesServo.enable(); + airbrakesServo.setPosition(0); + + while (true) + { + // checking for battery charge. if too low for the actuator (< 10.5 V), + // disable the real actuation of the servo + internalADC.sample(); + batterySensor.sample(); + float vbat = batterySensor.getLastSample().batVoltage - + 0.4; // subtracting 0.4 as offset + + if (vbat < 10.5) + { + airbrakesServo.disable(); + printf("Airbrakes can't be attuated, vbat too low\n"); + } + + scanf("%c\n", c); + moveServo(); + printf("vbat: %f\n", vbat); + } +}