diff --git a/CMakeLists.txt b/CMakeLists.txt index a3d1c0d95c5290e7b130fb5a023132dda2eea5fe..53388554c62a16673075b89389685826820750d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,6 +129,14 @@ target_include_directories(parafoil-guided-jesolo PRIVATE ${OBSW_INCLUDE_DIRS}) target_compile_definitions(parafoil-guided-jesolo PRIVATE ALGORITHM_CLOSED_LOOP JESOLO) sbs_target(parafoil-guided-jesolo stm32f429zi_death_stack_v2) +add_executable(parafoil-guided-dynamic src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER}) +target_include_directories(parafoil-guided-dynamic PRIVATE ${OBSW_INCLUDE_DIRS}) +target_compile_definitions(parafoil-guided-dynamic PRIVATE ALGORITHM_CLOSED_LOOP DYNAMIC_TARGET + DYNAMIC_TARGET_LATITUDE_OFFSET=50 + DYNAMIC_TARGET_LONGITUDE_OFFSET=50 +) +sbs_target(parafoil-guided-dynamic stm32f429zi_death_stack_v2) + add_executable(parafoil-t-approach-jesolo src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER}) target_include_directories(parafoil-t-approach-jesolo PRIVATE ${OBSW_INCLUDE_DIRS}) target_compile_definitions(parafoil-t-approach-jesolo PRIVATE ALGORITHM_EARLY_MANEUVER JESOLO) @@ -138,3 +146,12 @@ add_executable(parafoil-t-approach-milano src/Parafoil/parafoil-entry.cpp ${PARA target_include_directories(parafoil-t-approach-milano PRIVATE ${OBSW_INCLUDE_DIRS}) target_compile_definitions(parafoil-t-approach-milano PRIVATE ALGORITHM_EARLY_MANEUVER MILANO) sbs_target(parafoil-t-approach-milano stm32f429zi_death_stack_v2) + +add_executable(parafoil-t-approach-dynamic src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER}) +target_include_directories(parafoil-t-approach-dynamic PRIVATE ${OBSW_INCLUDE_DIRS}) +target_compile_definitions(parafoil-t-approach-dynamic PRIVATE + ALGORITHM_EARLY_MANEUVER DYNAMIC_TARGET + DYNAMIC_TARGET_LATITUDE_OFFSET=50 + DYNAMIC_TARGET_LONGITUDE_OFFSET=50 +) +sbs_target(parafoil-t-approach-dynamic stm32f429zi_death_stack_v2) diff --git a/src/Parafoil/Configs/WingConfig.h b/src/Parafoil/Configs/WingConfig.h index 2216b26695f3f6366b811c7a8d4e3dfd6a7011ed..d8eeea8b96b814ed33385cf7976eba1e974c6ee4 100644 --- a/src/Parafoil/Configs/WingConfig.h +++ b/src/Parafoil/Configs/WingConfig.h @@ -62,7 +62,6 @@ enum class AlgorithmId : size_t namespace Default { - #if defined(JESOLO) constexpr auto TARGET_LAT = 45.565264f; constexpr auto TARGET_LON = 12.577050f; @@ -87,6 +86,36 @@ constexpr auto ALGORITHM = AlgorithmId::CLOSED_LOOP; } // namespace Default +/** + * @brief Dynamic target configuration. If enabled, the target is not fixed to + * the one specified in the Default config, but is dynamically set to a fixed + * offset relative to the position of launch (pin detach). + */ +namespace DynamicTarget +{ + +/* linter off */ using namespace Boardcore::Units::Length; + +#if defined(DYNAMIC_TARGET) +constexpr auto ENABLED = true; +#else +constexpr auto ENABLED = false; +#endif + +#if defined(DYNAMIC_TARGET_LATITUDE_OFFSET) +constexpr auto LATITUDE_OFFSET = Meter{DYNAMIC_TARGET_LATITUDE_OFFSET}; +#else +constexpr auto LATITUDE_OFFSET = 0_m; +#endif + +#if defined(DYNAMIC_TARGET_LONGITUDE_OFFSET) +constexpr auto LONGITUDE_OFFSET = Meter{DYNAMIC_TARGET_LONGITUDE_OFFSET}; +#else +constexpr auto LONGITUDE_OFFSET = 0_m; +#endif + +} // namespace DynamicTarget + namespace PI { constexpr auto SATURATION_MIN_LIMIT = -Boardcore::Constants::PI; @@ -103,6 +132,7 @@ constexpr auto CONFIDENCE = 15; // [samples] constexpr auto M1_ALTITUDE_THRESHOLD = 250_m; constexpr auto M2_ALTITUDE_THRESHOLD = 150_m; constexpr auto TARGET_ALTITUDE_THRESHOLD = 50_m; + } // namespace Guidance } // namespace Wing diff --git a/src/Parafoil/StateMachines/WingController/WingController.cpp b/src/Parafoil/StateMachines/WingController/WingController.cpp index e88ef98d93dcb56f00018f1971c6f637f5e0eb83..33543e55b5be1a24fad6ab57f6788db8c7a9796b 100644 --- a/src/Parafoil/StateMachines/WingController/WingController.cpp +++ b/src/Parafoil/StateMachines/WingController/WingController.cpp @@ -26,6 +26,7 @@ #include <Parafoil/Configs/WESConfig.h> #include <Parafoil/Configs/WingConfig.h> #include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h> +#include <Parafoil/Sensors/Sensors.h> #include <Parafoil/StateMachines/NASController/NASController.h> #include <Parafoil/WindEstimation/WindEstimation.h> #include <Parafoil/Wing/AutomaticWingAlgorithm.h> @@ -141,6 +142,11 @@ State WingController::FlyingDeployment(const Boardcore::Event& event) calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( DPL_SERVO_ACTUATION_DETECTED, TOPIC_DPL, 2000); + if (Config::Wing::DynamicTarget::ENABLED) + initDynamicTarget( + Config::Wing::DynamicTarget::LATITUDE_OFFSET, + Config::Wing::DynamicTarget::LONGITUDE_OFFSET); + return HANDLED; } case EV_EXIT: @@ -398,6 +404,27 @@ Eigen::Vector2f WingController::getActiveTarget() return emGuidance.getActiveTarget(); } +void WingController::initDynamicTarget(Meter latitudeOffset, + Meter longitudeOffset) +{ + auto gps = getModule<Sensors>()->getUBXGPSLastSample(); + + // Convert the offset from meters to degrees + auto earthRadius = 6371_km; + float metersPerDegreeLongitude = + Meter{earthRadius}.value() * Constants::DEGREES_TO_RADIANS * + cosf(gps.latitude * Constants::DEGREES_TO_RADIANS); + float metersPerDegreeLatitude = + Meter{earthRadius}.value() * Constants::DEGREES_TO_RADIANS; + + float newLatitude = + gps.latitude + latitudeOffset.value() / metersPerDegreeLatitude; + float newLongitude = + gps.longitude + longitudeOffset.value() / metersPerDegreeLongitude; + + setTargetCoordinates(newLatitude, newLongitude); +} + uint8_t WingController::getSelectedAlgorithm() { return static_cast<uint8_t>(selectedAlgorithm.load()); diff --git a/src/Parafoil/StateMachines/WingController/WingController.h b/src/Parafoil/StateMachines/WingController/WingController.h index 5850784f0443f90a2e6f6b943cfda892d3b767b4..0b277170f63cc6dfab0bd893039d3fa1e42cf05a 100644 --- a/src/Parafoil/StateMachines/WingController/WingController.h +++ b/src/Parafoil/StateMachines/WingController/WingController.h @@ -29,6 +29,7 @@ #include <Parafoil/Wing/WingAlgorithm.h> #include <diagnostic/PrintLogger.h> #include <events/HSM.h> +#include <units/Length.h> #include <utils/DependencyManager/DependencyManager.h> #include <Eigen/Core> @@ -64,6 +65,7 @@ class Actuators; class NASController; class FlightStatsRecorder; class WindEstimation; +class Sensors; /** * State machine that manages the wings of the Parafoil. @@ -82,7 +84,7 @@ class WingController : public Boardcore::HSM<WingController>, public Boardcore::InjectableWithDeps<BoardScheduler, Actuators, NASController, FlightStatsRecorder, - WindEstimation> + WindEstimation, Sensors> { public: /** @@ -166,6 +168,15 @@ public: return emGuidance.calculateTargetAngle(currentPositionNED, heading); } + /** + * @brief Calling this method will calculate and update the current target + * position based on a offset from the current position. + * @param latitudeOffset The latitude offset in meters + * @param longitudeOffset The longitude offset in meters + */ + void initDynamicTarget(Boardcore::Units::Length::Meter latitudeOffset, + Boardcore::Units::Length::Meter longitudeOffset); + private: /** * @brief Loads all algorithms.