From bc1e2c98ebad9dd69345a363ee209613b6afc7f6 Mon Sep 17 00:00:00 2001
From: Angelo Prete <angelo.prete@skywarder.eu>
Date: Sat, 9 Mar 2024 19:50:38 +0100
Subject: [PATCH] [Parafoil] Fixed WingController, added twirling in Actuators
and updated config files
---
cmake/dependencies.cmake | 1 +
skyward-boardcore | 2 +-
src/boards/Parafoil/Actuators/Actuators.cpp | 17 +++-
src/boards/Parafoil/Actuators/Actuators.h | 18 ++--
.../Parafoil/Configs/ActuatorsConfigs.h | 11 ++-
src/boards/Parafoil/Configs/WingConfig.h | 24 +++--
src/boards/Parafoil/Sensors/Sensors.cpp | 4 +-
src/boards/Parafoil/Sensors/Sensors.h | 3 +-
.../FlightModeManager/FlightModeManager.cpp | 4 +-
.../FlightModeManager/FlightModeManager.h | 4 +-
.../WingController/WingController.cpp | 99 ++++++++-----------
.../WingController/WingController.h | 11 ++-
12 files changed, 102 insertions(+), 96 deletions(-)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index f54d49448..00048a404 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -36,6 +36,7 @@ set(PARAFOIL_COMPUTER
src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
src/boards/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+ src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
src/boards/Parafoil/Wing/FileWingAlgorithm.cpp
src/boards/Parafoil/Wing/WingAlgorithm.cpp
src/boards/Parafoil/WindEstimationScheme/WindEstimation.cpp
diff --git a/skyward-boardcore b/skyward-boardcore
index f57081da2..093d5b310 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit f57081da20964030a07990dd985afe5c845241f4
+Subproject commit 093d5b3108b4dd27612222eb99ff2790c96886be
diff --git a/src/boards/Parafoil/Actuators/Actuators.cpp b/src/boards/Parafoil/Actuators/Actuators.cpp
index 34d1dd26c..4f317e60d 100644
--- a/src/boards/Parafoil/Actuators/Actuators.cpp
+++ b/src/boards/Parafoil/Actuators/Actuators.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli, 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
@@ -57,7 +57,6 @@ bool Actuators::start()
bool Actuators::setServo(ServosList servoId, float percentage)
{
- percentage += offset;
if (percentage > WingConfig::MAX_SERVO_APERTURE)
{
percentage = WingConfig::MAX_SERVO_APERTURE;
@@ -207,8 +206,16 @@ float Actuators::getServoAngle(ServosList servoId)
return 0;
}
-void Actuators::setServosOffset(float offset) { this->offset = offset; }
+void Actuators::startTwirl()
+{
+ setServo(PARAFOIL_LEFT_SERVO, SERVO_TWIRL_RADIUS);
+ setServo(PARAFOIL_RIGHT_SERVO, 0);
+}
-float Actuators::getServosOffset() { return offset; }
+void Actuators::stopTwirl()
+{
+ setServo(PARAFOIL_LEFT_SERVO, 0);
+ setServo(PARAFOIL_RIGHT_SERVO, 0);
+}
} // namespace Parafoil
diff --git a/src/boards/Parafoil/Actuators/Actuators.h b/src/boards/Parafoil/Actuators/Actuators.h
index 00291ac77..dca587a95 100644
--- a/src/boards/Parafoil/Actuators/Actuators.h
+++ b/src/boards/Parafoil/Actuators/Actuators.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli, 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
@@ -102,18 +102,14 @@ struct Actuators : public Boardcore::Module
float getServoAngle(ServosList servoId);
/**
- * @brief Sets the offset for all servos.
- *
- * @param offset New offset [0-1].
+ * @brief Starts twirl (one servo is set to 0 and the other one is not).
*/
- void setServosOffset(float offset);
+ void startTwirl();
/**
- * @brief Returns the current offset of the servos.
- *
- * @return Current offset [0-1].
+ * @brief Stops twirling (both servos are set to 0).
*/
- float getServosOffset();
+ void stopTwirl();
private:
Boardcore::Servo* leftServo = nullptr;
@@ -123,8 +119,6 @@ private:
miosix::FastMutex leftServoMutex;
miosix::FastMutex rightServoMutex;
miosix::FastMutex rocketSignalingStateMutex;
-
- float offset = 0;
};
} // namespace Parafoil
diff --git a/src/boards/Parafoil/Configs/ActuatorsConfigs.h b/src/boards/Parafoil/Configs/ActuatorsConfigs.h
index 61eb7ea9b..b7a734978 100644
--- a/src/boards/Parafoil/Configs/ActuatorsConfigs.h
+++ b/src/boards/Parafoil/Configs/ActuatorsConfigs.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli, 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
@@ -36,7 +36,7 @@ static TIM_TypeDef* const SERVO_1_TIMER = TIM3;
constexpr Boardcore::TimerUtils::Channel SERVO_1_PWM_CH =
Boardcore::TimerUtils::Channel::CHANNEL_1;
-constexpr float LEFT_SERVO_ROTATION = 120; // [deg]
+constexpr float LEFT_SERVO_ROTATION = 180; // [deg]
constexpr float LEFT_SERVO_MIN_PULSE = 900; // [us]
constexpr float LEFT_SERVO_MAX_PULSE =
LEFT_SERVO_MIN_PULSE + 10 * LEFT_SERVO_ROTATION; // [us]
@@ -46,11 +46,14 @@ static TIM_TypeDef* const SERVO_2_TIMER = TIM3;
constexpr Boardcore::TimerUtils::Channel SERVO_2_PWM_CH =
Boardcore::TimerUtils::Channel::CHANNEL_2;
-constexpr float RIGHT_SERVO_ROTATION = 120; // [deg]
+constexpr float RIGHT_SERVO_ROTATION = 180; // [deg]
constexpr float RIGHT_SERVO_MIN_PULSE = 2100; // [us]
constexpr float RIGHT_SERVO_MAX_PULSE =
RIGHT_SERVO_MIN_PULSE - 10 * RIGHT_SERVO_ROTATION; // [us]
+// Parafoil twirl
+constexpr float SERVO_TWIRL_RADIUS = 0.75; // [%]
+
// Buzzer configs
static TIM_TypeDef* const BUZZER_TIMER = TIM1;
constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL =
diff --git a/src/boards/Parafoil/Configs/WingConfig.h b/src/boards/Parafoil/Configs/WingConfig.h
index 45b2014be..9c5fc145c 100644
--- a/src/boards/Parafoil/Configs/WingConfig.h
+++ b/src/boards/Parafoil/Configs/WingConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli, 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
@@ -31,6 +31,16 @@ namespace WingConfig
{
// Algorithm configuration
+#if defined(CLOSED_LOOP)
+constexpr int SELECTED_ALGORITHM = 0;
+#elif EARLY_MANEUVER
+constexpr int SELECTED_ALGORITHM = 1;
+#elif SEQUENCE
+constexpr int SELECTED_ALGORITHM = 2;
+#else
+constexpr int SELECTED_ALGORITHM = 0;
+#endif
+
#if defined(JESOLO)
constexpr float DEFAULT_TARGET_LAT = 45.565835;
constexpr float DEFAULT_TARGET_LON = 12.577307;
@@ -39,8 +49,7 @@ constexpr float DEFAULT_TARGET_LAT = 45.501148;
constexpr float DEFAULT_TARGET_LON = 9.156301;
#endif
-constexpr float OFFSET = 0.25;
-constexpr float MAX_SERVO_APERTURE = 0.5 + OFFSET;
+constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms]
constexpr int WING_UPDATE_PERIOD = 1000; // [ms]
@@ -53,15 +62,14 @@ constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD = 150; //[m]
constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50; //[m]
// TODO check this parameter preflight
-constexpr float KP = 0.4; //[m]
-constexpr float KI = 0.08; //[m]
+constexpr float KP = 0.0075; //[m]
+constexpr float KI = 0.0007; //[m]
constexpr float ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE = 300; // [meters]
constexpr int ALTITUDE_TRIGGER_CONFIDENCE = 10; // [number of sample]
constexpr int ALTITUDE_TRIGGER_PERIOD = 100; //[ms]
-// TODO add algorithm selection
-constexpr int SELECTED_ALGORITHM = 0;
+constexpr float MAX_SERVO_APERTURE = 0.5; //[%]
struct WingConfigStruct
{
diff --git a/src/boards/Parafoil/Sensors/Sensors.cpp b/src/boards/Parafoil/Sensors/Sensors.cpp
index ed9621a30..06383e246 100644
--- a/src/boards/Parafoil/Sensors/Sensors.cpp
+++ b/src/boards/Parafoil/Sensors/Sensors.cpp
@@ -418,9 +418,9 @@ void Sensors::batteryVoltageInit()
LOG_INFO(logger, "Battery voltage sensor setup done!");
}
-std::array<SensorInfo, 8> Sensors::getSensorInfo()
+std::array<SensorInfo, NUMBER_OF_SENSORS> Sensors::getSensorInfo()
{
- std::array<SensorInfo, 8> sensorState;
+ std::array<SensorInfo, NUMBER_OF_SENSORS> sensorState;
for (size_t i = 0; i < sensorsInit.size(); i++)
{
sensorState[i] = sensorsInit[i]();
diff --git a/src/boards/Parafoil/Sensors/Sensors.h b/src/boards/Parafoil/Sensors/Sensors.h
index 2455fc52b..fbb7996d8 100644
--- a/src/boards/Parafoil/Sensors/Sensors.h
+++ b/src/boards/Parafoil/Sensors/Sensors.h
@@ -81,7 +81,8 @@ public:
Boardcore::InternalADCData getInternalADCLastSample();
Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
- std::array<Boardcore::SensorInfo, 8> getSensorInfo();
+ std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
+ getSensorInfo();
Boardcore::BMX160* bmx160 = nullptr;
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
index 1afbd283f..31788a9a0 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, 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
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
index a89a89f37..4c411a110 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, 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
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
index 7d8dcaa65..0a86b129f 100644
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul, 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
@@ -135,38 +135,38 @@ State WingController::state_flying(const Event& event)
State WingController::state_calibration(const Boardcore::Event& event)
{
ModuleManager& modules = ModuleManager::getInstance();
+ static uint16_t calibrationTimeoutEventId;
+
switch (event)
{
case EV_ENTRY: // starts twirling and calibration wes
{
logStatus(WingControllerState::CALIBRATION);
- // TODO
- // modules.get<Actuators>()->setServo(ServosList::PARAFOIL_LEFT_SERVO,
- // 1);
- // modules.get<Actuators>()->setServo(ServosList::PARAFOIL_RIGHT_SERVO,
- // 0);
- // EventBroker::getInstance().postDelayed(DPL_WES_CAL_DONE,
- // TOPIC_DPL,
- // WES_CALIBRATION_TIMEOUT);
+
+ modules.get<Actuators>()->startTwirl();
+ calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_WES_CAL_DONE, TOPIC_DPL, WES_CALIBRATION_TIMEOUT);
+
modules.get<WindEstimation>()
->startWindEstimationSchemeCalibration();
- return transition(&WingController::state_controlled_descent);
+
+ return HANDLED;
}
case EV_EXIT:
{
+ EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId);
return HANDLED;
}
case EV_EMPTY:
{
return tranSuper(&WingController::state_flying);
}
- // case DPL_WES_CAL_DONE:
- // {
- // reset();
- // // Turn off the cutters
- // ModuleManager::getInstance().get<Actuators>()->cuttersOff();
- // return transition(&WingController::state_controlled_descent);
- // }
+ case DPL_WES_CAL_DONE:
+ {
+ modules.get<Actuators>()->stopTwirl();
+
+ return transition(&WingController::state_controlled_descent);
+ }
default:
{
return UNHANDLED;
@@ -191,8 +191,7 @@ State WingController::state_controlled_descent(const Boardcore::Event& event)
.get<NASController>()
->getNasState()
.e});
- ModuleManager::getInstance().get<Actuators>()->setServosOffset(
- OFFSET);
+
startAlgorithm();
return HANDLED;
}
@@ -259,52 +258,38 @@ bool WingController::addAlgorithms()
WingAlgorithm* algorithm;
WingAlgorithmData step;
- bool result;
- // We add an AutomaticWingAlgorithm and a FileWingAlgorithm
+ bool result = false;
+ // Algorithm 0
+ algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
+ PARAFOIL_RIGHT_SERVO, clGuidance);
+ result = algorithm->init();
+ algorithms.push_back(algorithm);
+ // Algorithm 1
algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
PARAFOIL_RIGHT_SERVO, emGuidance);
- // Ensure that the servos are correct
- algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
-
- // Init the algorithm
- result = algorithm->init();
-
- // Add the algorithm to the vector
+ result &= algorithm->init();
algorithms.push_back(algorithm);
+ // Algorithm 2
algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
-
- for (int i = 0; i < 3; i++)
- {
- step.servo1Angle = 120;
- step.servo2Angle = 0;
- step.timestamp = 0 + i * 40000000;
- algorithm->addStep(step);
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- step.timestamp += 10000000 + i * 40000000;
- algorithm->addStep(step);
- step.servo1Angle = 0;
- step.servo2Angle = 120;
- step.timestamp += 10000000 + i * 40000000;
- algorithm->addStep(step);
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- step.timestamp += 10000000 + i * 40000000;
- algorithm->addStep(step);
- }
-
- // Ensure that the servos are correct
- algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
-
- // Init the algorithm
- result = result && algorithm->init();
-
+ step.servo1Angle = 0;
+ step.servo2Angle = 120;
+ step.timestamp = 0;
+ algorithm->addStep(step);
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ step.timestamp += 1000 * WingConfig::WING_STRAIGHT_FLIGHT_TIMEOUT;
+ algorithm->addStep(step);
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ step.timestamp += WingConfig::WING_STRAIGHT_FLIGHT_TIMEOUT;
+ algorithm->addStep(step);
+ result &= algorithm->init();
// Add the algorithm to the vector
algorithms.push_back(algorithm);
- selectAlgorithm(0);
+ selectAlgorithm(SELECTED_ALGORITHM);
return result;
}
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.h b/src/boards/Parafoil/StateMachines/WingController/WingController.h
index bb845e3a2..aa2f17f59 100644
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.h
+++ b/src/boards/Parafoil/StateMachines/WingController/WingController.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli, 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
@@ -22,6 +22,7 @@
#pragma once
+#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
#include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
#include <Parafoil/Wing/WingAlgorithm.h>
#include <events/HSM.h>
@@ -154,6 +155,12 @@ private:
*/
EarlyManeuversGuidanceAlgorithm emGuidance;
+ /**
+ * @brief Instance of the Closed Loop Guidance Algorithm used by
+ * AutomaticWingAlgorithm
+ */
+ ClosedLoopGuidanceAlgorithm clGuidance;
+
/**
* @brief starts the selected algorithm
*/
--
GitLab