From d1c59f3ee6680bc74b3d9955749c60a8f39eeb37 Mon Sep 17 00:00:00 2001
From: Riccardo Musso <riccardo.musso@skywarder.eu>
Date: Wed, 2 Oct 2024 15:17:04 +0200
Subject: [PATCH] [Converter] implemented simple convertion function from NED
to angles
---
CMakeLists.txt | 5 +++-
.../AutomatedAntennas/Converter.h | 24 +++++++++++++++
.../automated-antennas-entry.cpp | 30 +++++++++++++++++++
3 files changed, 58 insertions(+), 1 deletion(-)
create mode 100644 src/entrypoints/Groundstation/AutomatedAntennas/Converter.h
create mode 100644 src/entrypoints/Groundstation/automated-antennas-entry.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 993c16b6b..1b86f1083 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -103,4 +103,7 @@ add_executable(nokia-groundstation-entry
${GROUNDSTATION_COMMON} ${GROUNDSTATION_NOKIA}
)
target_include_directories(nokia-groundstation-entry PRIVATE ${OBSW_INCLUDE_DIRS})
-sbs_target(nokia-groundstation-entry stm32f429zi_skyward_groundstation_v2)
\ No newline at end of file
+sbs_target(nokia-groundstation-entry stm32f429zi_skyward_groundstation_v2)
+
+add_executable(automated-antennas-entry src/entrypoints/Groundstation/automated-antennas-entry.cpp)
+sbs_target(automated-antennas-entry stm32f407vg_stm32f4discovery)
diff --git a/src/entrypoints/Groundstation/AutomatedAntennas/Converter.h b/src/entrypoints/Groundstation/AutomatedAntennas/Converter.h
new file mode 100644
index 000000000..fb75cd74c
--- /dev/null
+++ b/src/entrypoints/Groundstation/AutomatedAntennas/Converter.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <cmath>
+
+struct NEDCoords
+{
+ float n = 0;
+ float e = 0;
+ float d = 0;
+};
+
+struct AntennaAngles
+{
+ float theta1 = 0;
+ float theta2 = 0;
+};
+
+AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned)
+{
+ AntennaAngles angles;
+ angles.theta1 = std::atan2(ned.n, ned.e);
+ angles.theta2 = std::atan2(-ned.d, ned.n);
+ return angles;
+}
\ No newline at end of file
diff --git a/src/entrypoints/Groundstation/automated-antennas-entry.cpp b/src/entrypoints/Groundstation/automated-antennas-entry.cpp
new file mode 100644
index 000000000..a344a65ad
--- /dev/null
+++ b/src/entrypoints/Groundstation/automated-antennas-entry.cpp
@@ -0,0 +1,30 @@
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+
+#include "AutomatedAntennas/Converter.h"
+
+using namespace miosix;
+using namespace Boardcore;
+
+inline float randf() { return (std::rand() % 200 - 100) / 100.f; }
+
+int main()
+{
+ constexpr int N = 10000;
+
+ printf("Starting test\n");
+ uint64_t start = TimestampTimer::getTimestamp();
+
+ for (int i = 0; i < N; i++)
+ {
+ NEDCoords coords = {randf(), randf(), randf()};
+ AntennaAngles angles = rocketPositionToAntennaAngles(coords);
+ printf("NED: %.2f ; %.2f ; %.2f -> Angles %.2f ; %.2f\n", coords.n,
+ coords.e, coords.d, angles.theta1, angles.theta2);
+ }
+
+ uint64_t end = TimestampTimer::getTimestamp();
+ printf("Took %lld millis for %d calls.\n", (end - start) / 1000ull, N);
+
+ return 0;
+}
--
GitLab