diff --git a/CMakeLists.txt b/CMakeLists.txt index 993c16b6b44c9e5dd9f4e2a2b1110dc9850e000d..1b86f10831c1a778b356624812b5c30675a4de81 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 0000000000000000000000000000000000000000..fb75cd74c55a1de0cb68937befbea735babc4d86 --- /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 0000000000000000000000000000000000000000..a344a65ad83b21cf098964188fd9eb3b82092b38 --- /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; +}