diff --git a/src/Groundstation/Automated/ARPData.h b/src/Groundstation/Automated/ARPData.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2b85415c3f3b56276eb83bdea6819fe12128cc7
--- /dev/null
+++ b/src/Groundstation/Automated/ARPData.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: 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
+ * 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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Antennas
+{
+
+/**
+ * @brief Structure to handle the APR coordinates, a reduced set of the GPS
+ * data. ARP is assumed fixed, therefore do not move.
+ */
+struct ARPCoordinates
+{
+ uint64_t gpsTimestamp = 0;
+ float latitude = 0; // [deg]
+ float longitude = 0; // [deg]
+ float height = 0; // [m]
+ uint8_t satellites = 0; // [1]
+ uint8_t fix = 0; // 0 = no fix
+
+ static std::string header()
+ {
+ return "timestamp,latitude,longitude,height,satellites,fix\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << gpsTimestamp << "," << latitude << "," << longitude << ","
+ << height << "," << (int)satellites << "," << (int)fix << "\n";
+ }
+};
+} // namespace Antennas
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index cb56f69e61033e8c783f165cca22e4264580bc78..c1917662ef24774d8bd9e4005c2eec162e0117d5 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -44,6 +44,7 @@ using namespace miosix;
void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
{
+ TRACE("[info] Hub: Packet arrived from outgoing messages!!!\n");
LyraGS::BoardStatus* status = getModule<LyraGS::BoardStatus>();
LyraGS::RadioMain* radioMain = getModule<LyraGS::RadioMain>();
@@ -242,11 +243,12 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
}
// In case the message is spoofed from ethernet by another groundstation
- if (msg.sysid == MAV_SYSID_MAIN)
+ if (msg.msgid == MAVLINK_MSG_ID_ROCKET_FLIGHT_TM ||
+ msg.msgid == MAVLINK_MSG_ID_ROCKET_STATS_TM)
{
TRACE(
- "[info] Hub: A MAIN packet was received from ground packet (not "
- "radio)\n");
+ "[info] Hub: A MAIN packet was received from ground packet "
+ "(ethernet probably and NOT radio)\n");
/* The message received by ethernet (outgoing) in reality is not a
* command but the telemetry spoofed, therefore is then used as incoming
*/
@@ -269,11 +271,17 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
mavlink_rocket_flight_tm_t rocketTM;
mavlink_msg_rocket_flight_tm_decode(&msg, &rocketTM);
uint64_t timestamp = mavlink_msg_rocket_flight_tm_get_timestamp(&msg);
+ TRACE(
+ "[info] Hub: A FLIGHT_ROCKET_TM packet was received from ground "
+ "packet with ts %llu\n",
+ timestamp);
/* Messages older and within the discard interval are treated as old
* messages*/
if (timestamp <= lastFlightTMTimestamp &&
lastFlightTMTimestamp > timestamp + DISCARD_MSG_DELAY)
return;
+ TRACE("[info] Hub: A FLIGHT_ROCKET_TM packet is valid with ts %llu\n",
+ timestamp);
lastFlightTMTimestamp = timestamp;
NASState nasState{
mavlink_msg_rocket_flight_tm_get_timestamp(&msg),
@@ -293,15 +301,23 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
{
mavlink_rocket_stats_tm_t rocketST;
mavlink_msg_rocket_stats_tm_decode(&msg, &rocketST);
+ TRACE(
+ "[info] Hub: A ROCKET_STAT_TM packet was received from ground "
+ "packet with ts %llu\n",
+ rocketST.timestamp);
/* Messages older and within the discard interval are treated as old
* messages*/
if (rocketST.timestamp <= lastStatsTMTimestamp &&
lastStatsTMTimestamp > rocketST.timestamp + DISCARD_MSG_DELAY)
return;
+ TRACE("[info] Hub: A ROCKET_STAT_TM packet is valid, with ts %llu\n",
+ rocketST.timestamp);
lastStatsTMTimestamp = rocketST.timestamp;
- GPSData gpsState;
- gpsState = getRocketOrigin();
+ // TODO: The origin should have its own struct since only timestamp and
+ // [lat, lon, alt] are needed
+ GPSData gpsState;
+ getRocketOrigin(gpsState);
gpsState.gpsTimestamp = rocketST.timestamp;
gpsState.latitude = rocketST.ref_lat;
gpsState.longitude = rocketST.ref_lon;
@@ -326,25 +342,28 @@ void Hub::sendAck(const mavlink_message_t& msg)
dispatchIncomingMsg(ackMsg);
}
-GPSData Hub::getRocketOrigin()
+bool Hub::getRocketOrigin(Boardcore::GPSData& rocketOrigin)
{
Lock<FastMutex> lock(coordinatesMutex);
- return lastRocketCoordinates;
+ rocketOrigin = lastRocketCoordinates;
+ return originReceived;
}
-NASState Hub::getRocketNasState()
+bool Hub::getLastRocketNasState(Boardcore::NASState& nasState)
{
Lock<FastMutex> lock(nasStateMutex);
- flagNasSet = false;
- return lastRocketNasState;
+ nasState = lastRocketNasState;
+ hasNewNasSet = false;
+ return rocketNasSet;
}
-bool Hub::hasNasSet() { return flagNasSet; }
+bool Hub::hasNewNasState() { return hasNewNasSet; }
void Hub::setRocketNasState(const NASState& newRocketNasState)
{
Lock<FastMutex> lock(nasStateMutex);
- flagNasSet = true;
+ hasNewNasSet = true;
+ rocketNasSet = true;
lastRocketNasState = newRocketNasState;
}
diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h
index 5453eac7cb8e89f3339c2c1b099a54dfd075a12f..0828b3d2203ffa8ecb8ca051d6a37873147e9a65 100644
--- a/src/Groundstation/Automated/Hub.h
+++ b/src/Groundstation/Automated/Hub.h
@@ -79,14 +79,17 @@ public:
/**
* @brief Synchronized getter for the last rocket origin for NAS.
*/
- Boardcore::GPSData getRocketOrigin();
+ bool getRocketOrigin(Boardcore::GPSData& rocketOrigin);
/**
* @brief Synchronized getter for the last rocket NAS state.
+ *
+ * @return true only if the rocket NAS state and is valid and the value is
+ * new (got from radio)
*/
- Boardcore::NASState getRocketNasState();
+ bool getLastRocketNasState(Boardcore::NASState& nasState);
- bool hasNasSet();
+ bool hasNewNasState();
private:
/**
@@ -100,10 +103,12 @@ private:
void setRocketOrigin(const Boardcore::GPSData& newRocketCoordinates);
Boardcore::GPSData lastRocketCoordinates;
+ bool originReceived = false;
Boardcore::NASState lastRocketNasState;
+ bool rocketNasSet = false;
miosix::FastMutex coordinatesMutex;
miosix::FastMutex nasStateMutex;
- bool flagNasSet = false;
+ bool hasNewNasSet = false;
uint64_t lastFlightTMTimestamp;
uint64_t lastStatsTMTimestamp;
};
diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp
index f183a9a64b728425db6bc3a3c2982159b5e80874..34ce2d8e658efefb2c7b0a1a896126d67fed2f08 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.cpp
+++ b/src/Groundstation/Common/Ports/EthernetBase.cpp
@@ -82,7 +82,10 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
WizIp ip = IP_BASE;
ip.d = 1 + ipOffset; // Add to the ip the offset set on the dipswitch
this->wiz5500->setSourceIp(ip);
- this->wiz5500->setSourceMac(genNewRandomMac());
+ WizMac mac = MAC_BASE;
+ // Add to the mac address the offset set on the dipswitch
+ mac.c += 1 + ipOffset;
+ this->wiz5500->setSourceMac(mac);
}
else
{
@@ -100,6 +103,18 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
return false;
}
+ if (sniffOtherGs)
+ {
+ TRACE("[info] starting second UDP socket (inverse direction)\n");
+ if (!this->wiz5500->openUdp(1, SEND_PORT, {255, 255, 255, 255},
+ RECV_PORT, 500))
+
+ {
+ TRACE("[error] starting second UDP socket\n");
+ return false;
+ }
+ }
+
auto mav_handler = [this](EthernetMavDriver* channel,
const mavlink_message_t& msg) { handleMsg(msg); };
@@ -119,12 +134,29 @@ void EthernetBase::handleMsg(const mavlink_message_t& msg)
ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len)
{
+ ssize_t size = 0;
WizIp dst_ip;
uint16_t dst_port;
- return wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port);
+ TRACE("Haloo\n");
+
+ if (!sniffOtherGs)
+ size = wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port);
+ else
+ // In case of sniffing, there is a maximum waiting time for messages
+ // from the groundstation software to switch between one and other port
+ size = wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port,
+ RECEIVE_PORT_TIMEOUT_MS);
+ if (size <= 0 && sniffOtherGs)
+ {
+ TRACE("Second sniff\n");
+ size = wiz5500->recvfrom(1, pkt, max_len, dst_ip, dst_port,
+ RECEIVE_PORT_TIMEOUT_MS);
+ }
+ return size;
}
bool EthernetBase::send(uint8_t* pkt, size_t len)
{
return wiz5500->send(0, pkt, len, 100);
+ // return true;
}
diff --git a/src/Groundstation/Common/Ports/EthernetBase.h b/src/Groundstation/Common/Ports/EthernetBase.h
index 5f997f6caed74ecbfaf0dc24cc7353dcd116cd52..e2c1a830a628ed5a7e4d7965ac215b341ce03518 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.h
+++ b/src/Groundstation/Common/Ports/EthernetBase.h
@@ -34,6 +34,9 @@
namespace Groundstation
{
+// Timeout for the port receive
+static constexpr uint16_t RECEIVE_PORT_TIMEOUT_MS = 200;
+
Boardcore::WizIp genNewRandomIp();
Boardcore::WizMac genNewRandomMac();
@@ -45,8 +48,8 @@ class EthernetBase : public Boardcore::Transceiver,
{
public:
EthernetBase() {};
- EthernetBase(bool randomIp, uint8_t ipOffset)
- : randomIp{randomIp}, ipOffset{ipOffset} {};
+ EthernetBase(bool randomIp, uint8_t ipOffset, bool sniffing)
+ : randomIp{randomIp}, ipOffset{ipOffset}, sniffOtherGs{sniffing} {};
void handleINTn();
@@ -56,6 +59,7 @@ public:
protected:
bool start(std::unique_ptr<Boardcore::Wiz5500> wiz5500);
+ std::unique_ptr<Boardcore::Wiz5500> wiz5500;
private:
/**
@@ -68,10 +72,10 @@ private:
bool send(uint8_t* pkt, size_t len) override;
bool started = false;
- std::unique_ptr<Boardcore::Wiz5500> wiz5500;
std::unique_ptr<EthernetMavDriver> mav_driver;
- bool randomIp = true;
- uint8_t ipOffset = 0;
+ bool randomIp = true;
+ uint8_t ipOffset = 0;
+ bool sniffOtherGs = false;
};
} // namespace Groundstation
diff --git a/src/Groundstation/LyraGS/Ports/Ethernet.h b/src/Groundstation/LyraGS/Ports/Ethernet.h
index a267a7abe1cef7bc863076659560f706ab3e3167..4899208287603a70beb44f845202a6c594a01523 100644
--- a/src/Groundstation/LyraGS/Ports/Ethernet.h
+++ b/src/Groundstation/LyraGS/Ports/Ethernet.h
@@ -36,7 +36,10 @@ class EthernetGS : public Boardcore::InjectableWithDeps<
{
public:
EthernetGS() : Super{} {}
- EthernetGS(bool randomIp, uint8_t ipOffset) : Super{randomIp, ipOffset} {}
+ EthernetGS(bool randomIp, uint8_t ipOffset, bool sniffing)
+ : Super{randomIp, ipOffset, sniffing}
+ {
+ }
[[nodiscard]] bool start();
void sendMsg(const mavlink_message_t& msg);
void handleINTn();
diff --git a/src/Groundstation/LyraGS/lyra-gs-entry.cpp b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
index 869328463a5abc32c44df6001423c7d8c5337773..26ab6211c476afe08739d64e5051cca61e677f1f 100644
--- a/src/Groundstation/LyraGS/lyra-gs-entry.cpp
+++ b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
@@ -141,7 +141,7 @@ int main()
new LyraGS::RadioMain(dipRead.mainHasBackup, dipRead.mainTXenable);
LyraGS::BoardStatus* board_status = new LyraGS::BoardStatus(dipRead.isARP);
LyraGS::EthernetGS* ethernet =
- new LyraGS::EthernetGS(false, dipRead.ipConfig);
+ new LyraGS::EthernetGS(false, dipRead.ipConfig, true);
LyraGS::RadioPayload* radio_payload = new LyraGS::RadioPayload(
dipRead.payloadHasBackup, dipRead.payloadTXenable);
@@ -343,7 +343,7 @@ int main()
// Check presence of radio and ethernet
- if (board_status->isMainRadioPresent() && !dipRead.isARP)
+ if (board_status->isMainRadioPresent())
{
LOG_INFO(logger, "Main radio detected!\n");
led1On(); //< GREEN led on (CU)
@@ -351,7 +351,7 @@ int main()
else
std::cout << "Main NOT detected" << std::endl;
- if (board_status->isPayloadRadioPresent() && !dipRead.isARP)
+ if (board_status->isPayloadRadioPresent())
{
LOG_INFO(logger, "Payload radio detected!\n");
led2On(); //< YELLOW led on (CU)
@@ -359,7 +359,7 @@ int main()
else
std::cout << "Payload NOT detected" << std::endl;
- if (board_status->isEthernetPresent() && !dipRead.isARP)
+ if (board_status->isEthernetPresent())
{
LOG_INFO(logger, "Ethernet detected!\n");
led4On(); //< ORANGE led on (CU)