From b42c618a89a6dee7409f0aa5012e05aa2d6f670b Mon Sep 17 00:00:00 2001
From: Federico Lolli <federico.lolli@skywarder.eu>
Date: Fri, 31 May 2024 00:45:14 +0200
Subject: [PATCH] [ARP] Moved Follower out of obsw into boardcore
---
cmake/dependencies.cmake | 1 -
skyward-boardcore | 2 +-
.../Automated/Config/FollowerConfig.h | 39 --------
.../Automated/Config/PropagatorConfig.h | 35 -------
.../Automated/Follower/Follower.h | 97 -------------------
.../Automated/Follower/FollowerData.h | 72 --------------
.../Automated/SMController/SMController.cpp | 37 ++++++-
.../Automated/SMController/SMController.h | 9 +-
8 files changed, 41 insertions(+), 251 deletions(-)
delete mode 100644 src/boards/Groundstation/Automated/Config/FollowerConfig.h
delete mode 100644 src/boards/Groundstation/Automated/Config/PropagatorConfig.h
delete mode 100644 src/boards/Groundstation/Automated/Follower/Follower.h
delete mode 100644 src/boards/Groundstation/Automated/Follower/FollowerData.h
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 2755d6bf6..8d642e763 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -52,7 +52,6 @@ set(GROUNDSTATION_COMMON
set(GROUNDSTATION_AUTOMATED
src/boards/Groundstation/Automated/BoardStatus.cpp
src/boards/Groundstation/Automated/Radio/Radio.cpp
- src/boards/Groundstation/Automated/Follower/Follower.cpp
src/boards/Groundstation/Automated/Ports/Ethernet.cpp
src/boards/Groundstation/Automated/Hub.cpp
src/boards/Groundstation/Automated/Leds/Leds.cpp
diff --git a/skyward-boardcore b/skyward-boardcore
index 6b82b66d2..a9d67d2c6 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit 6b82b66d23e71d1db3f7fc1aeb1de9f0c02fbe4d
+Subproject commit a9d67d2c6c604c11313c18943dc6df66f8cdacbf
diff --git a/src/boards/Groundstation/Automated/Config/FollowerConfig.h b/src/boards/Groundstation/Automated/Config/FollowerConfig.h
deleted file mode 100644
index 909b55551..000000000
--- a/src/boards/Groundstation/Automated/Config/FollowerConfig.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Federico Mandelli
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <common/Events.h>
-#include <common/Topics.h>
-#include <stdint.h>
-
-#include <map>
-
-namespace Antennas
-{
-
-namespace FollowerConfig
-{
-
-constexpr uint32_t FOLLOWER_PERIOD = 100;
-}
-} // namespace Antennas
diff --git a/src/boards/Groundstation/Automated/Config/PropagatorConfig.h b/src/boards/Groundstation/Automated/Config/PropagatorConfig.h
deleted file mode 100644
index 93db7a714..000000000
--- a/src/boards/Groundstation/Automated/Config/PropagatorConfig.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Federico Lolli
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-namespace Antennas
-{
-
-namespace PropagatorConfig
-{
-
-/// @brief Period of the propagator algorithm [ms].
-constexpr uint32_t PROPAGATOR_PERIOD = 100; // 10 Hz
-
-} // namespace PropagatorConfig
-} // namespace Antennas
diff --git a/src/boards/Groundstation/Automated/Follower/Follower.h b/src/boards/Groundstation/Automated/Follower/Follower.h
deleted file mode 100644
index fa2918283..000000000
--- a/src/boards/Groundstation/Automated/Follower/Follower.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Emilio Corigliano, Niccolò Betto, Federico Lolli
- *
- * 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 <Groundstation/Automated/Config/FollowerConfig.h>
-#include <algorithms/Algorithm.h>
-#include <algorithms/NAS/NASState.h>
-#include <diagnostic/PrintLogger.h>
-#include <logger/Logger.h>
-#include <sensors/SensorData.h>
-
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-#include "FollowerData.h"
-
-namespace Antennas
-{
-
-class Follower : public Boardcore::Algorithm
-{
-public:
- Follower();
-
- bool init() override;
-
- // Setters for the GPS coordinates of the antenna
- void setAntennaCoordinates(const Boardcore::GPSData& gpsData);
- // Setters for the GPS coordinates of the rocket while in ramp
- void setInitialRocketCoordinates(const Boardcore::GPSData& gpsData);
- // Setters for the NAS state of the rocket
- void setLastRocketNasState(const Boardcore::NASState nasState);
-
- /**
- * @brief Getter for the target antenna position computed by the algorithm.
- * @returns The target antenna positions.
- */
- AntennaAngles getTargetAngles() { return targetAngles; }
-
-private:
- void step() override;
-
- /**
- * @brief Synchronized getter that returns a copy of the last NAS state
- * of the rocket
- */
- Boardcore::NASState getLastRocketNasState();
-
- /**
- * @brief Calculates the target angles from the given NED coordinates that
- * the antenna should point to.
- */
- AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned);
-
- // max number of retries for GPS data acquisition
- const uint8_t maxInitRetries = 120;
-
- bool antennaCoordinatesSet = false;
- bool rocketCoordinatesSet = false;
-
- Boardcore::NASState lastRocketNasState;
- miosix::FastMutex lastRocketNasStateMutex;
-
- // GPS coordinates of the antenna [lat, lon, alt] [deg, deg, m]
- Eigen::Vector3f antennaCoordinates;
- // GPS coordinates of the rocket while in ramp [lat, lon, alt] [deg, deg, m]
- Eigen::Vector3f initialRocketCoordinates;
- // Initial distance between the antenna and the rocket while in ramp [lat,
- // lon, alt] [deg, deg, m]
- Eigen::Vector2f initialAntennaRocketDistance;
-
- // Target yaw and pitch of the system [deg, deg]
- AntennaAngles targetAngles;
-
- Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Follower");
-};
-
-} // namespace Antennas
diff --git a/src/boards/Groundstation/Automated/Follower/FollowerData.h b/src/boards/Groundstation/Automated/Follower/FollowerData.h
deleted file mode 100644
index 5e827c6f7..000000000
--- a/src/boards/Groundstation/Automated/Follower/FollowerData.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Emilio Corigliano
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "sensors/SensorData.h"
-
-namespace Antennas
-{
-
-struct NEDCoords
-{
- float n = 0;
- float e = 0;
- float d = 0;
-};
-
-/**
- * @brief A structure for storing angles relative to the NED frame.
- */
-struct AntennaAngles
-{
- float yaw; //!< Angle between the X axis (N axis) and the target position
- //!< on the XY plane (NE plane), positive anti-clockwise [deg]
- float pitch; //!< Angle between the XY plane (NE plane) and the target
- //!< position [deg]
-};
-
-/**
- * @brief A structure for logging the ARP system coordinates set in the
- * Follower.
- */
-struct LogAntennasCoordinates : public Boardcore::GPSData
-{
- explicit LogAntennasCoordinates(const Boardcore::GPSData& data)
- : Boardcore::GPSData(data)
- {
- }
-
- LogAntennasCoordinates() {}
-};
-
-/**
- * @brief A structure for logging the Rocket coordinates set in the Follower.
- */
-struct LogRocketCoordinates : public Boardcore::GPSData
-{
- explicit LogRocketCoordinates(const Boardcore::GPSData& data)
- : Boardcore::GPSData(data)
- {
- }
-
- LogRocketCoordinates() {}
-};
-} // namespace Antennas
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp
index d4807967d..d9c5ab6cc 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.cpp
+++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp
@@ -23,8 +23,6 @@
#include "SMController.h"
#include <Groundstation/Automated/Actuators/Actuators.h>
-#include <Groundstation/Automated/Config/FollowerConfig.h>
-#include <Groundstation/Automated/Config/PropagatorConfig.h>
#include <Groundstation/Automated/Config/SMControllerConfig.h>
#include <Groundstation/Automated/Hub.h>
#include <Groundstation/Automated/Leds/Leds.h>
@@ -48,7 +46,8 @@ namespace Antennas
SMController::SMController(TaskScheduler* sched)
: HSM(&SMController::state_config), scheduler(sched),
- propagator(PropagatorConfig::PROPAGATOR_PERIOD), follower()
+ propagator(SMControllerConfig::UPDATE_PERIOD),
+ follower(SMControllerConfig::UPDATE_PERIOD)
{
EventBroker::getInstance().subscribe(this, TOPIC_ARP);
EventBroker::getInstance().subscribe(this, TOPIC_TMTC);
@@ -211,7 +210,39 @@ void SMController::update()
// update the follower with the propagated state
follower.setLastRocketNasState(predicted.getNasState());
+ VN300Data vn300Data = ModuleManager::getInstance()
+ .get<Sensors>()
+ ->getVN300LastSample();
+ follower.setLastAntennaAttitude(vn300Data);
follower.update(); // step the follower
+ FollowerState follow = follower.getState();
+
+ // actuate the steppers
+ auto steppers = ModuleManager::getInstance().get<Actuators>();
+ steppers->setSpeed(StepperList::STEPPER_X, follow.horizontalSpeed);
+ steppers->setSpeed(StepperList::STEPPER_Y, follow.verticalSpeed);
+
+ ErrorMovement actuation =
+ steppers->moveDeg(StepperList::STEPPER_X, follow.yaw);
+ if (actuation != ErrorMovement::OK)
+ {
+ LOG_ERR(
+ logger,
+ "Step antenna - STEPPER_X could not move or reached move "
+ "limit. Error: ",
+ actuation, "\n");
+ }
+
+ actuation = steppers->moveDeg(StepperList::STEPPER_Y, follow.pitch);
+ if (actuation != ErrorMovement::OK)
+ {
+ LOG_ERR(
+ logger,
+ "Step antenna - STEPPER_Y could not move or reached move "
+ "limit. Error: ",
+ actuation, "\n");
+ }
+
break;
}
default:
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.h b/src/boards/Groundstation/Automated/SMController/SMController.h
index 2b751da75..bddb7ff85 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.h
+++ b/src/boards/Groundstation/Automated/SMController/SMController.h
@@ -23,7 +23,7 @@
#pragma once
#include <Groundstation/Automated/Actuators/Actuators.h>
-#include <Groundstation/Automated/Follower/Follower.h>
+#include <algorithms/Follower/Follower.h>
#include <algorithms/NAS/NASState.h>
#include <algorithms/Propagator/Propagator.h>
#include <events/EventBroker.h>
@@ -101,7 +101,10 @@ public:
/**
* @brief Getter for follower target angles
*/
- AntennaAngles getTargetAngles() { return follower.getTargetAngles(); }
+ Boardcore::AntennaAngles getTargetAngles()
+ {
+ return follower.getTargetAngles();
+ }
private:
/**
@@ -115,7 +118,7 @@ private:
SMControllerStatus status;
Boardcore::Propagator propagator;
- Antennas::Follower follower;
+ Boardcore::Follower follower;
Boardcore::PrintLogger logger =
Boardcore::Logging::getLogger("SMController");
--
GitLab