From d13d1f9267d49fedd01db4cd2fbb8655187a64ee Mon Sep 17 00:00:00 2001 From: Emilio Corigliano <emilio.corigliano@skywarder.eu> Date: Wed, 2 Oct 2024 15:17:05 +0200 Subject: [PATCH] [AutomatedAntennas] Implemented first version of the Follower algorithm --- cmake/dependencies.cmake | 1 + .../Automated/Config/FollowerConfig.h | 39 +++++ .../Automated/Follower/Follower.cpp | 149 ++++++++++++++++++ .../Automated/Follower/Follower.h | 83 ++++++++++ 4 files changed, 272 insertions(+) create mode 100644 src/boards/Groundstation/Automated/Config/FollowerConfig.h create mode 100644 src/boards/Groundstation/Automated/Follower/Follower.cpp create mode 100644 src/boards/Groundstation/Automated/Follower/Follower.h diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 8819a4ddc..c5e3e55e1 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -142,6 +142,7 @@ set(GROUNDSTATION_BASE set(GROUNDSTATION_AUTOMATED src/boards/Groundstation/Automated/Radio/Radio.cpp src/boards/Groundstation/Automated/Radio/RadioStatus.cpp + src/boards/Groundstation/Automated/Follower/Follower.cpp src/boards/Groundstation/Automated/Hub.cpp ) diff --git a/src/boards/Groundstation/Automated/Config/FollowerConfig.h b/src/boards/Groundstation/Automated/Config/FollowerConfig.h new file mode 100644 index 000000000..dc7ace156 --- /dev/null +++ b/src/boards/Groundstation/Automated/Config/FollowerConfig.h @@ -0,0 +1,39 @@ +/* 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; +} +} \ No newline at end of file diff --git a/src/boards/Groundstation/Automated/Follower/Follower.cpp b/src/boards/Groundstation/Automated/Follower/Follower.cpp new file mode 100644 index 000000000..e36fbbfd8 --- /dev/null +++ b/src/boards/Groundstation/Automated/Follower/Follower.cpp @@ -0,0 +1,149 @@ +/* Copyright (c) 2023 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 "Follower.h" + +#include <Groundstation/Automated/Actuators/Actuators.h> +#include <Groundstation/Automated/Sensors/Sensors.h> +#include <utils/AeroUtils/AeroUtils.h> + +#include <Eigen/Dense> +#include <cmath> + +using namespace Boardcore; + +namespace Antennas +{ + +Follower::Follower() {} + +bool Follower::init() +{ + for (int i = 0; i < maxInitRetries; i++) + { + VN300Data vn300Data = ModuleManager::getInstance() + .get<Antennas::Sensors>() + ->getVN300LastSample(); + + if (vn300Data.fix_gps > 0) + { + antennaPosition.gpsTimestamp = vn300Data.insTimestamp; + antennaPosition.latitude = vn300Data.latitude; + antennaPosition.latitude = vn300Data.latitude; + antennaPosition.height = vn300Data.altitude; + antennaPosition.velocityNorth = vn300Data.nedVelX; + antennaPosition.velocityEast = vn300Data.nedVelY; + antennaPosition.velocityDown = vn300Data.nedVelZ; + antennaPosition.satellites = vn300Data.fix_gps; + antennaPosition.fix = (vn300Data.fix_gps > 0); + LOG_INFO(logger, "Initialization Succeeded"); + return true; + } + else + { + // Waiting for GPS fix for another second + LOG_WARN( + logger, + "Initialization Failed for the {} time, retrying in one second", + i); + miosix::Thread::sleep(1000); + } + } + + return false; +} + +/** + * @param get rocket NAS state. + */ +NASState Follower::getLastRocketNasState() { return lastRocketNasState; } + +GPSData Follower::getLastRocketGpsState() { return lastRocketGpsState; } + +/** + * @param set rocket NAS state. + */ +void Follower::setLastRocketState(const NASState& nas, const GPSData& gps) +{ + lastRocketNasState = nas; + lastRocketGpsState = gps; +} + +void Follower::step() +{ + // Calculating the NED coordinates in Automated Antennas reference system + Eigen::Vector2f antennasCoord{antennaPosition.latitude, + antennaPosition.longitude}; + Eigen::Vector2f rocketCoord{lastRocketGpsState.latitude, + lastRocketGpsState.longitude}; + Eigen::Vector2f neRocket = + Aeroutils::geodetic2NED(rocketCoord, antennasCoord); + + // Getting the position of the rocket wrt the antennas in NED frame + NEDCoords rocketPosition; + rocketPosition.n = neRocket[0]; + rocketPosition.e = neRocket[1]; + rocketPosition.d = (-antennaPosition.height); + + // Calculating absolute angles from the NED frame + AntennaAngles absoluteAngles = + rocketPositionToAntennaAngles(rocketPosition); + + // Calculation actuation angles wrt the current position + VN300Data vn300 = + ModuleManager::getInstance().get<Sensors>()->getVN300LastSample(); + AntennaAngles stepperAngles{absoluteAngles.theta1 - vn300.yaw, + absoluteAngles.theta2 - vn300.pitch}; + + // Propagator step + + // Calculate angular velocity for moving the antennas toward position + float horizontalSpeed = + (stepperAngles.theta1 * 1000) / (360 * FollowerConfig::FOLLOWER_PERIOD); + float verticalSpeed = + (stepperAngles.theta2 * 1000) / (360 * FollowerConfig::FOLLOWER_PERIOD); + + ModuleManager::getInstance().get<Actuators>()->setSpeed( + Actuators::StepperList::HORIZONTAL, horizontalSpeed); + ModuleManager::getInstance().get<Actuators>()->setSpeed( + Actuators::StepperList::VERTICAL, verticalSpeed); + + // Actuating steppers + ModuleManager::getInstance().get<Actuators>()->moveDeg( + Actuators::StepperList::HORIZONTAL, stepperAngles.theta1); + ModuleManager::getInstance().get<Actuators>()->moveDeg( + Actuators::StepperList::VERTICAL, stepperAngles.theta2); +} + +AntennaAngles Follower::rocketPositionToAntennaAngles(const NEDCoords& ned) +{ + AntennaAngles angles; + // Calculating the horizontal angle in absolute ned frame + angles.theta1 = std::atan2(ned.n, ned.e); + // Calculating the horizontal distance of the rocket + float ground_dist = std::sqrt(ned.n * ned.n + ned.e * ned.e); + // Calculating the vertical angle in absolute ned frame + angles.theta2 = std::atan2(-ned.d, ground_dist); + return angles; +} + +} // namespace Antennas diff --git a/src/boards/Groundstation/Automated/Follower/Follower.h b/src/boards/Groundstation/Automated/Follower/Follower.h new file mode 100644 index 000000000..37abaea7e --- /dev/null +++ b/src/boards/Groundstation/Automated/Follower/Follower.h @@ -0,0 +1,83 @@ +/* Copyright (c) 2023 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. + */ + +#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> + +namespace Antennas +{ + +struct NEDCoords +{ + float n = 0; + float e = 0; + float d = 0; +}; + +struct AntennaAngles +{ + float theta1 = 0; + float theta2 = 0; +}; + +class Follower : public Boardcore::Algorithm, public Boardcore::Module +{ +public: + Follower(); + + bool init() override; + + /** + * @param get rocket NAS state. + */ + Boardcore::NASState getLastRocketNasState(); + Boardcore::GPSData getLastRocketGpsState(); + + /** + * @param set rocket NAS state. + */ + void setLastRocketState(const Boardcore::NASState& nas, + const Boardcore::GPSData& gps); + +private: + void step() override; + + AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned); + + const uint8_t maxInitRetries = + 10; ///< max number of retries for GPS data acquisition + Boardcore::NASState lastRocketNasState; + Boardcore::GPSData lastRocketGpsState; + Boardcore::GPSData antennaPosition; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Follower"); +}; + +} // namespace Antennas -- GitLab