From 9c33e3f3eff30c53d23ce4d7a0da3c91fc73bfa1 Mon Sep 17 00:00:00 2001
From: Davide Basso <davide.basso@skywarder.eu>
Date: Sat, 14 Dec 2024 23:00:02 +0100
Subject: [PATCH] [Parafoil] Add AltitudeTrigger
Co-Authored-By: BearToCode <davidebasso03@gmail.com>
---
.vscode/settings.json | 1 +
src/Parafoil/Actuators/Actuators.cpp | 14 ++-
src/Parafoil/Actuators/Actuators.h | 8 +-
.../AltitudeTrigger/AltitudeTrigger.cpp | 95 ++++++++++++++
.../AltitudeTrigger/AltitudeTrigger.h | 86 +++++++++++++
src/Parafoil/BoardScheduler.h | 1 +
src/Parafoil/Configs/WingConfig.h | 119 ++++++++++++++++++
src/Parafoil/parafoil-entry.cpp | 9 +-
src/Payload/AltitudeTrigger/AltitudeTrigger.h | 2 +-
9 files changed, 323 insertions(+), 12 deletions(-)
create mode 100644 src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
create mode 100644 src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
create mode 100644 src/Parafoil/Configs/WingConfig.h
diff --git a/.vscode/settings.json b/.vscode/settings.json
index c6fa1d36e..f43029172 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -152,6 +152,7 @@
"GPIOF",
"GPIOG",
"hwmap",
+ "JESOLO",
"KALM",
"kalman",
"Leds",
diff --git a/src/Parafoil/Actuators/Actuators.cpp b/src/Parafoil/Actuators/Actuators.cpp
index 7b25f46ec..02324701b 100644
--- a/src/Parafoil/Actuators/Actuators.cpp
+++ b/src/Parafoil/Actuators/Actuators.cpp
@@ -29,6 +29,7 @@
using namespace std::chrono;
using namespace miosix;
using namespace Boardcore;
+using namespace Boardcore::Units::Angle;
namespace config = Parafoil::Config::Actuators;
namespace Parafoil
@@ -40,13 +41,13 @@ Actuators::Actuators()
config::LeftServo::TIMER, config::LeftServo::PWM_CH,
config::LeftServo::MIN_PULSE.count(),
config::LeftServo::MAX_PULSE.count());
- leftServo.fullRangeAngle = config::LeftServo::ROTATION.value();
+ leftServo.fullRangeAngle = config::LeftServo::ROTATION;
rightServo.servo = std::make_unique<Servo>(
config::RightServo::TIMER, config::RightServo::PWM_CH,
config::RightServo::MIN_PULSE.count(),
config::RightServo::MAX_PULSE.count());
- rightServo.fullRangeAngle = config::RightServo::ROTATION.value();
+ rightServo.fullRangeAngle = config::RightServo::ROTATION;
}
bool Actuators::start()
@@ -80,7 +81,7 @@ bool Actuators::setServoPosition(ServosList servoId, float position)
return true;
}
-bool Actuators::setServoAngle(ServosList servoId, float angle)
+bool Actuators::setServoAngle(ServosList servoId, Degree angle)
{
auto actuator = getServoActuator(servoId);
if (!actuator)
@@ -88,7 +89,8 @@ bool Actuators::setServoAngle(ServosList servoId, float angle)
miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
- actuator->servo->setPosition(angle / actuator->fullRangeAngle);
+ actuator->servo->setPosition(
+ (angle / actuator->fullRangeAngle.value()).value());
return true;
}
@@ -104,11 +106,11 @@ float Actuators::getServoPosition(ServosList servoId)
return actuator->servo->getPosition();
}
-float Actuators::getServoAngle(ServosList servoId)
+Degree Actuators::getServoAngle(ServosList servoId)
{
auto actuator = getServoActuator(servoId);
if (!actuator)
- return -1.f;
+ return Degree(-1.f);
miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
diff --git a/src/Parafoil/Actuators/Actuators.h b/src/Parafoil/Actuators/Actuators.h
index 524a42ede..673b6bb81 100644
--- a/src/Parafoil/Actuators/Actuators.h
+++ b/src/Parafoil/Actuators/Actuators.h
@@ -24,6 +24,7 @@
#include <actuators/Servo/Servo.h>
#include <common/MavlinkOrion.h>
+#include <units/Angle.h>
#include <utils/DependencyManager/DependencyManager.h>
#include <utils/ModuleManager/ModuleManager.hpp>
@@ -41,7 +42,8 @@ public:
struct ServoActuator
{
std::unique_ptr<Boardcore::Servo> servo;
- float fullRangeAngle; ///< The full range of the servo [degrees]
+ Units::Angle::Degree fullRangeAngle =
+ Units::Angle::Degree(0); ///< The full range of the servo [degrees]
miosix::FastMutex mutex;
};
@@ -67,7 +69,7 @@ public:
* @param angle Angle to set [degree].
* @return True if the the angle was set.
*/
- bool setServoAngle(ServosList servoId, float angle);
+ bool setServoAngle(ServosList servoId, Units::Angle::Degree angle);
/**
* @brief Wiggles the servo for few seconds.
@@ -112,7 +114,7 @@ public:
* @return float current Servo angle in range [0-180], (-1) if the servoId
* is invalid.
*/
- float getServoAngle(ServosList servoId);
+ Units::Angle::Degree getServoAngle(ServosList servoId);
/**
* @brief Starts twirl (one servo is set to 0 and the other one is not).
diff --git a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
new file mode 100644
index 000000000..7806a1a6c
--- /dev/null
+++ b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
@@ -0,0 +1,95 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Niccolò Betto, Davide Basso
+ *
+ * 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 "AltitudeTrigger.h"
+
+#include <Parafoil/BoardScheduler.h>
+#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/StateMachines/NASController/NASController.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Common;
+namespace config = Parafoil::Config;
+
+namespace Parafoil
+{
+
+bool AltitudeTrigger::start()
+{
+ auto& scheduler = getModule<BoardScheduler>()->altitudeTrigger();
+
+ auto task = scheduler.addTask([this] { update(); },
+ config::AltitudeTrigger::UPDATE_RATE);
+
+ if (task == 0)
+ return false;
+
+ started = true;
+ return true;
+}
+
+bool AltitudeTrigger::isStarted() { return started; }
+
+void AltitudeTrigger::enable()
+{
+ if (running)
+ return;
+
+ confidence = 0;
+ running = true;
+}
+
+void AltitudeTrigger::disable() { running = false; }
+
+bool AltitudeTrigger::isEnabled() { return running; }
+
+void AltitudeTrigger::setDeploymentAltitude(float altitude)
+{
+ targetAltitude = altitude;
+}
+
+void AltitudeTrigger::update()
+{
+ if (!running)
+ return;
+
+ // NED frame, flip the D sign to get above-ground-level altitude
+ auto nasState = getModule<NASController>()->getNasState();
+ float altitude = -nasState.d;
+
+ if (altitude < targetAltitude)
+ confidence++;
+ else
+ confidence = 0;
+
+ if (confidence >= config::AltitudeTrigger::CONFIDENCE)
+ {
+ confidence = 0;
+ EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+ TOPIC_ALT);
+ running = false;
+ }
+}
+
+} // namespace Parafoil
diff --git a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
new file mode 100644
index 000000000..f5514b088
--- /dev/null
+++ b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
@@ -0,0 +1,86 @@
+/* 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 <Parafoil/Configs/WingConfig.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include <atomic>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Parafoil
+{
+class BoardScheduler;
+class NASController;
+
+class AltitudeTrigger
+ : public Boardcore::InjectableWithDeps<BoardScheduler, NASController>
+{
+public:
+ /**
+ * @brief Adds the update() task to the task scheduler.
+ */
+ bool start();
+
+ /**
+ * @return If the AltitudeTrigger is started.
+ */
+ bool isStarted();
+
+ /**
+ * @brief Enable the AltitudeTrigger.
+ */
+ void enable();
+
+ /**
+ * @brief Disable the AltitudeTrigger.
+ */
+ void disable();
+
+ /**
+ * @return The status of the AltitudeTrigger
+ */
+ bool isEnabled();
+
+ /**
+ * @return Set the altitude of the AltitudeTrigger
+ */
+ void setDeploymentAltitude(float altitude);
+
+private:
+ // Update method that posts a FLIGHT_WING_ALT_PASSED when the correct
+ // altitude is reached
+ void update();
+
+ std::atomic<bool> started{false};
+ std::atomic<bool> running{false};
+
+ // Number of times that the algorithm detects to be below the fixed
+ // altitude
+ int confidence = 0;
+
+ std::atomic<float> targetAltitude{
+ Config::AltitudeTrigger::DEPLOYMENT_ALTITUDE.value()};
+};
+
+} // namespace Parafoil
diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h
index 75a8d06c7..4e8d51eef 100644
--- a/src/Parafoil/BoardScheduler.h
+++ b/src/Parafoil/BoardScheduler.h
@@ -55,6 +55,7 @@ public:
Boardcore::TaskScheduler& nasController() { return critical; }
Boardcore::TaskScheduler& sensors() { return high; }
Boardcore::TaskScheduler& pinHandler() { return high; }
+ Boardcore::TaskScheduler& altitudeTrigger() { return medium; }
Boardcore::TaskScheduler& actuators() { return low; }
static Priority::PriorityLevel flightModeManagerPriority()
diff --git a/src/Parafoil/Configs/WingConfig.h b/src/Parafoil/Configs/WingConfig.h
new file mode 100644
index 000000000..68315dc5c
--- /dev/null
+++ b/src/Parafoil/Configs/WingConfig.h
@@ -0,0 +1,119 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto, Davide Basso
+ *
+ * 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 <units/Angle.h>
+#include <units/Frequency.h>
+#include <units/Length.h>
+#include <utils/Constants.h>
+
+#include <chrono>
+
+namespace Parafoil
+{
+namespace Config
+{
+namespace Wing
+{
+
+/* linter off */ using namespace std::chrono_literals;
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace Boardcore::Units::Length;
+/* linter off */ using namespace Boardcore::Units::Angle;
+
+constexpr auto UPDATE_RATE = 1_hz;
+constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15s;
+constexpr auto COMMAND_PERIOD = 6s;
+constexpr auto WING_DECREMENT = 30_deg;
+
+/**
+ * @brief The available algorithms for the wing controller.
+ */
+enum class AlgorithmId : size_t
+{
+ CLOSED_LOOP = 0,
+ EARLY_MANEUVER,
+ SEQUENCE,
+ ROTATION,
+ PROGRESSIVE_ROTATION,
+};
+
+namespace Default
+{
+
+#if defined(JESOLO)
+constexpr auto TARGET_LAT = 45.565264f;
+constexpr auto TARGET_LON = 12.577050f;
+#else // Milan
+constexpr auto TARGET_LAT = 45.5013853;
+constexpr auto TARGET_LON = 9.1544219;
+#endif
+
+#if defined(CLOSED_LOOP)
+constexpr auto ALGORITHM = AlgorithmId::CLOSED_LOOP;
+#elif EARLY_MANEUVER
+constexpr auto ALGORITHM = AlgorithmId::EARLY_MANEUVER;
+#elif SEQUENCE
+constexpr auto ALGORITHM = AlgorithmId::SEQUENCE;
+#elif ROTATION
+constexpr auto ALGORITHM = AlgorithmId::ROTATION;
+#elif PROGRESSIVE_ROTATION
+constexpr auto ALGORITHM = AlgorithmId::PROGRESSIVE_ROTATION;
+#else
+constexpr auto ALGORITHM = AlgorithmId::CLOSED_LOOP;
+#endif
+
+} // namespace Default
+
+namespace PI
+{
+constexpr auto SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
+constexpr auto SATURATION_MAX_LIMIT = Boardcore::Constants::PI;
+
+constexpr auto KP = 0.9f;
+constexpr auto KI = 0.05f;
+} // namespace PI
+
+namespace Guidance
+{
+
+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
+
+namespace AltitudeTrigger
+{
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace Boardcore::Units::Length;
+
+constexpr auto DEPLOYMENT_ALTITUDE = 300_m;
+constexpr auto CONFIDENCE = 10; // [samples]
+constexpr auto UPDATE_RATE = 10_hz;
+} // namespace AltitudeTrigger
+
+} // namespace Config
+} // namespace Parafoil
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index da56cf303..971f6cf58 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -21,6 +21,7 @@
*/
#include <Parafoil/Actuators/Actuators.h>
+#include <Parafoil/AltitudeTrigger/AltitudeTrigger.h>
#include <Parafoil/BoardScheduler.h>
#include <Parafoil/Buses.h>
#include <Parafoil/PinHandler/PinHandler.h>
@@ -112,6 +113,9 @@ int main()
// TODO: Radio
+ // Flight algorithms
+ auto altitudeTrigger = new AltitudeTrigger();
+ initResult &= depman.insert(altitudeTrigger);
// TODO: Wing Algorithm
// Actuators
@@ -129,6 +133,7 @@ int main()
START_MODULE(pinHandler);
// START_MODULE(radio) { miosix::led2On(); }
START_MODULE(nasController);
+ START_MODULE(altitudeTrigger);
// START_MODULE(wingController);
START_MODULE(actuators);
@@ -141,12 +146,12 @@ int main()
EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
// Turn on the initialization led on the CU
miosix::ledOn();
- std::cout << "Payload initialization Ok!" << std::endl;
+ std::cout << "Parafoil initialization Ok!" << std::endl;
}
else
{
EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
- std::cerr << "*** Payload initialization error ***" << std::endl;
+ std::cerr << "*** Parafoil initialization error ***" << std::endl;
}
std::cout << "Sensors status:" << std::endl;
diff --git a/src/Payload/AltitudeTrigger/AltitudeTrigger.h b/src/Payload/AltitudeTrigger/AltitudeTrigger.h
index 657cec82f..faa6e8d90 100644
--- a/src/Payload/AltitudeTrigger/AltitudeTrigger.h
+++ b/src/Payload/AltitudeTrigger/AltitudeTrigger.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
- * Authors: Federico Mandelli, Nicclò Betto
+ * Authors: Federico Mandelli, Niccolò Betto
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
--
GitLab