diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index 137982583afbacdfdedd8fca606b158f4d2e515c..f6da796c59041012045391121a61ac4731be1a30 100644
--- a/src/boards/Groundstation/Automated/Hub.cpp
+++ b/src/boards/Groundstation/Automated/Hub.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Davide Mor, Federico Lolli, Nicolò Caruso
+ * Authors: Davide Mor, Federico Lolli, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -25,7 +25,6 @@
#include <Groundstation/Automated/Actuators/Actuators.h>
#include <Groundstation/Automated/SMA/SMA.h>
#include <Groundstation/Common/Config/GeneralConfig.h>
-#include <Groundstation/LyraGS/BoardStatus.h>
#include <Groundstation/LyraGS/Ports/Ethernet.h>
#include <Groundstation/LyraGS/Ports/SerialLyraGS.h>
#include <Groundstation/LyraGS/Radio/Radio.h>
@@ -45,10 +44,27 @@ using namespace miosix;
void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
{
- // TODO: Dispatch to correct radio using mavlink ids
bool send_ok = false;
- LyraGS::RadioMain* radio = getModule<LyraGS::RadioMain>();
+ LyraGS::BoardStatus* status = getModule<LyraGS::BoardStatus>();
+ LyraGS::RadioMain* radioMain = getModule<LyraGS::RadioMain>();
+
+ if (status->isMainRadioPresent() && msg.sysid == MAV_SYSID_MAIN)
+ {
+ if (!radioMain->sendMsg(msg))
+ {
+ sendNack(msg, 306);
+ }
+ }
+
+ if (status->isPayloadRadioPresent() && msg.sysid == MAV_SYSID_PAYLOAD)
+ {
+ LyraGS::RadioPayload* radioPayload = getModule<LyraGS::RadioPayload>();
+ if (!radioPayload->sendMsg(msg))
+ {
+ sendNack(msg, 306);
+ }
+ }
if (msg.sysid == MAV_SYSID_ARP)
{
@@ -143,13 +159,14 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
mavlink_msg_set_rocket_coordinates_arp_tc_get_height(&msg);
GPSData gpsData;
- gpsData.latitude = latitude;
- gpsData.longitude = longitude;
- gpsData.height = height;
- gpsData.fix = 3;
- gpsData.satellites = 42;
-
- getModule<SMA>()->setInitialRocketCoordinates(gpsData);
+ gpsData.gpsTimestamp = TimestampTimer::getTimestamp();
+ gpsData.latitude = latitude;
+ gpsData.longitude = longitude;
+ gpsData.height = height;
+ gpsData.fix = 3;
+ gpsData.satellites = 42;
+
+ getModule<SMA>()->setRocketNASOrigin(gpsData);
sendAck(msg);
break;
}
@@ -165,11 +182,12 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
mavlink_msg_set_antenna_coordinates_arp_tc_get_height(&msg);
GPSData gpsData;
- gpsData.latitude = latitude;
- gpsData.longitude = longitude;
- gpsData.height = height;
- gpsData.fix = 3;
- gpsData.satellites = 42;
+ gpsData.gpsTimestamp = TimestampTimer::getTimestamp();
+ gpsData.latitude = latitude;
+ gpsData.longitude = longitude;
+ gpsData.height = height;
+ gpsData.fix = 3;
+ gpsData.satellites = 42;
getModule<SMA>()->setAntennaCoordinates(gpsData);
sendAck(msg);
@@ -229,13 +247,6 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
}
}
}
- send_ok |= radio->sendMsg(msg);
-
- // If both of the sends went wrong, just send a nack
- if (!send_ok)
- {
- sendNack(msg, 306);
- }
}
void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
@@ -267,29 +278,6 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
mavlink_msg_rocket_flight_tm_get_nas_bias_y(&msg),
mavlink_msg_rocket_flight_tm_get_nas_bias_z(&msg))};
- GPSData gpsState = getRocketOrigin();
- /** In case there is no previous correct gps data, we first set the
- * origin with the first packet with a fix >= 3 */
- if (gpsState.fix < 3)
- {
- gpsState.fix = mavlink_msg_rocket_flight_tm_get_gps_fix(&msg);
- if (gpsState.fix >= 3)
- {
- gpsState.gpsTimestamp =
- mavlink_msg_rocket_flight_tm_get_timestamp(&msg);
- gpsState.latitude =
- mavlink_msg_rocket_flight_tm_get_gps_lat(&msg);
- gpsState.longitude =
- mavlink_msg_rocket_flight_tm_get_gps_lon(&msg);
- gpsState.height =
- mavlink_msg_rocket_flight_tm_get_gps_alt(&msg);
-
- setRocketOrigin(gpsState);
-
- Logger::getInstance().log(gpsState);
- }
- }
-
// Set the rocket NAS
setRocketNasState(nasState);
@@ -303,6 +291,8 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
* take then the origin reference*/
if (gpsState.fix >= 3)
{
+ gpsState.gpsTimestamp =
+ mavlink_msg_rocket_stats_tm_get_timestamp(&msg);
gpsState.latitude = mavlink_msg_rocket_stats_tm_get_ref_lat(&msg);
gpsState.longitude = mavlink_msg_rocket_stats_tm_get_ref_lon(&msg);
gpsState.height = mavlink_msg_rocket_stats_tm_get_ref_alt(&msg);
diff --git a/src/boards/Groundstation/Automated/Hub.h b/src/boards/Groundstation/Automated/Hub.h
index 0f70716439012c960ccd16ef1a6029bc51fad28b..c27e13c67bbd80af7a8d336f444033d2af818981 100644
--- a/src/boards/Groundstation/Automated/Hub.h
+++ b/src/boards/Groundstation/Automated/Hub.h
@@ -47,7 +47,8 @@ namespace Antennas
*/
class Hub : public Boardcore::InjectableWithDeps<
Boardcore::InjectableBase<Groundstation::HubBase>, SMA,
- LyraGS::RadioMain, LyraGS::SerialLyraGS, LyraGS::EthernetGS>
+ LyraGS::RadioMain, LyraGS::RadioPayload, LyraGS::BoardStatus,
+ LyraGS::SerialLyraGS, LyraGS::EthernetGS>
{
public:
/**
diff --git a/src/boards/Groundstation/Automated/SMA/SMA.cpp b/src/boards/Groundstation/Automated/SMA/SMA.cpp
index 059735760b59e030fda4415f9a8dc717a32db583..fb91a29f954df38646eb5ae3af0aa5de2c7d0f51 100644
--- a/src/boards/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/boards/Groundstation/Automated/SMA/SMA.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Federico Lolli, Nicolò Caruso
+ * Authors: Federico Lolli, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -76,8 +76,7 @@ void SMA::setAntennaCoordinates(const Boardcore::GPSData& antennaCoordinates)
}
}
-void SMA::setInitialRocketCoordinates(
- const Boardcore::GPSData& rocketCoordinates)
+void SMA::setRocketNASOrigin(const Boardcore::GPSData& rocketCoordinates)
{
if (!testState(&SMA::state_fix_rocket) &&
!testState(&SMA::state_fix_rocket_nf))
@@ -88,7 +87,7 @@ void SMA::setInitialRocketCoordinates(
}
else
{
- follower.setInitialRocketCoordinates(rocketCoordinates);
+ follower.setRocketNASOrigin(rocketCoordinates);
EventBroker::getInstance().post(ARP_FIX_ROCKET, TOPIC_ARP);
}
}
@@ -138,6 +137,16 @@ void SMA::setFatal() { fatalInit = true; };
void SMA::update()
{
+ GPSData rocketCoordinates;
+ Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
+ rocketCoordinates = hub->getRocketOrigin();
+
+ if (rocketCoordinates.fix != 0)
+ {
+ // update follower with the rocket GPS data
+ follower.setRocketNASOrigin(rocketCoordinates);
+ }
+
switch (status.state)
{
// when in insert_info state, wait for antenna fix (manual insertion)
@@ -189,18 +198,10 @@ void SMA::update()
case SMAState::FIX_ROCKET:
case SMAState::FIX_ROCKET_NF:
{
- GPSData rocketCoordinates;
-
- Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
-
- rocketCoordinates = hub->getRocketOrigin();
if (rocketCoordinates.fix != 0)
{
- // update follower with the rocket GPS data
- follower.setInitialRocketCoordinates(rocketCoordinates);
-
LOG_INFO(Logging::getLogger("automated_antennas"),
- "Rocket GPS position acquired [{}, {}] [deg]",
+ "Rocket NAS position with fix acquired [{}, {}] [deg]",
rocketCoordinates.latitude,
rocketCoordinates.longitude);
@@ -213,7 +214,6 @@ void SMA::update()
case SMAState::ACTIVE:
{
// retrieve the last NAS Rocket state
- Hub* hub = static_cast<Hub*>(getModule<HubBase>());
if (hub->hasNasSet())
{
NASState nasState = hub->getRocketNasState();
@@ -271,7 +271,6 @@ void SMA::update()
VN300Data fakeAttitudeData;
// retrieve the last NAS Rocket state
- Hub* hub = static_cast<Hub*>(getModule<HubBase>());
if (hub->hasNasSet())
{
NASState nasState = hub->getRocketNasState();
diff --git a/src/boards/Groundstation/Automated/SMA/SMA.h b/src/boards/Groundstation/Automated/SMA/SMA.h
index e460036e8d25928d796d197da8d3e5321a4b3029..2bc290d9a150501f70d4b64d8aa3fb163cf72891 100644
--- a/src/boards/Groundstation/Automated/SMA/SMA.h
+++ b/src/boards/Groundstation/Automated/SMA/SMA.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Federico Lolli, Nicolò Caruso
+ * Authors: Federico Lolli, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -82,8 +82,7 @@ public:
* @brief Setter for the initial rocket coordinates
* @details log an error if not in the correct state
*/
- void setInitialRocketCoordinates(
- const Boardcore::GPSData& antennaCoordinates);
+ void setRocketNASOrigin(const Boardcore::GPSData& antennaCoordinates);
/**
* @brief Starts the FSM thread and adds an update function into the
diff --git a/src/entrypoints/Groundstation/lyra-gs-entry.cpp b/src/entrypoints/Groundstation/lyra-gs-entry.cpp
index aae2b97758d65807736c8d536338a4258746ab19..6e5b617156537d1ede595f48063beb1834b5d724 100644
--- a/src/entrypoints/Groundstation/lyra-gs-entry.cpp
+++ b/src/entrypoints/Groundstation/lyra-gs-entry.cpp
@@ -345,7 +345,7 @@ int main()
// Check presence of radio and ethernet
- if (board_status->isMainRadioPresent())
+ if (board_status->isMainRadioPresent() && !dipRead.isARP)
{
LOG_INFO(logger, "Main radio detected!\n");
led1On(); //< GREEN led on (CU)
@@ -353,7 +353,7 @@ int main()
else
std::cout << "Main NOT detected" << std::endl;
- if (board_status->isPayloadRadioPresent())
+ if (board_status->isPayloadRadioPresent() && !dipRead.isARP)
{
LOG_INFO(logger, "Payload radio detected!\n");
led2On(); //< YELLOW led on (CU)
@@ -361,7 +361,7 @@ int main()
else
std::cout << "Payload NOT detected" << std::endl;
- if (board_status->isEthernetPresent())
+ if (board_status->isEthernetPresent() && !dipRead.isARP)
{
LOG_INFO(logger, "Ethernet detected!\n");
led4On(); //< ORANGE led on (CU)