diff --git a/CMakeLists.txt b/CMakeLists.txt
index cbd46c1eb554e1898822d969ec8dbf91b5f42a18..2b881eb2878c27d9341cbbca24327abf3486ce15 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -105,9 +105,12 @@ add_executable(nokia-groundstation-entry
target_include_directories(nokia-groundstation-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(nokia-groundstation-entry stm32f429zi_skyward_groundstation_v2)
-add_executable(automated-antennas-entry src/entrypoints/Groundstation/AutomatedAntennas/automated-antennas-entry.cpp)
+add_executable(automated-antennas-entry
+ src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
+ ${ANTENNAS} ${GROUNDSTATION_COMMON} ${GROUNDSTATION_AUTOMATED}
+)
target_include_directories(automated-antennas-entry PRIVATE ${OBSW_INCLUDE_DIRS})
-sbs_target(automated-antennas-entry stm32f407vg_stm32f4discovery)
+sbs_target(automated-antennas-entry stm32f767zi_automated_antennas)
add_executable(test-automated-radio
src/entrypoints/Groundstation/Automated/test-automated-radio.cpp
diff --git a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
index 025c181a47aa4448e41ab8cb5f3b512bf5815f37..0697cbb0f0fe5d9213ba86ae30e8c7586c1e548c 100644
--- a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
+++ b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
@@ -20,38 +20,183 @@
* THE SOFTWARE.
*/
+#include <Groundstation/Automated/Actuators/Actuators.h>
+#include <Groundstation/Automated/Buses.h>
+#include <Groundstation/Automated/Follower/Follower.h>
+#include <Groundstation/Automated/Hub.h>
+#include <Groundstation/Automated/Radio/Radio.h>
+#include <Groundstation/Automated/Radio/RadioStatus.h>
+#include <Groundstation/Automated/Sensors/Sensors.h>
+#include <Groundstation/Common/Ports/Serial.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <diagnostic/PrintLogger.h>
+#include <diagnostic/StackLogger.h>
+#include <drivers/interrupt/external_interrupts.h>
#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
#include <miosix.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/ButtonHandler/ButtonHandler.h>
-#include <Groundstation/Base/Buses.h>
-#include <Groundstation/Base/Hub.h>
-#include <Groundstation/Base/Radio/Radio.h>
-#include <Groundstation/Base/Radio/RadioStatus.h>
-#include <Groundstation/Common/Ports/Serial.h>
-#include "Converter.h"
+#include <iostream>
+#include <thread>
+#include <utils/ModuleManager/ModuleManager.hpp>
-using namespace miosix;
+#include "actuators/stepper/StepperPWM.h"
+
+using namespace Groundstation;
+using namespace Antennas;
using namespace Boardcore;
+using namespace miosix;
+
+GpioPin button = GpioPin(GPIOG_BASE, 10); ///< Emergency stop button
-inline float randf() { return (std::rand() % 200 - 100) / 100.f; }
+void __attribute__((used)) EXTI10_IRQHandlerImpl()
+{
+ ModuleManager::getInstance().get<Actuators>()->IRQemergencyStop();
+}
int main()
{
- constexpr int N = 10000;
+ ModuleManager &modules = ModuleManager::getInstance();
+ PrintLogger logger = Logging::getLogger("automated_antennas");
+ bool ok = true;
+
+ button.mode(Mode::INPUT);
+ enableExternalInterrupt(button.getPort(), button.getNumber(),
+ InterruptTrigger::RISING_EDGE);
+
+ TaskScheduler *scheduler = new TaskScheduler();
+ Hub *hub = new Hub();
+ Buses *buses = new Buses();
+ Serial *serial = new Serial();
+ RadioMain *radio_main = new RadioMain();
+ RadioStatus *radio_status = new RadioStatus();
+ Actuators *actuators = new Actuators();
+ Sensors *sensors = new Sensors();
+ Follower *follower = new Follower();
+
+ // Inserting Modules
+ {
+ ok &= modules.insert(follower);
+ ok &= modules.insert<HubBase>(hub);
+ ok &= modules.insert(buses);
+ ok &= modules.insert(serial);
+ ok &= modules.insert(radio_main);
+ ok &= modules.insert(radio_status);
+ ok &= modules.insert(actuators);
+ ok &= modules.insert(sensors);
+ }
+
+ // Starting Modules
+ {
+#ifndef NO_SD_LOGGING
+ // Starting the Logger
+ if (!Logger::getInstance().start())
+ {
+ ok = false;
+ LOG_ERR(logger, "Error initializing the Logger\n");
+ }
+ else
+ {
+ LOG_INFO(logger, "Logger started successfully\n");
+ }
+#endif
+
+ // Starting scheduler
+ if (!scheduler->start())
+ {
+ ok = false;
+ LOG_ERR(logger, "Error initializing the Scheduler\n");
+ }
- printf("Starting test\n");
- uint64_t start = TimestampTimer::getTimestamp();
+ if (!serial->start())
+ {
+ ok = false;
+ LOG_ERR(logger, "Failed to start serial!\n");
+ }
- for (int i = 0; i < N; i++)
+ if (!radio_main->start())
+ {
+ ok = false;
+ LOG_ERR(logger, "Failed to start main radio!\n");
+ }
+
+ if (!radio_status->start())
+ {
+ ok = false;
+ LOG_ERR(logger, "Failed to start radio status!\n");
+ }
+
+ if (!sensors->start())
+ {
+ ok = false;
+ LOG_ERR(logger, "Failed to start sensors!\n");
+ }
+
+ actuators->start();
+
+ if (!follower->init())
+ {
+ ok = false;
+ LOG_ERR(logger, "Failed to start follower!\n");
+ }
+ }
+
+ follower->begin();
+ scheduler->addTask(std::bind(&Follower::update, follower), 100);
+
+ std::thread gpsFix(
+ [&]()
+ {
+ GPSData antennaPosition;
+ while (1)
+ {
+ VN300Data vn300Data = ModuleManager::getInstance()
+ .get<Antennas::Sensors>()
+ ->getVN300LastSample();
+
+ if (vn300Data.fix_gps != 0)
+ {
+ antennaPosition.gpsTimestamp = vn300Data.insTimestamp;
+ antennaPosition.latitude = vn300Data.latitude;
+ antennaPosition.longitude = vn300Data.longitude;
+ 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,
+ "GPS fix "
+ "acquired !coord[%f, %f] ",
+ antennaPosition.latitude,
+ antennaPosition.longitude);
+ follower->setAntennaPosition(antennaPosition);
+
+ led3On();
+ return;
+ }
+
+ Thread::sleep(1000);
+ }
+ });
+
+ if (radio_status->isMainRadioPresent())
+ {
+ led1On();
+ }
+ else
{
- 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);
+ led2On();
}
- uint64_t end = TimestampTimer::getTimestamp();
- printf("Took %llu millis for %d calls.\n", (end - start) / 1000ull, N);
+ gpsFix.join();
+
+ while (true)
+ {
+ Thread::wait();
+ }
return 0;
}