From f29caa9042abac61fac395a90343972c8511ac7e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Sat, 16 Nov 2024 08:16:11 +0100
Subject: [PATCH 01/23] [ARP] Log propagated NAS and main radio info
Boardstatus: Now logging informations about the radio in MainRadioLog structure
SMA: Now logging also the NAS state
---
src/Groundstation/Automated/SMA/SMA.cpp | 2 ++
src/Groundstation/LyraGS/BoardStatus.cpp | 5 ++++
src/Groundstation/LyraGS/BoardStatus.h | 30 ++++++++++++++++++++++++
3 files changed, 37 insertions(+)
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 57b170743..24c538b50 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -236,6 +236,7 @@ void SMA::update()
AntennaAngles target = follower.getTargetAngles();
Boardcore::Logger::getInstance().log(
Boardcore::AntennaAnglesLog(target, predicted.nPropagations));
+ Boardcore::Logger::getInstance().log(predicted.getNasState());
// actuate the steppers
auto steppers = getModule<Actuators>();
@@ -301,6 +302,7 @@ void SMA::update()
AntennaAngles target = follower.getTargetAngles();
Boardcore::Logger::getInstance().log(
Boardcore::AntennaAnglesLog(target, predicted.nPropagations));
+ Boardcore::Logger::getInstance().log(predicted.getNasState());
// actuate the steppers
steppers->setSpeed(StepperList::STEPPER_X, follow.horizontalSpeed);
diff --git a/src/Groundstation/LyraGS/BoardStatus.cpp b/src/Groundstation/LyraGS/BoardStatus.cpp
index 0b7f6bb91..b65ac904b 100644
--- a/src/Groundstation/LyraGS/BoardStatus.cpp
+++ b/src/Groundstation/LyraGS/BoardStatus.cpp
@@ -108,6 +108,11 @@ void BoardStatus::arpRoutine()
tm.main_rx_rssi = stats.rx_rssi;
last_main_stats = stats;
+
+ Logger::getInstance().log(MainRadioLog{
+ tm.timestamp, tm.main_packet_tx_error_count, tm.main_tx_bitrate,
+ tm.main_packet_rx_success_count, tm.main_packet_rx_drop_count,
+ tm.main_rx_bitrate, tm.main_rx_rssi});
}
if (ethernet_present)
diff --git a/src/Groundstation/LyraGS/BoardStatus.h b/src/Groundstation/LyraGS/BoardStatus.h
index 3eb286799..4dcce99de 100644
--- a/src/Groundstation/LyraGS/BoardStatus.h
+++ b/src/Groundstation/LyraGS/BoardStatus.h
@@ -43,6 +43,36 @@ class RadioMain;
class RadioPayload;
class EthernetGS;
+/**
+ * @brief Logging struct for the main radio informations
+ *
+ */
+struct MainRadioLog
+{
+ uint64_t timestamp = 0;
+ uint16_t main_packet_tx_error_count = 0;
+ uint32_t main_tx_bitrate = 0;
+ uint16_t main_packet_rx_success_count = 0;
+ uint16_t main_packet_rx_drop_count = 0;
+ uint32_t main_rx_bitrate = 0;
+ float main_rx_rssi = 0;
+
+ static std::string header()
+ {
+ return "timestamp,main_packet_tx_error_count,main_tx_bitrate,main_"
+ "packet_rx_success_count,main_packet_rx_drop_count,main_rx_"
+ "bitrate,main_rx_rssi\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << main_packet_tx_error_count << ","
+ << main_tx_bitrate << "," << main_tx_bitrate << ","
+ << main_packet_rx_success_count << "," << main_packet_rx_drop_count
+ << "," << main_rx_bitrate << "," << main_rx_rssi << "\n";
+ }
+};
+
/**
* @brief Utility to calculate the bitrate
*/
--
GitLab
From 39a050e2cb47a66939facbe5d3593ace773ab165 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Mon, 25 Nov 2024 15:40:33 +0100
Subject: [PATCH 02/23] [ARP] Main radio log
Added Radio log structure for logging Radio-related informations for the main radio
---
scripts/logdecoder/logdecoder.cpp | 9 +++-
src/Groundstation/Automated/SMA/SMA.cpp | 2 -
src/Groundstation/LyraGS/BoardStatus.h | 31 +----------
src/Groundstation/LyraGS/Radio/RadioData.h | 62 ++++++++++++++++++++++
4 files changed, 71 insertions(+), 33 deletions(-)
create mode 100644 src/Groundstation/LyraGS/Radio/RadioData.h
diff --git a/scripts/logdecoder/logdecoder.cpp b/scripts/logdecoder/logdecoder.cpp
index 72530d5fd..58e94616c 100644
--- a/scripts/logdecoder/logdecoder.cpp
+++ b/scripts/logdecoder/logdecoder.cpp
@@ -23,6 +23,7 @@
#include <Groundstation/Automated/Actuators/ActuatorsData.h>
#include <Groundstation/Automated/PinHandler/PinData.h>
#include <Groundstation/Automated/SMA/SMAData.h>
+#include <Groundstation/LyraGS/Radio/RadioData.h>
#include <Main/PinHandler/PinData.h>
#include <Main/Sensors/SensorsData.h>
#include <Main/StateMachines/ABKController/ABKControllerData.h>
@@ -46,6 +47,7 @@
#include <RIGv2/StateMachines/TARS3/TARS3Data.h>
#include <algorithms/MEA/MEAData.h>
#include <fmt/format.h>
+#include <algorithms/Propagator/PropagatorData.h>
#include <logger/Deserializer.h>
#include <logger/LogTypes.h>
#include <logger/Logger.h>
@@ -134,9 +136,14 @@ void registerTypes(Deserializer& ds)
// Groundstation (ARP)
ds.registerType<Antennas::StepperXData>();
ds.registerType<Antennas::StepperYData>();
+ ds.registerType<VN300Data>();
+ ds.registerType<NASState>();
+ ds.registerType<PropagatorState>();
+ ds.registerType<AntennaAnglesLog>();
+ ds.registerType<GPSData>();
ds.registerType<Antennas::SMAStatus>();
ds.registerType<Antennas::PinChangeData>();
- ds.registerType<AntennaAnglesLog>();
+ ds.registerType<LyraGS::MainRadioLog>();
}
// cppcheck-suppress passedByValue
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 24c538b50..57b170743 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -236,7 +236,6 @@ void SMA::update()
AntennaAngles target = follower.getTargetAngles();
Boardcore::Logger::getInstance().log(
Boardcore::AntennaAnglesLog(target, predicted.nPropagations));
- Boardcore::Logger::getInstance().log(predicted.getNasState());
// actuate the steppers
auto steppers = getModule<Actuators>();
@@ -302,7 +301,6 @@ void SMA::update()
AntennaAngles target = follower.getTargetAngles();
Boardcore::Logger::getInstance().log(
Boardcore::AntennaAnglesLog(target, predicted.nPropagations));
- Boardcore::Logger::getInstance().log(predicted.getNasState());
// actuate the steppers
steppers->setSpeed(StepperList::STEPPER_X, follow.horizontalSpeed);
diff --git a/src/Groundstation/LyraGS/BoardStatus.h b/src/Groundstation/LyraGS/BoardStatus.h
index 4dcce99de..4d789f71a 100644
--- a/src/Groundstation/LyraGS/BoardStatus.h
+++ b/src/Groundstation/LyraGS/BoardStatus.h
@@ -32,6 +32,7 @@
#include <Groundstation/Common/Radio/RadioBase.h>
#include <Groundstation/LyraGS/Ports/Ethernet.h>
#include <Groundstation/LyraGS/Radio/Radio.h>
+#include <Groundstation/LyraGS/Radio/RadioData.h>
#include <common/MavlinkLyra.h>
#include <drivers/timer/TimestampTimer.h>
#include <utils/DependencyManager/DependencyManager.h>
@@ -43,36 +44,6 @@ class RadioMain;
class RadioPayload;
class EthernetGS;
-/**
- * @brief Logging struct for the main radio informations
- *
- */
-struct MainRadioLog
-{
- uint64_t timestamp = 0;
- uint16_t main_packet_tx_error_count = 0;
- uint32_t main_tx_bitrate = 0;
- uint16_t main_packet_rx_success_count = 0;
- uint16_t main_packet_rx_drop_count = 0;
- uint32_t main_rx_bitrate = 0;
- float main_rx_rssi = 0;
-
- static std::string header()
- {
- return "timestamp,main_packet_tx_error_count,main_tx_bitrate,main_"
- "packet_rx_success_count,main_packet_rx_drop_count,main_rx_"
- "bitrate,main_rx_rssi\n";
- }
-
- void print(std::ostream& os) const
- {
- os << timestamp << "," << main_packet_tx_error_count << ","
- << main_tx_bitrate << "," << main_tx_bitrate << ","
- << main_packet_rx_success_count << "," << main_packet_rx_drop_count
- << "," << main_rx_bitrate << "," << main_rx_rssi << "\n";
- }
-};
-
/**
* @brief Utility to calculate the bitrate
*/
diff --git a/src/Groundstation/LyraGS/Radio/RadioData.h b/src/Groundstation/LyraGS/Radio/RadioData.h
new file mode 100644
index 000000000..5edb3ce4b
--- /dev/null
+++ b/src/Groundstation/LyraGS/Radio/RadioData.h
@@ -0,0 +1,62 @@
+/* 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>
+
+/**
+ * @brief Logging struct for the main radio informations
+ *
+ */
+
+namespace LyraGS
+{
+struct MainRadioLog
+{
+ uint64_t timestamp = 0;
+ uint16_t main_packet_tx_error_count = 0;
+ uint32_t main_tx_bitrate = 0;
+ uint16_t main_packet_rx_success_count = 0;
+ uint16_t main_packet_rx_drop_count = 0;
+ uint32_t main_rx_bitrate = 0;
+ float main_rx_rssi = 0;
+
+ static std::string header()
+ {
+ return "timestamp,main_packet_tx_error_count,main_tx_bitrate,main_"
+ "packet_rx_success_count,main_packet_rx_drop_count,main_rx_"
+ "bitrate,main_rx_rssi\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << main_packet_tx_error_count << ","
+ << main_tx_bitrate << "," << main_tx_bitrate << ","
+ << main_packet_rx_success_count << "," << main_packet_rx_drop_count
+ << "," << main_rx_bitrate << "," << main_rx_rssi << "\n";
+ }
+};
+} // namespace LyraGS
--
GitLab
From 9fb28400f955cf815ec739dc1038e1a2181fc1c4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Thu, 5 Dec 2024 19:12:14 +0100
Subject: [PATCH 03/23] [ARP] SMA calibration with vn300 soft-hard iron
calibration
Now the CALIBRATION state performs a soft-hard iron calibration of the VN300.
---
src/Groundstation/Automated/SMA/SMA.cpp | 9 ++++++++
src/Groundstation/Automated/SMA/SMA.h | 1 +
.../Automated/Sensors/Sensors.cpp | 22 ++++++++++++++++++-
src/Groundstation/Automated/Sensors/Sensors.h | 22 +++++++++++++++++++
4 files changed, 53 insertions(+), 1 deletion(-)
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 57b170743..03321da8e 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -329,6 +329,12 @@ void SMA::update()
break;
}
+ case SMAState::CALIBRATE:
+ {
+ if (!sensors->isCalibrating())
+ EventBroker::getInstance().post(ARP_CAL_DONE, TOPIC_ARP);
+ }
+ break;
default:
{
break;
@@ -702,6 +708,9 @@ State SMA::state_calibrate(const Event& event)
{
case EV_ENTRY:
{
+ auto* sensors = getModule<Sensors>();
+ if (!sensors->calibrate() && sensors->isCalibrating())
+ transition(&SMA::state_test);
logStatus(SMAState::CALIBRATE);
return HANDLED;
}
diff --git a/src/Groundstation/Automated/SMA/SMA.h b/src/Groundstation/Automated/SMA/SMA.h
index 2bc290d9a..d86934ac0 100644
--- a/src/Groundstation/Automated/SMA/SMA.h
+++ b/src/Groundstation/Automated/SMA/SMA.h
@@ -97,6 +97,7 @@ public:
/**
* @brief move the stepper `stepperId` of `angle` degrees
+ *
*/
ActuationStatus moveStepperDeg(StepperList stepperId, float angle);
diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp
index 866231a74..315756771 100644
--- a/src/Groundstation/Automated/Sensors/Sensors.cpp
+++ b/src/Groundstation/Automated/Sensors/Sensors.cpp
@@ -63,9 +63,29 @@ bool Sensors::vn300Init()
return true;
}
+bool Sensors::calibrate()
+{
+ // Already in calibration mode.
+ if (calibrating)
+ return false;
+ calibrationStart = std::chrono::nanoseconds(miosix::getTime());
+ calibrating = true;
+ vn300->startHSIEstimator(VN300_CAL_CONVERGENCE);
+ return true;
+}
+
+bool Sensors::isCalibrating() { return calibrating; }
+
void Sensors::vn300Callback()
{
- Logger::getInstance().log(vn300->getLastSample());
+ if (calibrating)
+ {
+ if (calibrationStart - std::chrono::nanoseconds(miosix::getTime()) >
+ VN300_CAL_TIME)
+ vn300->stopHSIEstimator();
+ }
+ else
+ Logger::getInstance().log(vn300->getLastSample());
}
VN300Data Sensors::getVN300LastSample() { return vn300->getLastSample(); }
diff --git a/src/Groundstation/Automated/Sensors/Sensors.h b/src/Groundstation/Automated/Sensors/Sensors.h
index 87ce9a237..43d06d76b 100644
--- a/src/Groundstation/Automated/Sensors/Sensors.h
+++ b/src/Groundstation/Automated/Sensors/Sensors.h
@@ -21,14 +21,23 @@
*/
#pragma once
+#include <drivers/timer/TimestampTimer.h>
#include <utils/DependencyManager/DependencyManager.h>
+#include <chrono>
+
#include "Groundstation/LyraGS/Buses.h"
#include "sensors/SensorManager.h"
#include "sensors/Vectornav/VN300/VN300.h"
namespace Antennas
{
+
+static constexpr uint8_t VN300_CAL_CONVERGENCE =
+ 4; ///< Calibration convergence parameter for VN300 soft and hard iron
+ ///< calibration. 5: converge in 60-90sec, 1: converge in 15-20sec
+static constexpr std::chrono::seconds VN300_CAL_TIME = std::chrono::seconds(30);
+
class Sensors : public Boardcore::InjectableWithDeps<LyraGS::Buses>
{
public:
@@ -44,14 +53,27 @@ public:
*/
Boardcore::VN300Data getVN300LastSample();
+ /**
+ * @brief Trigger the calibration process for soft-hard iron in the VN300
+ */
+ bool calibrate();
+
+ /**
+ * @brief Returns the status of the calibration
+ */
+ bool isCalibrating();
+
private:
bool vn300Init();
void vn300Callback();
+ std::atomic<bool> calibrating{false};
+
Boardcore::VN300* vn300 = nullptr;
Boardcore::SensorManager* sm = nullptr;
Boardcore::SensorManager::SensorMap_t sensorsMap;
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
+ std::chrono::nanoseconds calibrationStart;
};
} // namespace Antennas
--
GitLab
From 11fa2fcb479fe22d2048e0ce7c7578b107ce78a0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Mon, 9 Dec 2024 14:30:02 +0100
Subject: [PATCH 04/23] [ARP] Fix on calibration time for VN300 soft-hard iron
---
src/Groundstation/Automated/SMA/SMA.cpp | 3 +++
src/Groundstation/Automated/Sensors/Sensors.cpp | 6 +++++-
2 files changed, 8 insertions(+), 1 deletion(-)
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 03321da8e..cf800147d 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -332,7 +332,10 @@ void SMA::update()
case SMAState::CALIBRATE:
{
if (!sensors->isCalibrating())
+ {
EventBroker::getInstance().post(ARP_CAL_DONE, TOPIC_ARP);
+ LOG_DEBUG(logger, "Exit from calibration\n");
+ }
}
break;
default:
diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp
index 315756771..0fe17cf13 100644
--- a/src/Groundstation/Automated/Sensors/Sensors.cpp
+++ b/src/Groundstation/Automated/Sensors/Sensors.cpp
@@ -80,9 +80,13 @@ void Sensors::vn300Callback()
{
if (calibrating)
{
- if (calibrationStart - std::chrono::nanoseconds(miosix::getTime()) >
+ LOG_DEBUG(logger, "Sensor Manager VN300 calibrating\n");
+ if (std::chrono::nanoseconds(miosix::getTime()) - calibrationStart >
VN300_CAL_TIME)
+ {
vn300->stopHSIEstimator();
+ calibrating = false;
+ }
}
else
Logger::getInstance().log(vn300->getLastSample());
--
GitLab
From 38aa310981ad006f85548a9b29356af464eeb495 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Mon, 16 Dec 2024 08:31:01 +0100
Subject: [PATCH 05/23] [ARP] Ethernet spoofing for rocket messages
Now the Hub do take into consideration the messages as MAIN for spoofing.
If received from ethernet is treated as if received from the radio.
Also, the messages are also considered if older than 10seconds, to avoid discarding packets if the main has been reset.
---
src/Groundstation/Automated/Hub.cpp | 26 ++++++++++++++++++++++++++
src/Groundstation/Automated/Hub.h | 9 +++++++++
2 files changed, 35 insertions(+)
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index adc96f2c6..cb56f69e6 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -60,6 +60,7 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
sendNack(msg, 306);
}
+ // Message for ARP
if (msg.sysid == MAV_SYSID_ARP)
{
switch (msg.msgid)
@@ -239,6 +240,18 @@ 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)
+ {
+ TRACE(
+ "[info] Hub: A MAIN packet was received from ground packet (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
+ */
+ dispatchIncomingMsg(msg);
+ }
}
void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
@@ -255,6 +268,13 @@ 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);
+ /* Messages older and within the discard interval are treated as old
+ * messages*/
+ if (timestamp <= lastFlightTMTimestamp &&
+ lastFlightTMTimestamp > timestamp + DISCARD_MSG_DELAY)
+ return;
+ lastFlightTMTimestamp = timestamp;
NASState nasState{
mavlink_msg_rocket_flight_tm_get_timestamp(&msg),
Eigen::Matrix<float, 13, 1>(
@@ -273,6 +293,12 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
{
mavlink_rocket_stats_tm_t rocketST;
mavlink_msg_rocket_stats_tm_decode(&msg, &rocketST);
+ /* Messages older and within the discard interval are treated as old
+ * messages*/
+ if (rocketST.timestamp <= lastStatsTMTimestamp &&
+ lastStatsTMTimestamp > rocketST.timestamp + DISCARD_MSG_DELAY)
+ return;
+ lastStatsTMTimestamp = rocketST.timestamp;
GPSData gpsState;
gpsState = getRocketOrigin();
diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h
index 9130dd058..5453eac7c 100644
--- a/src/Groundstation/Automated/Hub.h
+++ b/src/Groundstation/Automated/Hub.h
@@ -42,6 +42,13 @@ class BoardStatus;
namespace Antennas
{
+/* This is used to avoid discarding messages in case the rocket timestamp is
+ * reset. Therefore if older than the discard msg delay, resets the messages
+ * last timestamp */
+static constexpr uint64_t DISCARD_MSG_DELAY =
+ 10 * 1000000; ///< Maximum time for which the message, if older is
+ ///< discarded. [micros]
+
/**
* @brief Central hub connecting all outgoing and ingoing modules.
*/
@@ -97,6 +104,8 @@ private:
miosix::FastMutex coordinatesMutex;
miosix::FastMutex nasStateMutex;
bool flagNasSet = false;
+ uint64_t lastFlightTMTimestamp;
+ uint64_t lastStatsTMTimestamp;
};
} // namespace Antennas
--
GitLab
From 4d50817d944a8d0e4f9e2d3e9e5b3acc165bf3b8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Mon, 16 Dec 2024 14:17:01 +0100
Subject: [PATCH 06/23] [ARP] Removed old test entrypoints
Too old and not used anymore
---
.../Automated/test-actuators.cpp | 269 ------------------
.../Automated/test-automated-radio.cpp | 102 -------
.../Automated/test-smcontroller.cpp | 205 -------------
src/Groundstation/Automated/test-steps.cpp | 236 ---------------
4 files changed, 812 deletions(-)
delete mode 100644 src/Groundstation/Automated/test-actuators.cpp
delete mode 100644 src/Groundstation/Automated/test-automated-radio.cpp
delete mode 100644 src/Groundstation/Automated/test-smcontroller.cpp
delete mode 100644 src/Groundstation/Automated/test-steps.cpp
diff --git a/src/Groundstation/Automated/test-actuators.cpp b/src/Groundstation/Automated/test-actuators.cpp
deleted file mode 100644
index a92629b15..000000000
--- a/src/Groundstation/Automated/test-actuators.cpp
+++ /dev/null
@@ -1,269 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Niccolò Betto
- *
- * 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.
- */
-
-#include <Groundstation/Automated/Actuators/Actuators.h>
-#include <Groundstation/Automated/Hub.h>
-#include <Groundstation/Automated/Sensors/Sensors.h>
-#include <Groundstation/Common/Ports/Serial.h>
-#include <Groundstation/LyraGS/Buses.h>
-#include <diagnostic/PrintLogger.h>
-#include <drivers/interrupt/external_interrupts.h>
-#include <miosix.h>
-#include <scheduler/TaskScheduler.h>
-#include <utils/ButtonHandler/ButtonHandler.h>
-
-#define STEPPER_SPEED 0.25
-#define TEST_WAIT 5000
-#define NO_SD_LOGGING
-
-#define START_MODULE(name, lambda) \
- do \
- { \
- std::function<bool()> _fun = lambda; \
- if (!_fun()) \
- { \
- LOG_ERR(logger, "Failed to start module " name); \
- errorLoop(); \
- } \
- else \
- { \
- LOG_DEBUG(logger, "Successfully started module " name); \
- } \
- } while (0)
-
-using namespace Groundstation;
-using namespace Antennas;
-using namespace Boardcore;
-using namespace miosix;
-
-void ledWaitLoop(int ms)
-{
- int waited = 0;
- while (waited < ms)
- {
- led2On();
- Thread::sleep(100);
- led2Off();
- Thread::sleep(100);
- waited += 200;
- }
-}
-
-void errorLoop()
-{
- while (1)
- {
- userLed4::high();
- Thread::sleep(100);
- userLed4::low();
- Thread::sleep(100);
- }
-}
-
-void test1(Actuators* actuators)
-{
- PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")};
- LOG_INFO(logger, "Executing Test 1");
-
- // theta1 : HORIZONTAL
- // theta2 : VERTICAL
- LOG_DEBUG(logger, "Setting speed to speed\n");
- actuators->setSpeed(StepperList::STEPPER_X, STEPPER_SPEED);
- actuators->setSpeed(StepperList::STEPPER_Y, STEPPER_SPEED);
-
- LOG_DEBUG(logger, "Moving 90deg horizontally\n");
- actuators->moveDeg(StepperList::STEPPER_X, 90);
- ledWaitLoop(TEST_WAIT);
-
- LOG_DEBUG(logger, "Moving back to the initial horizontal position\n");
- actuators->moveDeg(StepperList::STEPPER_X, -90);
- ledWaitLoop(TEST_WAIT);
-
- LOG_DEBUG(logger, "Moving -90deg horizontally\n");
- actuators->moveDeg(StepperList::STEPPER_X, -90);
- ledWaitLoop(TEST_WAIT);
-
- LOG_DEBUG(logger, "Moving back to the initial horizontal position\n");
- actuators->moveDeg(StepperList::STEPPER_X, 90);
- ledWaitLoop(TEST_WAIT);
-
- LOG_INFO(logger, "Test 1 completed\n");
-}
-
-void test2(Actuators* actuators)
-{
- PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")};
- LOG_INFO(logger, "Executing Test 2");
-
- actuators->setSpeed(StepperList::STEPPER_X, STEPPER_SPEED);
- actuators->setSpeed(StepperList::STEPPER_Y, STEPPER_SPEED);
- actuators->moveDeg(StepperList::STEPPER_Y, 22.5);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_X, 90);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_X, -90);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_X, -90);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_X, 90);
- ledWaitLoop(TEST_WAIT);
-
- // Return to (0,0)
- actuators->moveDeg(StepperList::STEPPER_Y, -22.5);
- ledWaitLoop(TEST_WAIT);
-
- LOG_INFO(logger, "Test 2 completed\n");
-}
-
-void test3(Actuators* actuators)
-{
- actuators->setSpeed(StepperList::STEPPER_X, STEPPER_SPEED);
- actuators->setSpeed(StepperList::STEPPER_Y, STEPPER_SPEED);
-
- actuators->moveDeg(StepperList::STEPPER_X, 180);
- ledWaitLoop(TEST_WAIT + 4000);
- actuators->moveDeg(StepperList::STEPPER_X, -180);
- ledWaitLoop(TEST_WAIT + 4000);
-
- actuators->moveDeg(StepperList::STEPPER_Y, 90);
- ledWaitLoop(TEST_WAIT);
- actuators->moveDeg(StepperList::STEPPER_Y, -90);
- ledWaitLoop(TEST_WAIT);
-}
-
-void test6(Actuators* actuators)
-{
- PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")};
- LOG_INFO(logger, "Executing Test 6");
-
- actuators->setSpeed(StepperList::STEPPER_X, STEPPER_SPEED);
- actuators->setSpeed(StepperList::STEPPER_Y, STEPPER_SPEED);
-
- actuators->moveDeg(StepperList::STEPPER_Y, 22.5);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_Y, -22.5);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_Y, 45);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_Y, -45);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_Y, 65.5);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_Y, -65.5);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_Y, 90);
- ledWaitLoop(TEST_WAIT);
-
- actuators->moveDeg(StepperList::STEPPER_Y, -90);
- ledWaitLoop(TEST_WAIT);
-
- LOG_INFO(logger, "Test 6 completed\n");
-}
-
-int main()
-{
- ledOff();
-
- Hub* hub = new Hub();
- LyraGS::Buses* buses = new LyraGS::Buses();
- Serial* serial = new Serial();
- Actuators* actuators = new Actuators();
- Sensors* sensors = new Sensors();
-
- DependencyManager manager;
- PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")};
- bool ok = true;
-
- LOG_INFO(logger, "test-actuators\n");
-
- // Insert modules
- {
- ok &= manager.insert<HubBase>(hub);
- ok &= manager.insert(buses);
- ok &= manager.insert(serial);
- ok &= manager.insert(actuators);
- ok &= manager.insert(sensors);
-
- // If insertion failed, stop right here
- if (!ok)
- {
- LOG_ERR(logger, "Failed to insert all modules!\n");
- errorLoop();
- }
- else
- {
- LOG_DEBUG(logger, "All modules inserted successfully!\n");
- }
-
- if (!manager.inject())
- {
- LOG_ERR(logger, "[error] Failed to inject the dependencies!\n");
- errorLoop();
- }
- else
- {
- LOG_DEBUG(logger, "All modules injected successfully!\n");
- }
- }
-
- // Start modules
- {
-#ifndef NO_SD_LOGGING
- START_MODULE("Logger", [&] { return Logger::getInstance().start(); });
-#endif
- START_MODULE("Serial", [&] { return serial->start(); });
- START_MODULE("Sensors", [&] { return sensors->start(); });
-
- actuators->start();
- LOG_DEBUG(logger, "Actuators started successfully!\n");
- }
-
- // Setup success LED
- led1On();
- LOG_INFO(logger, "Modules setup successful");
-
- LOG_INFO(logger, "Starting tests\n");
-
- actuators->arm();
-
- test1(actuators);
- test2(actuators);
- test3(actuators);
- test6(actuators);
-
- LOG_INFO(logger, "Tests completed\n");
-
- led1Off();
- while (1)
- Thread::sleep(1000);
- return 0;
-}
diff --git a/src/Groundstation/Automated/test-automated-radio.cpp b/src/Groundstation/Automated/test-automated-radio.cpp
deleted file mode 100644
index c33b71cdd..000000000
--- a/src/Groundstation/Automated/test-automated-radio.cpp
+++ /dev/null
@@ -1,102 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Davide Mor, Emilio Corigliano
- *
- * 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.
- */
-
-#include <Groundstation/Automated/Buses.h>
-#include <Groundstation/Automated/Hub.h>
-#include <Groundstation/Automated/Radio/Radio.h>
-#include <Groundstation/Common/Ports/Serial.h>
-#include <Groundstation/LyraGS/BoardStatus.h>
-#include <miosix.h>
-
-using namespace Groundstation;
-using namespace Antennas;
-using namespace Boardcore;
-using namespace miosix;
-
-void spinLoop()
-{
- while (1)
- Thread::sleep(1000);
-}
-
-void errorLoop()
-{
- while (1)
- {
- led1On();
- Thread::sleep(100);
- led1Off();
- Thread::sleep(100);
- }
-}
-
-int main()
-{
- ledOff();
-
- Hub* hub = new Hub();
- Buses* buses = new Buses();
- Serial* serial = new Serial();
- RadioMain* radio_main = new RadioMain();
- BoardStatus* board_status = new BoardStatus();
-
- ModuleManager& modules = ModuleManager::getInstance();
-
- bool ok = true;
-
- ok &= modules.insert<HubBase>(hub);
- ok &= modules.insert(buses);
- ok &= modules.insert(serial);
- ok &= modules.insert(radio_main);
- ok &= modules.insert(board_status);
-
- // If insertion failed, stop right here
- if (!ok)
- {
- printf("[error] Failed to insert all modules!\n");
- errorLoop();
- }
-
- // Ok now start them
-
- ok &= serial->start();
- if (!ok)
- printf("[error] Failed to start serial!\n");
-
- ok &= radio_main->start();
- if (!ok)
- printf("[error] Failed to start main radio!\n");
-
- ok &= board_status->start();
- if (!ok)
- printf("[error] Failed to start radio status!\n");
-
- if (board_status->isMainRadioPresent())
- led2On();
-
- if (!ok)
- errorLoop();
-
- led1On();
- spinLoop();
- return 0;
-}
diff --git a/src/Groundstation/Automated/test-smcontroller.cpp b/src/Groundstation/Automated/test-smcontroller.cpp
deleted file mode 100644
index a77712da3..000000000
--- a/src/Groundstation/Automated/test-smcontroller.cpp
+++ /dev/null
@@ -1,205 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Federico Lolli
- *
- * 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.
- */
-
-#include <Groundstation/Automated/SMA/SMA.h>
-#include <common/Events.h>
-#include <common/Topics.h>
-#include <events/EventBroker.h>
-#include <utils/Debug.h>
-
-#include <memory>
-#include <unordered_set>
-
-#define ARP_EVENTS \
- {ARP_INIT_OK, ARP_INIT_ERROR, ARP_CAL_DONE, ARP_FIX_ANTENNAS, \
- ARP_FIX_ROCKET}
-
-#define TMTC_EVENTS \
- {TMTC_ARP_FORCE_INIT, \
- TMTC_ARP_RESET_ALGORITHM, \
- TMTC_ARP_FORCE_NO_FEEDBACK, \
- TMTC_ARP_ARM, \
- TMTC_ARP_DISARM, \
- TMTC_ARP_CALIBRATE, \
- TMTC_ARP_ENTER_TEST_MODE, \
- TMTC_ARP_EXIT_TEST_MODE}
-
-#define TEST_STATE(INITIAL_STATE, EVENT, TOPIC_SM, FINAL_STATE) \
- sm->forceState(&SMA::INITIAL_STATE); \
- EventBroker::getInstance().post(EVENT, TOPIC_SM); \
- Thread::sleep(20); \
- TRACE("Testing %-26s in " #INITIAL_STATE " -> " #FINAL_STATE " ... ", \
- getEventString(EVENT).c_str()); \
- correct = sm->testState(&SMA::FINAL_STATE); \
- printf(correct ? "OK\n" : "FAIL\n"); \
- ok &= correct;
-
-#define TEST_NO_TRANSITION(INITIAL_STATE, EVENT, TOPIC_SM) \
- sm->forceState(&SMA::INITIAL_STATE); \
- EventBroker::getInstance().post(EVENT, TOPIC_SM); \
- Thread::sleep(20); \
- TRACE("Testing %-26s in " #INITIAL_STATE " -X ... ", \
- getEventString(EVENT).c_str()); \
- correct = sm->testState(&SMA::INITIAL_STATE); \
- printf(correct ? "OK\n" : "FAIL\n"); \
- ok &= correct;
-
-#define TEST_ALL_OTHER(INITIAL_STATE, ...) \
- to_exclude = {__VA_ARGS__}; \
- for (Events ev : arp_events) \
- { \
- if (to_exclude.find(ev) == to_exclude.end()) \
- { \
- TEST_NO_TRANSITION(INITIAL_STATE, ev, TOPIC_ARP); \
- } \
- } \
- for (Events ev : tmtc_events) \
- { \
- if (to_exclude.find(ev) == to_exclude.end()) \
- { \
- TEST_NO_TRANSITION(INITIAL_STATE, ev, TOPIC_TMTC); \
- } \
- }
-
-// for (auto state : {__VA_ARGS__})
-// {
-
-using namespace Boardcore;
-using namespace Common;
-using namespace Antennas;
-
-SMA* sm = new SMA();
-
-int main()
-{
- bool correct, ok = true;
- std::unordered_set<Events> to_exclude;
- std::vector<Events> arp_events = ARP_EVENTS;
- std::vector<Events> tmtc_events = TMTC_EVENTS;
-
- sm->start();
- TRACE("SMA started\n");
-
- // TEST STATE: INIT
- TEST_STATE(state_init, ARP_INIT_OK, TOPIC_ARP, state_init_done);
- TEST_STATE(state_init, ARP_INIT_ERROR, TOPIC_ARP, state_init_error);
- TEST_ALL_OTHER(state_init, ARP_INIT_OK, ARP_INIT_ERROR);
-
- // TEST STATE: INIT_DONE
- TEST_STATE(state_init_done, TMTC_ARP_ARM, TOPIC_TMTC, state_armed);
- TEST_STATE(state_init_done, TMTC_ARP_FORCE_NO_FEEDBACK, TOPIC_TMTC,
- state_insert_info);
- TEST_ALL_OTHER(state_init_done, TMTC_ARP_ARM, TMTC_ARP_FORCE_NO_FEEDBACK);
-
- // TEST STATE: INIT_ERROR
- TEST_STATE(state_init_error, TMTC_ARP_FORCE_INIT, TOPIC_TMTC,
- state_init_done);
- TEST_STATE(state_init_error, TMTC_ARP_FORCE_NO_FEEDBACK, TOPIC_TMTC,
- state_insert_info);
- TEST_ALL_OTHER(state_init_error, TMTC_ARP_FORCE_INIT,
- TMTC_ARP_FORCE_NO_FEEDBACK);
-
- // TEST STATE: INSERT_INFO
- TEST_STATE(state_insert_info, ARP_FIX_ANTENNAS, TOPIC_ARP, state_arm_ready);
- TEST_ALL_OTHER(state_insert_info, ARP_FIX_ANTENNAS);
-
- // TEST STATE: ARM READY
- TEST_STATE(state_arm_ready, TMTC_ARP_ARM, TOPIC_TMTC, state_armed_nf);
- TEST_ALL_OTHER(state_arm_ready, TMTC_ARP_ARM);
-
- // TEST STATE: ARMED
- TEST_STATE(state_armed, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done);
- TEST_STATE(state_armed, TMTC_ARP_CALIBRATE, TOPIC_TMTC, state_calibrate);
- TEST_STATE(state_armed, TMTC_ARP_ENTER_TEST_MODE, TOPIC_TMTC, state_test);
- TEST_ALL_OTHER(state_armed, TMTC_ARP_DISARM, TMTC_ARP_CALIBRATE,
- TMTC_ARP_ENTER_TEST_MODE);
-
- // TEST STATE: TEST
- TEST_STATE(state_test, TMTC_ARP_EXIT_TEST_MODE, TOPIC_TMTC, state_armed);
- TEST_STATE(state_test, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done);
- TEST_ALL_OTHER(state_test, TMTC_ARP_EXIT_TEST_MODE, TMTC_ARP_DISARM);
-
- // TEST STATE: CALIBRATE
- TEST_STATE(state_calibrate, ARP_CAL_DONE, TOPIC_ARP, state_fix_antennas);
- TEST_STATE(state_calibrate, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done);
- TEST_STATE(state_calibrate, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC,
- state_armed);
- TEST_ALL_OTHER(state_calibrate, ARP_CAL_DONE, TMTC_ARP_DISARM,
- TMTC_ARP_RESET_ALGORITHM);
-
- // TEST STATE: FIX_ANTENNAS
- TEST_STATE(state_fix_antennas, ARP_FIX_ANTENNAS, TOPIC_ARP,
- state_fix_rocket);
- TEST_STATE(state_fix_antennas, TMTC_ARP_DISARM, TOPIC_TMTC,
- state_init_done);
- TEST_STATE(state_fix_antennas, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC,
- state_armed);
- TEST_ALL_OTHER(state_fix_antennas, ARP_FIX_ANTENNAS, TMTC_ARP_DISARM,
- TMTC_ARP_RESET_ALGORITHM);
-
- // TEST STATE: FIX_ROCKET
- TEST_STATE(state_fix_rocket, ARP_FIX_ROCKET, TOPIC_ARP, state_active);
- TEST_STATE(state_fix_rocket, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done);
- TEST_STATE(state_fix_rocket, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC,
- state_armed);
- TEST_ALL_OTHER(state_fix_rocket, ARP_FIX_ROCKET, TMTC_ARP_DISARM,
- TMTC_ARP_RESET_ALGORITHM);
-
- // TEST STATE: ACTIVE
- TEST_STATE(state_active, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done);
- TEST_STATE(state_active, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC, state_armed);
- TEST_ALL_OTHER(state_active, TMTC_ARP_DISARM, TMTC_ARP_RESET_ALGORITHM);
-
- // TEST STATE: ARMED_NO_FEEDBACK
- TEST_STATE(state_armed_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_arm_ready);
- TEST_STATE(state_armed_nf, TMTC_ARP_CALIBRATE, TOPIC_TMTC,
- state_fix_rocket_nf);
- TEST_STATE(state_armed_nf, TMTC_ARP_ENTER_TEST_MODE, TOPIC_TMTC,
- state_test_nf);
- TEST_ALL_OTHER(state_armed_nf, TMTC_ARP_DISARM, TMTC_ARP_CALIBRATE,
- TMTC_ARP_ENTER_TEST_MODE);
-
- // TEST STATE: TEST_NO_FEEDBACK
- TEST_STATE(state_test_nf, TMTC_ARP_EXIT_TEST_MODE, TOPIC_TMTC,
- state_armed_nf);
- TEST_STATE(state_test_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_arm_ready);
- TEST_ALL_OTHER(state_test_nf, TMTC_ARP_EXIT_TEST_MODE, TMTC_ARP_DISARM);
-
- // TEST STATE: FIX_ROCKET_NO_FEEDBACK
- TEST_STATE(state_fix_rocket_nf, ARP_FIX_ROCKET, TOPIC_ARP, state_active_nf);
- TEST_STATE(state_fix_rocket_nf, TMTC_ARP_DISARM, TOPIC_TMTC,
- state_arm_ready);
- TEST_STATE(state_fix_rocket_nf, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC,
- state_armed_nf);
- TEST_ALL_OTHER(state_fix_rocket_nf, ARP_FIX_ROCKET, TMTC_ARP_DISARM,
- TMTC_ARP_RESET_ALGORITHM);
-
- // TEST STATE: ACTIVE_NO_FEEDBACK
- TEST_STATE(state_active_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_arm_ready);
- TEST_STATE(state_active_nf, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC,
- state_armed_nf);
- TEST_ALL_OTHER(state_active_nf, TMTC_ARP_DISARM, TMTC_ARP_RESET_ALGORITHM);
-
- TRACE("Testing SMA ... ");
- ok ? printf("OK\n") : printf("FAIL\n");
- return 0;
-}
diff --git a/src/Groundstation/Automated/test-steps.cpp b/src/Groundstation/Automated/test-steps.cpp
deleted file mode 100644
index 7a69211c5..000000000
--- a/src/Groundstation/Automated/test-steps.cpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Emilio Corigliano
- *
- * 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.
- */
-
-#include <Groundstation/Automated/Actuators/Actuators.h>
-#include <Groundstation/Automated/Buses.h>
-#include <Groundstation/Automated/Sensors/Sensors.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 <iostream>
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-#include "actuators/stepper/StepperPWM.h"
-
-using namespace miosix;
-using namespace Boardcore;
-using namespace Antennas;
-
-/**
- * Test used to characterize the automated antennas.
- */
-
-GpioPin button = GpioPin(GPIOC_BASE, 13);
-
-void __attribute__((used)) EXTI13_IRQHandlerImpl()
-{
- ModuleManager::getInstance().get<Actuators>()->IRQemergencyStop();
-}
-
-int main()
-{
- bool initResult = true;
- PrintLogger logger = Logging::getLogger("automated_antennas");
- ModuleManager& modules = ModuleManager::getInstance();
- TaskScheduler* scheduler = new TaskScheduler();
-
- button.mode(Mode::INPUT);
- enableExternalInterrupt(button.getPort(), button.getNumber(),
- InterruptTrigger::RISING_EDGE);
-
- // INSERTING MODULES
- {
- if (!modules.insert<Buses>(new Buses()))
- {
- initResult = false;
- LOG_ERR(logger, "Error inserting the Buses module");
- }
- else
- {
- LOG_INFO(logger, "Successfully inserted Buses module\n");
- }
-
- if (!modules.insert<Actuators>(new Actuators()))
- {
- initResult = false;
- LOG_ERR(logger, "Error inserting the Actuators module");
- }
- else
- {
- LOG_INFO(logger, "Successfully inserted Actuators module\n");
- }
-
- if (!modules.insert<Sensors>(new Sensors()))
- {
- initResult = false;
- LOG_ERR(logger, "Error inserting the Sensors module");
- }
- else
- {
- LOG_INFO(logger, "Successfully inserted Sensors module\n");
- }
-
- // Insert algorithm modules
- }
-
- // ADDING TASKS
- {
- scheduler->addTask(
- []()
- {
- Logger::getInstance().log(CpuMeter::getCpuStats());
- CpuMeter::resetCpuStats();
- Logger::getInstance().logStats();
- StackLogger::getInstance().log();
- },
- 100);
- }
-
- // STARTING MODULES
- {
-#ifndef NO_SD_LOGGING
- // Starting the Logger
- if (!Logger::getInstance().start())
- {
- initResult = false;
- LOG_ERR(logger, "Error initializing the Logger\n");
- }
- else
- {
- LOG_INFO(logger, "Logger started successfully\n");
- }
-#endif
-
- // Starting scheduler
- if (!scheduler->start())
- {
- initResult = false;
- LOG_ERR(logger, "Error initializing the Scheduler\n");
- }
- else
- {
- LOG_INFO(logger, "Scheduler started successfully\n");
- }
-
- // Starting the Actuators
- modules.get<Actuators>()->start();
-
- // Starting the Sensors
- if (!modules.get<Sensors>()->start())
- {
- initResult = false;
- LOG_ERR(logger, "Error initializing the Sensors\n");
- }
- else
- {
- LOG_INFO(logger, "Sensors started successfully\n");
- }
- }
-
- if (!initResult)
- {
- printf("Errors present during module insertion\n");
- return -1;
- }
-
- // Periodically statistics
- float speed0 = 0.5;
- float speed = speed0;
-
- modules.get<Actuators>()->setSpeed(StepperList::STEPPER_X, speed);
-
- modules.get<Actuators>()->setSpeed(StepperList::STEPPER_Y, speed);
-
- // bool increasing = true;
- // float speedMax = 1.0;
- // float stepSpeed = 0.1;
- // scheduler->addTask(
- // [&]()
- // {
- // if (increasing)
- // {
- // if (speed < speedMax)
- // {
- // speed += stepSpeed;
- // modules.get<Actuators>()->setSpeed(
- // StepperList::STEPPER_X, speed);
- // }
- // else
- // {
- // increasing = false;
- // }
- // }
- // else
- // {
- // if (speed > 0)
- // {
- // speed -= stepSpeed;
- // modules.get<Actuators>()->setSpeed(
- // StepperList::STEPPER_X, speed);
- // }
- // else
- // {
- // increasing = true;
- // }
- // }
- // },
- // 100);
- VN300Data data;
- while (!ModuleManager::getInstance().get<Actuators>()->isEmergencyStopped())
- {
- {
- data = modules.get<Sensors>()->getVN300LastSample();
- printf("acc[%.3f,%.3f,%.3f] ypr[%.3f,%.3f,%.3f]\n",
- data.accelerationX, data.accelerationY, data.accelerationZ,
- data.yaw, data.pitch, data.roll);
- modules.get<Actuators>()->moveDeg(StepperList::STEPPER_X, 90);
- modules.get<Actuators>()->moveDeg(StepperList::STEPPER_Y, 90);
- Thread::sleep(1500);
- }
-
- {
- data = modules.get<Sensors>()->getVN300LastSample();
- printf("acc[%.3f,%.3f,%.3f] ypr[%.3f,%.3f,%.3f]\n",
- data.accelerationX, data.accelerationY, data.accelerationZ,
- data.yaw, data.pitch, data.roll);
- modules.get<Actuators>()->moveDeg(StepperList::STEPPER_X, -90);
- modules.get<Actuators>()->moveDeg(StepperList::STEPPER_Y, -90);
- Thread::sleep(1500);
- }
- }
-
- // Stopping threads
- {
- Logger::getInstance().stop();
- printf("Logger stopped! Board can be reset/shutdown9\n");
- }
-
- while (1)
- ;
-}
--
GitLab
From 858ea1223eca421b44ab4147bd109d9d35474be0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Tue, 14 Jan 2025 12:36:50 +0100
Subject: [PATCH 07/23] [ARP] Ethernet sniffing
Implemented ethernet sniffing functionality.
Changed MAC address assignement since there could have been clashes on the MAC address disallowing ethernet sniffing functionalities
Now ROCKET_STATS_TM and ROCKET_FLIGHT_TM are mangaged even if received from ethernet sniffing
---
src/Groundstation/Automated/Hub.cpp | 43 +++++++++++++------
src/Groundstation/Automated/Hub.h | 13 ++++--
.../Common/Ports/EthernetBase.cpp | 36 +++++++++++++++-
src/Groundstation/Common/Ports/EthernetBase.h | 14 +++---
src/Groundstation/LyraGS/Ports/Ethernet.h | 5 ++-
src/Groundstation/LyraGS/lyra-gs-entry.cpp | 8 ++--
6 files changed, 91 insertions(+), 28 deletions(-)
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index cb56f69e6..c1917662e 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 5453eac7c..0828b3d22 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 f183a9a64..34ce2d8e6 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 5f997f6ca..e2c1a830a 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 a267a7abe..489920828 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 869328463..26ab6211c 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)
--
GitLab
From 7e1565554e5154c1bdae21f8d1aac7b134bebfc4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Tue, 14 Jan 2025 12:39:11 +0100
Subject: [PATCH 08/23] [ARP] SMA rework for reduce update and data checks
The active feedback and no feedback now are togheter managed in the update.
Also now the data is acquired once for VN300 or from stepper.
Also changes on the logic of the Hub and Follower, since now there are checks on the validity of NAS and ORIGIN elements which should have been received at least once
---
src/Groundstation/Automated/SMA/SMA.cpp | 131 +++++++-----------------
src/Groundstation/Automated/SMA/SMA.h | 1 +
2 files changed, 36 insertions(+), 96 deletions(-)
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index cf800147d..cbf1c5461 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -138,41 +138,45 @@ void SMA::setFatal() { fatalInit = true; };
void SMA::update()
{
GPSData rocketCoordinates, antennaCoordinates;
- VN300Data vn300Data;
+ VN300Data data;
+ auto steppers = getModule<Actuators>();
- Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
- auto* sensors = getModule<Sensors>();
- rocketCoordinates = hub->getRocketOrigin();
+ Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
+ auto* sensors = getModule<Sensors>();
- // Update the antenna position except in case of no feedback
- if (status.state != SMAState::FIX_ROCKET_NF &&
- status.state != SMAState::INIT_ERROR &&
- status.state != SMAState::ACTIVE_NF &&
- status.state != SMAState::ARM_READY &&
- status.state != SMAState::ACTIVE_NF)
+ // Update the antenna position if in feedback
+ if (status.state == SMAState::FEEDBACK)
{
// update antenna coordinates
- vn300Data = sensors->getVN300LastSample();
- if (vn300Data.gpsFix == 3)
+ data = sensors->getVN300LastSample();
+ if (data.gpsFix == 3)
{
// build the GPSData struct with the VN300 data
- antennaCoordinates.gpsTimestamp = vn300Data.insTimestamp;
- antennaCoordinates.latitude = vn300Data.latitude;
- antennaCoordinates.longitude = vn300Data.longitude;
- antennaCoordinates.height = vn300Data.altitude;
- antennaCoordinates.velocityNorth = vn300Data.velocityX;
- antennaCoordinates.velocityEast = vn300Data.velocityY;
- antennaCoordinates.velocityDown = vn300Data.velocityZ;
- antennaCoordinates.satellites = vn300Data.gpsFix;
- antennaCoordinates.fix = vn300Data.gpsFix;
+ antennaCoordinates.gpsTimestamp = data.insTimestamp;
+ antennaCoordinates.latitude = data.latitude;
+ antennaCoordinates.longitude = data.longitude;
+ antennaCoordinates.height = data.altitude;
+ antennaCoordinates.velocityNorth = data.velocityX;
+ antennaCoordinates.velocityEast = data.velocityY;
+ antennaCoordinates.velocityDown = data.velocityZ;
+ antennaCoordinates.satellites = data.gpsFix;
+ antennaCoordinates.fix = data.gpsFix;
// update follower with coordinates
follower.setAntennaCoordinates(antennaCoordinates);
}
}
+ else
+ {
+ // Fake attitude data, taking the stepper current positions as pitch and
+ // yaw
+ data.pitch = steppers->getCurrentDegPosition(StepperList::STEPPER_Y);
+ data.yaw = steppers->getCurrentDegPosition(StepperList::STEPPER_X);
+ }
// update follower with the rocket GPS data
- follower.setRocketNASOrigin(rocketCoordinates);
+ if (hub->getRocketOrigin(rocketCoordinates))
+ follower.setRocketNASOrigin(rocketCoordinates);
switch (status.state)
{
@@ -211,89 +215,26 @@ void SMA::update()
}
break;
}
+
// in active state, update the follower and propagator inner states
case SMAState::ACTIVE:
- {
- // retrieve the last NAS Rocket state
- if (hub->hasNasSet())
- {
- NASState nasState = hub->getRocketNasState();
-
- // update the propagator with the NAS state
- // and retrieve the propagated state
- propagator.setRocketNasState(nasState);
- }
- propagator.update(); // step the propagator
- PropagatorState predicted = propagator.getState();
-
- // update the follower with the propagated state
- follower.setLastRocketNasState(predicted.getNasState());
- follower.setLastAntennaAttitude(vn300Data);
- follower.update(); // step the follower
- FollowerState follow = follower.getState();
-
- // Log the target angles and propagations info
- AntennaAngles target = follower.getTargetAngles();
- Boardcore::Logger::getInstance().log(
- Boardcore::AntennaAnglesLog(target, predicted.nPropagations));
-
- // actuate the steppers
- auto steppers = getModule<Actuators>();
- steppers->setSpeed(StepperList::STEPPER_X, follow.horizontalSpeed);
- steppers->setSpeed(StepperList::STEPPER_Y, follow.verticalSpeed);
-
- ActuationStatus actuation =
- steppers->moveDeg(StepperList::STEPPER_X, follow.yaw);
- if (actuation != ActuationStatus::OK)
- {
- LOG_ERR(
- logger,
- "Step antenna - STEPPER_X could not move or reached move "
- "limit. Error: ",
- actuation, "\n");
- }
-
- actuation = steppers->moveDeg(StepperList::STEPPER_Y, follow.pitch);
- if (actuation != ActuationStatus::OK)
- {
- LOG_ERR(
- logger,
- "Step antenna - STEPPER_Y could not move or reached move "
- "limit. Error: ",
- actuation, "\n");
- }
-
- break;
- }
case SMAState::ACTIVE_NF:
{
- VN300Data fakeAttitudeData;
-
// retrieve the last NAS Rocket state
- if (hub->hasNasSet())
- {
- NASState nasState = hub->getRocketNasState();
+
+ NASState nasState;
+ if (hub->hasNewNasState() && hub->getLastRocketNasState(nasState))
// update the propagator with the NAS state
// and retrieve the propagated state
propagator.setRocketNasState(nasState);
- }
+
propagator.update(); // step the propagator
PropagatorState predicted = propagator.getState();
- auto steppers = getModule<Actuators>();
-
- // set the attitude as the current position of the steppers
- // FIXME this method of setting the attitude is too dirty
- // if the follower is updated something may break here
- fakeAttitudeData.pitch =
- steppers->getCurrentDegPosition(StepperList::STEPPER_Y);
- fakeAttitudeData.yaw =
- steppers->getCurrentDegPosition(StepperList::STEPPER_X);
-
// update the follower with the propagated state
follower.setLastRocketNasState(predicted.getNasState());
- follower.setLastAntennaAttitude(fakeAttitudeData);
+ follower.setLastAntennaAttitude(data);
follower.update(); // step the follower
FollowerState follow = follower.getState();
@@ -326,9 +267,9 @@ void SMA::update()
"limit. Error: ",
actuation, "\n");
}
-
- break;
}
+ break;
+
case SMAState::CALIBRATE:
{
if (!sensors->isCalibrating())
@@ -339,9 +280,7 @@ void SMA::update()
}
break;
default:
- {
break;
- }
}
}
@@ -795,7 +734,7 @@ State SMA::state_fix_rocket(const Event& event)
// init the follower before leaving the state
// (compute initial arp-rocket distance and bearing)
if (!follower.init())
- LOG_ERR(logger, "Follower initialization failed");
+ LOG_ERR(logger, "Follower initialization failed\n");
leds->setOn(LedColor::YELLOW);
return HANDLED;
diff --git a/src/Groundstation/Automated/SMA/SMA.h b/src/Groundstation/Automated/SMA/SMA.h
index d86934ac0..b73b2c66a 100644
--- a/src/Groundstation/Automated/SMA/SMA.h
+++ b/src/Groundstation/Automated/SMA/SMA.h
@@ -109,6 +109,7 @@ public:
/**
* @brief Setter for the multipliers of the steppers
*/
+ // TODO: See if has sense to remove it...
void setMultipliers(StepperList axis, float multiplier);
/**
--
GitLab
From 1288aff1162feeba0d31c095232b88369a9507ec Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Tue, 14 Jan 2025 14:17:56 +0100
Subject: [PATCH 09/23] [ARP] Increased time for receiving the packets in
Ethernet
Increased time for packet receival since a lot of them were lost
---
src/Groundstation/Common/Ports/EthernetBase.cpp | 3 ---
src/Groundstation/Common/Ports/EthernetBase.h | 2 +-
2 files changed, 1 insertion(+), 4 deletions(-)
diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp
index 34ce2d8e6..52b2e982a 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.cpp
+++ b/src/Groundstation/Common/Ports/EthernetBase.cpp
@@ -137,8 +137,6 @@ ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len)
ssize_t size = 0;
WizIp dst_ip;
uint16_t dst_port;
- TRACE("Haloo\n");
-
if (!sniffOtherGs)
size = wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port);
else
@@ -148,7 +146,6 @@ ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len)
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);
}
diff --git a/src/Groundstation/Common/Ports/EthernetBase.h b/src/Groundstation/Common/Ports/EthernetBase.h
index e2c1a830a..82adc3c1a 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.h
+++ b/src/Groundstation/Common/Ports/EthernetBase.h
@@ -35,7 +35,7 @@ namespace Groundstation
{
// Timeout for the port receive
-static constexpr uint16_t RECEIVE_PORT_TIMEOUT_MS = 200;
+static constexpr uint16_t RECEIVE_PORT_TIMEOUT_MS = 500;
Boardcore::WizIp genNewRandomIp();
Boardcore::WizMac genNewRandomMac();
--
GitLab
From 22bb6e3f08d51a269bda29615b2d56e6d5cf0596 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Fri, 17 Jan 2025 16:42:12 +0100
Subject: [PATCH 10/23] [ARP] SMA calibrate movements, Hub mutex for having
NAS, Cmake removed test-actuators
Calibrate state now can have movements by the setStepperAngle commands, not only the tests.
---
CMakeLists.txt | 7 -------
src/Groundstation/Automated/Hub.cpp | 7 ++++++-
src/Groundstation/Automated/SMA/SMA.cpp | 6 ++++--
3 files changed, 10 insertions(+), 10 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 56712bb3a..670a5267d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -103,13 +103,6 @@ add_executable(nokia-groundstation-entry
target_include_directories(nokia-groundstation-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(nokia-groundstation-entry stm32f429zi_nokia)
-add_executable(test-actuators
- src/Groundstation/Automated/test-actuators.cpp
- ${LYRA_GS} ${GROUNDSTATION_COMMON}
-)
-target_include_directories(test-actuators PRIVATE ${OBSW_INCLUDE_DIRS})
-sbs_target(test-actuators stm32f767zi_lyra_gs)
-
add_executable(lyra-gs-entry
src/Groundstation/LyraGS/lyra-gs-entry.cpp
${LYRA_GS} ${GROUNDSTATION_COMMON}
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index c1917662e..523568364 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -357,7 +357,11 @@ bool Hub::getLastRocketNasState(Boardcore::NASState& nasState)
return rocketNasSet;
}
-bool Hub::hasNewNasState() { return hasNewNasSet; }
+bool Hub::hasNewNasState()
+{
+ Lock<FastMutex> lock(nasStateMutex);
+ return hasNewNasSet;
+}
void Hub::setRocketNasState(const NASState& newRocketNasState)
{
@@ -371,4 +375,5 @@ void Hub::setRocketOrigin(const GPSData& newRocketCoordinates)
{
Lock<FastMutex> lock(coordinatesMutex);
lastRocketCoordinates = newRocketCoordinates;
+ originReceived = true;
}
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index cbf1c5461..3470e27db 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -94,7 +94,8 @@ void SMA::setRocketNASOrigin(const Boardcore::GPSData& rocketCoordinates)
ActuationStatus SMA::moveStepperDeg(StepperList stepperId, float angle)
{
- if (!testState(&SMA::state_test) && !testState(&SMA::state_test_nf))
+ if (!testState(&SMA::state_test) && !testState(&SMA::state_test_nf) &&
+ !testState(&SMA::state_calibrate))
{
LOG_ERR(logger, "Stepper can only be manually moved in the TEST state");
return ActuationStatus::NOT_TEST;
@@ -199,6 +200,7 @@ void SMA::update()
}
break;
}
+ // TODO: See if semantically what we want
// in fix_rocket state, wait for the GPS fix of the rocket
case SMAState::FIX_ROCKET:
case SMAState::FIX_ROCKET_NF:
@@ -223,8 +225,8 @@ void SMA::update()
// retrieve the last NAS Rocket state
NASState nasState;
+ // In case there is a new NAS packet
if (hub->hasNewNasState() && hub->getLastRocketNasState(nasState))
-
// update the propagator with the NAS state
// and retrieve the propagated state
propagator.setRocketNasState(nasState);
--
GitLab
From 4270eafd7a62c705c3eb7935364c817b514c7937 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Fri, 24 Jan 2025 14:53:26 +0100
Subject: [PATCH 11/23] [ARP] Fix on SMA for feedback states check
Using only feedback macrostate made not enter in the feedback data reception part
---
cmake/dependencies.cmake | 1 +
src/Groundstation/Automated/Hub.h | 6 +++---
src/Groundstation/Automated/SMA/SMA.cpp | 7 ++++++-
3 files changed, 10 insertions(+), 4 deletions(-)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 2f96687df..16ba69168 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -45,6 +45,7 @@ set(MAIN_COMPUTER
set(GROUNDSTATION_COMMON
src/Groundstation/Common/Radio/RadioBase.cpp
src/Groundstation/Common/Ports/EthernetBase.cpp
+ src/Groundstation/Common/Ports/EthernetSniffer.cpp
src/Groundstation/Common/Ports/Serial.cpp
src/Groundstation/Common/HubBase.cpp
)
diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h
index 0828b3d22..0c537945b 100644
--- a/src/Groundstation/Automated/Hub.h
+++ b/src/Groundstation/Automated/Hub.h
@@ -108,9 +108,9 @@ private:
bool rocketNasSet = false;
miosix::FastMutex coordinatesMutex;
miosix::FastMutex nasStateMutex;
- bool hasNewNasSet = false;
- uint64_t lastFlightTMTimestamp;
- uint64_t lastStatsTMTimestamp;
+ bool hasNewNasSet = false;
+ uint64_t lastFlightTMTimestamp = 0;
+ uint64_t lastStatsTMTimestamp = 0;
};
} // namespace Antennas
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 3470e27db..9fa53f0d1 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -145,8 +145,13 @@ void SMA::update()
Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
auto* sensors = getModule<Sensors>();
+ // TODO: Verify if same with the macrostate
// Update the antenna position if in feedback
- if (status.state == SMAState::FEEDBACK)
+ // if (testState(&SMA::state_feedback))
+ if (testState(&SMA::state_init_done) || testState(&SMA::state_armed) ||
+ testState(&SMA::state_fix_antennas) ||
+ testState(&SMA::state_fix_rocket) || testState(&SMA::state_active) ||
+ testState(&SMA::state_test))
{
// update antenna coordinates
data = sensors->getVN300LastSample();
--
GitLab
From 734c7a9b4c46e6f9189509b5a70eb133235949f1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Fri, 24 Jan 2025 14:54:48 +0100
Subject: [PATCH 12/23] [ARP] Sniffing class EthernetSniffer and changes to
support it
Now the sniffing is done by the EthernetSniffing class
This only receives from the port that EthernetBase use as destination port
To allow this a new class with its own thread should be done.
Also, EthernetSniffing needed a shared Wiz pointer from the EthernetBase class
Lyra-gs-entry: Needed to be changed to the new interface which requires the sniffing boolean parameter
---
.../Common/Ports/EthernetBase.cpp | 45 ++++----
src/Groundstation/Common/Ports/EthernetBase.h | 15 +--
.../Common/Ports/EthernetSniffer.cpp | 101 ++++++++++++++++++
.../Common/Ports/EthernetSniffer.h | 71 ++++++++++++
src/Groundstation/LyraGS/Ports/Ethernet.cpp | 4 +-
src/Groundstation/LyraGS/lyra-gs-entry.cpp | 6 +-
6 files changed, 206 insertions(+), 36 deletions(-)
create mode 100644 src/Groundstation/Common/Ports/EthernetSniffer.cpp
create mode 100644 src/Groundstation/Common/Ports/EthernetSniffer.h
diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp
index 52b2e982a..e66be196a 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.cpp
+++ b/src/Groundstation/Common/Ports/EthernetBase.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Davide Mor
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, 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
@@ -65,7 +65,7 @@ Boardcore::Wiz5500::PhyState EthernetBase::getState()
return wiz5500->getPhyState();
}
-bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
+bool EthernetBase::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
{
this->wiz5500 = std::move(wiz5500);
@@ -103,16 +103,10 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
return false;
}
- if (sniffOtherGs)
+ if (!this->wiz5500->openUdp(1, SEND_PORT, {255, 255, 255, 255}, RECV_PORT,
+ 500))
{
- 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;
- }
+ return false;
}
auto mav_handler = [this](EthernetMavDriver* channel,
@@ -122,7 +116,17 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
if (!mav_driver->start())
return false;
+ TRACE("[info] mavlink driver started correctly\n");
+ // Create and start a second mavlink driver to sniff the ethernet port
+ if (sniffOtherGs)
+ {
+ getModule<EthernetSniffer>()->init(1, RECV_PORT, SEND_PORT);
+ if (!getModule<EthernetSniffer>()->start(wiz5500))
+ return false;
+ }
+
+ TRACE("[info] Ethernet sniffing started correctly\n");
return true;
}
@@ -134,26 +138,13 @@ 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;
- 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)
- {
- size = wiz5500->recvfrom(1, pkt, max_len, dst_ip, dst_port,
- RECEIVE_PORT_TIMEOUT_MS);
- }
- return size;
+ return wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port);
}
bool EthernetBase::send(uint8_t* pkt, size_t len)
{
- return wiz5500->send(0, pkt, len, 100);
+ return wiz5500->send(0, pkt, len, 1000);
// return true;
}
diff --git a/src/Groundstation/Common/Ports/EthernetBase.h b/src/Groundstation/Common/Ports/EthernetBase.h
index 82adc3c1a..57e6622e5 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.h
+++ b/src/Groundstation/Common/Ports/EthernetBase.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Davide Mor
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, 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
@@ -24,6 +24,7 @@
#include <ActiveObject.h>
#include <Groundstation/Common/HubBase.h>
+#include <Groundstation/Common/Ports/EthernetSniffer.h>
#include <common/MavlinkLyra.h>
#include <drivers/WIZ5500/WIZ5500.h>
#include <radio/MavlinkDriver/MavlinkDriver.h>
@@ -43,8 +44,9 @@ Boardcore::WizMac genNewRandomMac();
using EthernetMavDriver =
Boardcore::MavlinkDriver<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
-class EthernetBase : public Boardcore::Transceiver,
- public Boardcore::InjectableWithDeps<HubBase>
+class EthernetBase
+ : public Boardcore::Transceiver,
+ public Boardcore::InjectableWithDeps<HubBase, EthernetSniffer>
{
public:
EthernetBase() {};
@@ -58,8 +60,8 @@ public:
Boardcore::Wiz5500::PhyState getState();
protected:
- bool start(std::unique_ptr<Boardcore::Wiz5500> wiz5500);
- std::unique_ptr<Boardcore::Wiz5500> wiz5500;
+ bool start(std::shared_ptr<Boardcore::Wiz5500> wiz5500);
+ std::shared_ptr<Boardcore::Wiz5500> wiz5500;
private:
/**
@@ -76,6 +78,7 @@ private:
bool randomIp = true;
uint8_t ipOffset = 0;
bool sniffOtherGs = false;
+ bool firstPort = true;
};
} // namespace Groundstation
diff --git a/src/Groundstation/Common/Ports/EthernetSniffer.cpp b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
new file mode 100644
index 000000000..8b6be2b7f
--- /dev/null
+++ b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
@@ -0,0 +1,101 @@
+/* Copyright (c) 2025 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.
+ */
+
+#include <Groundstation/Common/Config/EthernetConfig.h>
+#include <Groundstation/Common/HubBase.h>
+
+#include <random>
+
+#include "EthernetBase.h"
+
+using namespace Groundstation;
+using namespace Boardcore;
+using namespace miosix;
+
+void EthernetSniffer::handleINTn()
+{
+ if (wiz5500)
+ wiz5500->handleINTn();
+}
+
+bool EthernetSniffer::send(uint8_t* pkt, size_t len)
+{
+ // Send is not needed in sniffing, therefore not implemented
+ return false;
+};
+
+Boardcore::Wiz5500::PhyState EthernetSniffer::getState()
+{
+ return wiz5500->getPhyState();
+}
+
+void EthernetSniffer::init(uint16_t portNumber, uint16_t srcPort,
+ uint16_t dstPort)
+{
+ portNr = portNumber;
+ srcPort = srcPort;
+ dstPort = dstPort;
+}
+
+bool EthernetSniffer::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
+{
+ TRACE("Movin\n");
+ this->wiz5500 = std::move(wiz5500);
+
+ TRACE("Opening\n");
+ // We open the port for sniffing using the port we specified
+ // if (!this->wiz5500->openUdp(portNr, srcPort, {255, 255, 255, 255},
+ // dstPort,
+ // 500))
+ // {
+ // return false;
+ // }
+
+ TRACE("Mavlinker\n");
+
+ auto mav_handler = [this](EthernetMavDriver* channel,
+ const mavlink_message_t& msg) { handleMsg(msg); };
+
+ mav_driver = std::make_unique<EthernetMavDriver>(this, mav_handler, 1, 10);
+
+ TRACE("Starting\n");
+
+ if (!mav_driver->start())
+ return false;
+
+ TRACE("Start ok\n");
+
+ return true;
+}
+
+void EthernetSniffer::handleMsg(const mavlink_message_t& msg)
+{
+ // Dispatch the message through the hub.
+ getModule<HubBase>()->dispatchOutgoingMsg(msg);
+}
+
+ssize_t EthernetSniffer::receive(uint8_t* pkt, size_t max_len)
+{
+ WizIp dst_ip;
+ uint16_t dst_port;
+ return wiz5500->recvfrom(portNr, pkt, max_len, dst_ip, dst_port);
+}
diff --git a/src/Groundstation/Common/Ports/EthernetSniffer.h b/src/Groundstation/Common/Ports/EthernetSniffer.h
new file mode 100644
index 000000000..7f4ea7a74
--- /dev/null
+++ b/src/Groundstation/Common/Ports/EthernetSniffer.h
@@ -0,0 +1,71 @@
+/* Copyright (c) 2025 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 <ActiveObject.h>
+#include <Groundstation/Common/HubBase.h>
+#include <common/MavlinkLyra.h>
+#include <drivers/WIZ5500/WIZ5500.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include <memory>
+
+namespace Groundstation
+{
+
+using EthernetMavDriver =
+ Boardcore::MavlinkDriver<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
+
+class EthernetSniffer : public Boardcore::Transceiver,
+ public Boardcore::InjectableWithDeps<HubBase>
+{
+public:
+ void handleINTn();
+
+ Boardcore::Wiz5500::PhyState getState();
+
+ bool start(std::shared_ptr<Boardcore::Wiz5500> wiz5500);
+
+ void init(uint16_t portNumber, uint16_t srcPort, uint16_t dstPort);
+
+private:
+ std::shared_ptr<Boardcore::Wiz5500> wiz5500;
+
+ /**
+ * @brief Called internally when a message is received.
+ */
+ void handleMsg(const mavlink_message_t& msg);
+
+ ssize_t receive(uint8_t* pkt, size_t max_len) override;
+
+ bool send(uint8_t* pkt, size_t len) override;
+
+ bool started = false;
+ std::unique_ptr<EthernetMavDriver> mav_driver;
+ uint16_t portNr;
+ uint16_t srcPort;
+ uint16_t dstPort;
+};
+
+} // namespace Groundstation
diff --git a/src/Groundstation/LyraGS/Ports/Ethernet.cpp b/src/Groundstation/LyraGS/Ports/Ethernet.cpp
index 6b3e3270f..6013c835a 100644
--- a/src/Groundstation/LyraGS/Ports/Ethernet.cpp
+++ b/src/Groundstation/LyraGS/Ports/Ethernet.cpp
@@ -45,7 +45,7 @@ namespace LyraGS
bool EthernetGS::start()
{
- std::unique_ptr<Wiz5500> wiz5500 = std::make_unique<Wiz5500>(
+ std::shared_ptr<Wiz5500> wiz5500 = std::make_shared<Wiz5500>(
getModule<Buses>()->ethernet_bus, miosix::ethernet::cs::getPin(),
miosix::ethernet::intr::getPin(), SPI::ClockDivider::DIV_64);
@@ -55,7 +55,7 @@ bool EthernetGS::start()
if (!present)
return false;
- if (!EthernetBase::start(std::move(wiz5500)))
+ if (!EthernetBase::start(wiz5500))
return false;
ethernetGSGlobal = this;
diff --git a/src/Groundstation/LyraGS/lyra-gs-entry.cpp b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
index 26ab6211c..29a0fdef2 100644
--- a/src/Groundstation/LyraGS/lyra-gs-entry.cpp
+++ b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
@@ -93,6 +93,8 @@ void errorLoop()
}
}
+static bool constexpr ethernetSniffing = true;
+
/**
* @brief Lyra GS entrypoint.
* This entrypoint performs the following operations:
@@ -141,7 +143,8 @@ 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, true);
+ new LyraGS::EthernetGS(false, dipRead.ipConfig, ethernetSniffing);
+ EthernetSniffer* ethernetSniffer = new EthernetSniffer();
LyraGS::RadioPayload* radio_payload = new LyraGS::RadioPayload(
dipRead.payloadHasBackup, dipRead.payloadTXenable);
@@ -160,6 +163,7 @@ int main()
ok &= manager.insert(serial);
ok &= manager.insert<LyraGS::RadioMain>(radio_main);
ok &= manager.insert<LyraGS::EthernetGS>(ethernet);
+ ok &= manager.insert<EthernetSniffer>(ethernetSniffer);
ok &= manager.insert<LyraGS::RadioPayload>(radio_payload);
ok &= manager.insert(board_status);
--
GitLab
From 9a5b376a9c285e5477b3ad52ab315a76ec0c7e79 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Sat, 8 Feb 2025 10:58:20 +0100
Subject: [PATCH 13/23] [ARP] EthernetSniffing fix due to deferenced shared
pointer
Fix hardfault: The ethernet sniffer by using the std::move for the Wiz5500 was deferencing the pointer and threfore in the recvfrom an HardFault occured.
Debug: Removed and changed some TRACE strings
Fix UDP socket: The sniffing socket was initialized opend correctly but then the init had switched arguments. This surely impacted on the recvfrom.
---
.../Common/Ports/EthernetBase.cpp | 15 +++--------
.../Common/Ports/EthernetSniffer.cpp | 26 +++++++------------
2 files changed, 14 insertions(+), 27 deletions(-)
diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp
index e66be196a..dbae92339 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.cpp
+++ b/src/Groundstation/Common/Ports/EthernetBase.cpp
@@ -67,7 +67,7 @@ Boardcore::Wiz5500::PhyState EthernetBase::getState()
bool EthernetBase::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
{
- this->wiz5500 = std::move(wiz5500);
+ this->wiz5500 = wiz5500;
// Reset the device
this->wiz5500->reset();
@@ -103,12 +103,6 @@ bool EthernetBase::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
return false;
}
- if (!this->wiz5500->openUdp(1, SEND_PORT, {255, 255, 255, 255}, RECV_PORT,
- 500))
- {
- return false;
- }
-
auto mav_handler = [this](EthernetMavDriver* channel,
const mavlink_message_t& msg) { handleMsg(msg); };
@@ -116,17 +110,16 @@ bool EthernetBase::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
if (!mav_driver->start())
return false;
- TRACE("[info] mavlink driver started correctly\n");
- // Create and start a second mavlink driver to sniff the ethernet port
+ // In case of sniffing mode initialize and start the EthernetSniffer
if (sniffOtherGs)
{
- getModule<EthernetSniffer>()->init(1, RECV_PORT, SEND_PORT);
+ getModule<EthernetSniffer>()->init(1, SEND_PORT, RECV_PORT);
if (!getModule<EthernetSniffer>()->start(wiz5500))
return false;
}
- TRACE("[info] Ethernet sniffing started correctly\n");
+ TRACE("[info] Ethernet module started correctly\n");
return true;
}
diff --git a/src/Groundstation/Common/Ports/EthernetSniffer.cpp b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
index 8b6be2b7f..564024f89 100644
--- a/src/Groundstation/Common/Ports/EthernetSniffer.cpp
+++ b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
@@ -58,31 +58,25 @@ void EthernetSniffer::init(uint16_t portNumber, uint16_t srcPort,
bool EthernetSniffer::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
{
- TRACE("Movin\n");
- this->wiz5500 = std::move(wiz5500);
+ this->wiz5500 = wiz5500;
- TRACE("Opening\n");
- // We open the port for sniffing using the port we specified
- // if (!this->wiz5500->openUdp(portNr, srcPort, {255, 255, 255, 255},
- // dstPort,
- // 500))
- // {
- // return false;
- // }
-
- TRACE("Mavlinker\n");
+ TRACE("[info] Opening sniffing UDP socket\n");
+ // We open the UDP socket for sniffing
+ if (!this->wiz5500->openUdp(1, SEND_PORT, {255, 255, 255, 255}, RECV_PORT,
+ 500))
+ {
+ return false;
+ }
auto mav_handler = [this](EthernetMavDriver* channel,
const mavlink_message_t& msg) { handleMsg(msg); };
- mav_driver = std::make_unique<EthernetMavDriver>(this, mav_handler, 1, 10);
-
- TRACE("Starting\n");
+ mav_driver = std::make_unique<EthernetMavDriver>(this, mav_handler, 0, 10);
if (!mav_driver->start())
return false;
- TRACE("Start ok\n");
+ TRACE("[info] EthernetSniffer start ok\n");
return true;
}
--
GitLab
From cdd8602b22535195b4f9d4a006e250bfc22bac82 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Thu, 6 Mar 2025 16:45:30 +0100
Subject: [PATCH 14/23] [ARP] Better Hub debug printing, EthernetSniffer not
hardcoded
Hub: Now better debug printing for the sniffing to see from which source the messages arrive
EthernetBase: Changed MAC initialization to avoid switch filtering such messages
EthernetSniffer: No more hardcoded ports
---
src/Groundstation/Automated/Hub.cpp | 20 ++++++++++++-------
.../Common/Ports/EthernetBase.cpp | 13 +++++++++++-
.../Common/Ports/EthernetSniffer.cpp | 8 ++++----
3 files changed, 29 insertions(+), 12 deletions(-)
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index 523568364..f77e4bcaf 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -247,7 +247,8 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
msg.msgid == MAVLINK_MSG_ID_ROCKET_STATS_TM)
{
TRACE(
- "[info] Hub: A MAIN packet was received from ground packet "
+ "[info][SNIFFING] 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
@@ -272,7 +273,8 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
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 "
+ "[info][Radio/Sniffing] Hub: A FLIGHT_ROCKET_TM packet was "
+ "received "
"packet with ts %llu\n",
timestamp);
/* Messages older and within the discard interval are treated as old
@@ -280,8 +282,10 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
if (timestamp <= lastFlightTMTimestamp &&
lastFlightTMTimestamp > timestamp + DISCARD_MSG_DELAY)
return;
- TRACE("[info] Hub: A FLIGHT_ROCKET_TM packet is valid with ts %llu\n",
- timestamp);
+ TRACE(
+ "[info][Radio/Sniffing] 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),
@@ -302,7 +306,7 @@ 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 "
+ "[info][Radio/Sniffing] Hub: A ROCKET_STAT_TM packet was received "
"packet with ts %llu\n",
rocketST.timestamp);
/* Messages older and within the discard interval are treated as old
@@ -310,8 +314,10 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
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);
+ TRACE(
+ "[info][Radio/Sniffing] Hub: A ROCKET_STAT_TM packet is valid, "
+ "with ts %llu\n",
+ rocketST.timestamp);
lastStatsTMTimestamp = rocketST.timestamp;
// TODO: The origin should have its own struct since only timestamp and
diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp
index dbae92339..76e66d89a 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.cpp
+++ b/src/Groundstation/Common/Ports/EthernetBase.cpp
@@ -85,6 +85,17 @@ bool EthernetBase::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
WizMac mac = MAC_BASE;
// Add to the mac address the offset set on the dipswitch
mac.c += 1 + ipOffset;
+ // In case of sniffing change ulteriorly the MAC to avoid switch to
+ // filter based on not whole MAC...
+ if (sniffOtherGs)
+ {
+ mac.a += 1;
+ mac.b += 1;
+ mac.c += 1;
+ mac.d += 1;
+ mac.e += 1;
+ mac.f += 1;
+ }
this->wiz5500->setSourceMac(mac);
}
else
@@ -115,7 +126,7 @@ bool EthernetBase::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
if (sniffOtherGs)
{
getModule<EthernetSniffer>()->init(1, SEND_PORT, RECV_PORT);
- if (!getModule<EthernetSniffer>()->start(wiz5500))
+ if (!getModule<EthernetSniffer>()->start(this->wiz5500))
return false;
}
diff --git a/src/Groundstation/Common/Ports/EthernetSniffer.cpp b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
index 564024f89..e08e1c538 100644
--- a/src/Groundstation/Common/Ports/EthernetSniffer.cpp
+++ b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
@@ -51,9 +51,9 @@ Boardcore::Wiz5500::PhyState EthernetSniffer::getState()
void EthernetSniffer::init(uint16_t portNumber, uint16_t srcPort,
uint16_t dstPort)
{
- portNr = portNumber;
- srcPort = srcPort;
- dstPort = dstPort;
+ portNr = portNumber;
+ this->srcPort = srcPort;
+ this->dstPort = dstPort;
}
bool EthernetSniffer::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
@@ -62,7 +62,7 @@ bool EthernetSniffer::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
TRACE("[info] Opening sniffing UDP socket\n");
// We open the UDP socket for sniffing
- if (!this->wiz5500->openUdp(1, SEND_PORT, {255, 255, 255, 255}, RECV_PORT,
+ if (!this->wiz5500->openUdp(portNr, srcPort, {255, 255, 255, 255}, dstPort,
500))
{
return false;
--
GitLab
From 52d7b0c7d706ad70aed45d86011e0e938da36a4d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 27 Mar 2025 11:01:32 +0100
Subject: [PATCH 15/23] [ARP] Sniffing reception window fix, sniffing if ARP
board
- Fixed window for receiving messages, was not correct
- Entrypoint enable sniffer only if is ARP configuration
---
src/Groundstation/Automated/Hub.cpp | 2 +-
src/Groundstation/LyraGS/lyra-gs-entry.cpp | 4 ++--
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index f77e4bcaf..51565c928 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -280,7 +280,7 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
/* Messages older and within the discard interval are treated as old
* messages*/
if (timestamp <= lastFlightTMTimestamp &&
- lastFlightTMTimestamp > timestamp + DISCARD_MSG_DELAY)
+ timestamp > lastFlightTMTimestamp - DISCARD_MSG_DELAY)
return;
TRACE(
"[info][Radio/Sniffing] Hub: A FLIGHT_ROCKET_TM packet is valid "
diff --git a/src/Groundstation/LyraGS/lyra-gs-entry.cpp b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
index 29a0fdef2..5cec13c85 100644
--- a/src/Groundstation/LyraGS/lyra-gs-entry.cpp
+++ b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
@@ -142,8 +142,8 @@ int main()
LyraGS::RadioMain* radio_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, ethernetSniffing);
+ LyraGS::EthernetGS* ethernet = new LyraGS::EthernetGS(
+ false, dipRead.ipConfig, dipRead.isARP & ethernetSniffing);
EthernetSniffer* ethernetSniffer = new EthernetSniffer();
LyraGS::RadioPayload* radio_payload = new LyraGS::RadioPayload(
dipRead.payloadHasBackup, dipRead.payloadTXenable);
--
GitLab
From 256faa641d4573d670bb96a411b9fc781a1b5188 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 10 Apr 2025 12:25:04 +0200
Subject: [PATCH 16/23] [ARP] Sniffed messages log
---
scripts/logdecoder/logdecoder.cpp | 3 ++
src/Groundstation/Automated/Hub.cpp | 4 +-
src/Groundstation/Automated/Hub.h | 1 +
src/Groundstation/Automated/LogSniffing.h | 46 +++++++++++++++++++++++
4 files changed, 53 insertions(+), 1 deletion(-)
create mode 100644 src/Groundstation/Automated/LogSniffing.h
diff --git a/scripts/logdecoder/logdecoder.cpp b/scripts/logdecoder/logdecoder.cpp
index 58e94616c..2ad19f143 100644
--- a/scripts/logdecoder/logdecoder.cpp
+++ b/scripts/logdecoder/logdecoder.cpp
@@ -52,6 +52,8 @@
#include <logger/LogTypes.h>
#include <logger/Logger.h>
#include <tscpp/stream.h>
+#include <Groundstation/Automated/LogSniffing.h>
+
#include <chrono>
#include <filesystem>
@@ -144,6 +146,7 @@ void registerTypes(Deserializer& ds)
ds.registerType<Antennas::SMAStatus>();
ds.registerType<Antennas::PinChangeData>();
ds.registerType<LyraGS::MainRadioLog>();
+ ds.registerType<Antennas::LogSniffing();
}
// cppcheck-suppress passedByValue
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index 51565c928..02ca18864 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -254,6 +254,8 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
* command but the telemetry spoofed, therefore is then used as incoming
*/
dispatchIncomingMsg(msg);
+ LogSniffing sniffing = {TimestampTimer::getTimestamp(),1};
+ Logger::getInstance().log(sniffing);
}
}
@@ -271,7 +273,7 @@ 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);
+ uint64_t timestamp = rocketTM.timestamp;
TRACE(
"[info][Radio/Sniffing] Hub: A FLIGHT_ROCKET_TM packet was "
"received "
diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h
index 0c537945b..30ca02482 100644
--- a/src/Groundstation/Automated/Hub.h
+++ b/src/Groundstation/Automated/Hub.h
@@ -33,6 +33,7 @@
#include <miosix.h>
#include <sensors/SensorData.h>
#include <utils/DependencyManager/DependencyManager.h>
+#include <Groundstation/Automated/LogSniffing.h>
namespace LyraGS
{
diff --git a/src/Groundstation/Automated/LogSniffing.h b/src/Groundstation/Automated/LogSniffing.h
new file mode 100644
index 000000000..13d38523f
--- /dev/null
+++ b/src/Groundstation/Automated/LogSniffing.h
@@ -0,0 +1,46 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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
+{
+
+struct LogSniffing
+{
+ uint64_t timestamp;
+ uint8_t sniffed;
+
+ static std::string header() { return "timestamp,sniffed\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << sniffed << "\n";
+ }
+};
+
+} // namespace Boarcore
--
GitLab
From 380d1e5fc598e456bb1dd53ef266f7574b0c2578 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 17 Apr 2025 13:04:08 +0200
Subject: [PATCH 17/23] [ARP] lyra-gs-entry printing dipswitch setting
[ARP] CI/CD format fix
---
src/Groundstation/Automated/Hub.cpp | 4 ++--
src/Groundstation/Automated/Hub.h | 2 +-
src/Groundstation/Automated/LogSniffing.h | 2 +-
src/Groundstation/LyraGS/lyra-gs-entry.cpp | 19 +++++++++++++++++++
4 files changed, 23 insertions(+), 4 deletions(-)
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index 02ca18864..7e95a699c 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -254,7 +254,7 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
* command but the telemetry spoofed, therefore is then used as incoming
*/
dispatchIncomingMsg(msg);
- LogSniffing sniffing = {TimestampTimer::getTimestamp(),1};
+ LogSniffing sniffing = {TimestampTimer::getTimestamp(), 1};
Logger::getInstance().log(sniffing);
}
}
@@ -273,7 +273,7 @@ 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 = rocketTM.timestamp;
+ uint64_t timestamp = rocketTM.timestamp;
TRACE(
"[info][Radio/Sniffing] Hub: A FLIGHT_ROCKET_TM packet was "
"received "
diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h
index 30ca02482..12c78b197 100644
--- a/src/Groundstation/Automated/Hub.h
+++ b/src/Groundstation/Automated/Hub.h
@@ -22,6 +22,7 @@
#pragma once
+#include <Groundstation/Automated/LogSniffing.h>
#include <Groundstation/Automated/SMA/SMA.h>
#include <Groundstation/Common/HubBase.h>
#include <Groundstation/LyraGS/BoardStatus.h>
@@ -33,7 +34,6 @@
#include <miosix.h>
#include <sensors/SensorData.h>
#include <utils/DependencyManager/DependencyManager.h>
-#include <Groundstation/Automated/LogSniffing.h>
namespace LyraGS
{
diff --git a/src/Groundstation/Automated/LogSniffing.h b/src/Groundstation/Automated/LogSniffing.h
index 13d38523f..4b10019fb 100644
--- a/src/Groundstation/Automated/LogSniffing.h
+++ b/src/Groundstation/Automated/LogSniffing.h
@@ -43,4 +43,4 @@ struct LogSniffing
}
};
-} // namespace Boarcore
+} // namespace Antennas
diff --git a/src/Groundstation/LyraGS/lyra-gs-entry.cpp b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
index 5cec13c85..959829b5a 100644
--- a/src/Groundstation/LyraGS/lyra-gs-entry.cpp
+++ b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
@@ -61,6 +61,17 @@ struct DipStatusLyraGS
uint8_t ipConfig;
};
+/**
+ * Dipswitch configuration
+ * arp mb pb mtx ptx ip3 ip2 ip1
+ * | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 |
+ * ---------------------------------
+ * | I | I | I | I | I | I | I | I |
+ * | O | O | O | O | O | O | O | O |
+ * ---------------------------------
+ * | H | G | F | E | D | C | B | A |
+ */
+
DipStatusLyraGS getDipStatus(uint8_t read)
{
DipStatusLyraGS dipRead;
@@ -131,6 +142,14 @@ int main()
DipSwitch dip(sh, clk, qh, std::chrono::microseconds(microSecClk));
DipStatusLyraGS dipRead = getDipStatus(dip.read());
+ std::cout << "Dipswitch state:"
+ << "\n\t Is ARP: " << dipRead.isARP
+ << "\n\t Main radio backup: " << dipRead.mainHasBackup
+ << "\n\t Payload radio backup: " << dipRead.payloadHasBackup
+ << "\n\t Main TX: " << dipRead.mainTXenable
+ << "\n\t Main TX: " << dipRead.payloadTXenable
+ << "\n\t Ip offset: " << (int)dipRead.ipConfig << "\n";
+
DependencyManager manager;
PrintLogger logger = Logging::getLogger("lyra_gs");
--
GitLab
From 81bfd6b5fd18d542b93a859c2c930598ad51d086 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Mon, 5 May 2025 18:42:19 +0200
Subject: [PATCH 18/23] [ARP] SMA setting gains for feedback and no feedback
- SMA: Now the SMA has configurations to change between feedback and
no feedback the yaw and pitch gain
---
.../Automated/Config/SMAConfig.h | 13 +++++++++++-
src/Groundstation/Automated/SMA/SMA.cpp | 20 +++++++++++++++----
2 files changed, 28 insertions(+), 5 deletions(-)
diff --git a/src/Groundstation/Automated/Config/SMAConfig.h b/src/Groundstation/Automated/Config/SMAConfig.h
index e22f0d193..acd709f01 100644
--- a/src/Groundstation/Automated/Config/SMAConfig.h
+++ b/src/Groundstation/Automated/Config/SMAConfig.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Federico Lolli
+ * Author: 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
@@ -34,5 +34,16 @@ namespace SMAConfig
/// @brief Period of the propagator algorithm [ms].
constexpr milliseconds UPDATE_PERIOD = 100ms; // 10 Hz
+// No feedback gains for the Follower
+static constexpr float YAW_GAIN_NF =
+ 1.0; ///< Yaw gain for the no feedback states
+static constexpr float PITCH_GAIN_NF =
+ 1.0; ///< Pitch gain for the no feedback states
+
+// Feedback gains for the Follower
+static constexpr float YAW_GAIN_F = 0.1; ///< Yaw gain for the feedback states
+static constexpr float PITCH_GAIN_F =
+ 1.0; ///< Pitch gain for the feedback states
+
} // namespace SMAConfig
} // namespace Antennas
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 9fa53f0d1..a5064828e 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -328,13 +328,19 @@ State SMA::state_feedback(const Event& event)
case EV_ENTRY:
{
logStatus(SMAState::FEEDBACK);
- getModule<Leds>()->setOn(LedColor::YELLOW);
+ getModule<Leds>()->setOn(LedColor::RED);
+
+ // Set the gains for the no feedback phase
+ if (!follower.setMaxGain(SMAConfig::YAW_GAIN_F,
+ SMAConfig::PITCH_GAIN_F))
+ LOG_ERR(logger, "Follower gain set failed!\n");
+
return HANDLED;
}
case EV_EXIT:
{
getModule<Actuators>()->disarm();
- getModule<Leds>()->setOff(LedColor::YELLOW);
+ getModule<Leds>()->setOff(LedColor::RED);
return HANDLED;
}
case EV_EMPTY:
@@ -364,13 +370,19 @@ State SMA::state_no_feedback(const Event& event)
case EV_ENTRY:
{
logStatus(SMAState::NO_FEEDBACK);
- getModule<Leds>()->setOn(LedColor::YELLOW);
+ getModule<Leds>()->setOn(LedColor::RED);
+
+ // Set the gains for the no feedback phase
+ if (!follower.setMaxGain(SMAConfig::YAW_GAIN_NF,
+ SMAConfig::PITCH_GAIN_NF))
+ LOG_ERR(logger, "Follower gain set failed!\n");
+
return HANDLED;
}
case EV_EXIT:
{
getModule<Actuators>()->disarm();
- getModule<Leds>()->setOff(LedColor::YELLOW);
+ getModule<Leds>()->setOff(LedColor::RED);
return HANDLED;
}
case EV_EMPTY:
--
GitLab
From 3f8954939faecb036d9e56c2d37612f96eb7bb25 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 8 May 2025 11:14:58 +0200
Subject: [PATCH 19/23] [ARP] Added HubData struct for more logging
informations
HubData now logs the packets received from the ground and from rocket and the sniffed packets.
---
scripts/logdecoder/logdecoder.cpp | 7 ++--
src/Groundstation/Automated/Hub.cpp | 12 ++++++
src/Groundstation/Automated/Hub.h | 3 ++
src/Groundstation/Automated/HubData.h | 53 +++++++++++++++++++++++++++
4 files changed, 72 insertions(+), 3 deletions(-)
create mode 100644 src/Groundstation/Automated/HubData.h
diff --git a/scripts/logdecoder/logdecoder.cpp b/scripts/logdecoder/logdecoder.cpp
index 2ad19f143..d6ca802b9 100644
--- a/scripts/logdecoder/logdecoder.cpp
+++ b/scripts/logdecoder/logdecoder.cpp
@@ -21,6 +21,8 @@
*/
#include <Groundstation/Automated/Actuators/ActuatorsData.h>
+#include <Groundstation/Automated/HubData.h>
+#include <Groundstation/Automated/LogSniffing.h>
#include <Groundstation/Automated/PinHandler/PinData.h>
#include <Groundstation/Automated/SMA/SMAData.h>
#include <Groundstation/LyraGS/Radio/RadioData.h>
@@ -52,8 +54,6 @@
#include <logger/LogTypes.h>
#include <logger/Logger.h>
#include <tscpp/stream.h>
-#include <Groundstation/Automated/LogSniffing.h>
-
#include <chrono>
#include <filesystem>
@@ -146,7 +146,8 @@ void registerTypes(Deserializer& ds)
ds.registerType<Antennas::SMAStatus>();
ds.registerType<Antennas::PinChangeData>();
ds.registerType<LyraGS::MainRadioLog>();
- ds.registerType<Antennas::LogSniffing();
+ ds.registerType<Antennas::LogSniffing>();
+ ds.registerType<Antennas::HubData>();
}
// cppcheck-suppress passedByValue
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index 7e95a699c..d9a62adbe 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -44,6 +44,10 @@ using namespace miosix;
void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
{
+ logHubData.timestamp = TimestampTimer::getTimestamp();
+ logHubData.groundRx = logHubData.groundRx + 1;
+ Logger::getInstance().log(logHubData);
+
TRACE("[info] Hub: Packet arrived from outgoing messages!!!\n");
LyraGS::BoardStatus* status = getModule<LyraGS::BoardStatus>();
LyraGS::RadioMain* radioMain = getModule<LyraGS::RadioMain>();
@@ -256,6 +260,10 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
dispatchIncomingMsg(msg);
LogSniffing sniffing = {TimestampTimer::getTimestamp(), 1};
Logger::getInstance().log(sniffing);
+
+ logHubData.timestamp = TimestampTimer::getTimestamp();
+ logHubData.sniffedRx = logHubData.sniffedRx + 1;
+ Logger::getInstance().log(logHubData);
}
}
@@ -268,6 +276,10 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
(void)serial;
#endif
+ logHubData.timestamp = TimestampTimer::getTimestamp();
+ logHubData.rocketRx = logHubData.rocketRx + 1;
+ Logger::getInstance().log(logHubData);
+
// Extracting NAS rocket state
if (msg.msgid == MAVLINK_MSG_ID_ROCKET_FLIGHT_TM)
{
diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h
index 12c78b197..a61b174e8 100644
--- a/src/Groundstation/Automated/Hub.h
+++ b/src/Groundstation/Automated/Hub.h
@@ -22,6 +22,7 @@
#pragma once
+#include <Groundstation/Automated/HubData.h>
#include <Groundstation/Automated/LogSniffing.h>
#include <Groundstation/Automated/SMA/SMA.h>
#include <Groundstation/Common/HubBase.h>
@@ -112,6 +113,8 @@ private:
bool hasNewNasSet = false;
uint64_t lastFlightTMTimestamp = 0;
uint64_t lastStatsTMTimestamp = 0;
+
+ HubData logHubData; // Data for logging
};
} // namespace Antennas
diff --git a/src/Groundstation/Automated/HubData.h b/src/Groundstation/Automated/HubData.h
new file mode 100644
index 000000000..ca56c5e4e
--- /dev/null
+++ b/src/Groundstation/Automated/HubData.h
@@ -0,0 +1,53 @@
+/* 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 save informations about the Hub reception
+ */
+struct HubData
+{
+ uint64_t timestamp = 0;
+ uint16_t groundRx = 0;
+ uint16_t rocketRx = 0;
+ uint16_t sniffedRx = 0;
+
+ static std::string header()
+ {
+ return "timestamp,groundRx,rocketRx,sniffedRx\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << groundRx << "," << rocketRx << ","
+ << sniffedRx << "\n";
+ }
+};
+} // namespace Antennas
--
GitLab
From e980e012e060d26315c7229ca118b672c6fcac75 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 8 May 2025 11:55:07 +0200
Subject: [PATCH 20/23] [ARP] Added cpu mean to the HubData
---
src/Groundstation/Automated/Hub.cpp | 7 +++++++
src/Groundstation/Automated/HubData.h | 5 +++--
2 files changed, 10 insertions(+), 2 deletions(-)
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index d9a62adbe..21d366c1e 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -31,6 +31,7 @@
#include <algorithms/NAS/NASState.h>
#include <common/Events.h>
#include <common/MavlinkLyra.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
#include <logger/Logger.h>
#include <sensors/SensorData.h>
@@ -46,6 +47,8 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
{
logHubData.timestamp = TimestampTimer::getTimestamp();
logHubData.groundRx = logHubData.groundRx + 1;
+ logHubData.cpuMean = CpuMeter::getCpuStats().mean;
+
Logger::getInstance().log(logHubData);
TRACE("[info] Hub: Packet arrived from outgoing messages!!!\n");
@@ -263,6 +266,8 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
logHubData.timestamp = TimestampTimer::getTimestamp();
logHubData.sniffedRx = logHubData.sniffedRx + 1;
+ logHubData.cpuMean = CpuMeter::getCpuStats().mean;
+
Logger::getInstance().log(logHubData);
}
}
@@ -278,6 +283,8 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
logHubData.timestamp = TimestampTimer::getTimestamp();
logHubData.rocketRx = logHubData.rocketRx + 1;
+ logHubData.cpuMean = CpuMeter::getCpuStats().mean;
+
Logger::getInstance().log(logHubData);
// Extracting NAS rocket state
diff --git a/src/Groundstation/Automated/HubData.h b/src/Groundstation/Automated/HubData.h
index ca56c5e4e..375b112c9 100644
--- a/src/Groundstation/Automated/HubData.h
+++ b/src/Groundstation/Automated/HubData.h
@@ -38,16 +38,17 @@ struct HubData
uint16_t groundRx = 0;
uint16_t rocketRx = 0;
uint16_t sniffedRx = 0;
+ float cpuMean = 0;
static std::string header()
{
- return "timestamp,groundRx,rocketRx,sniffedRx\n";
+ return "timestamp,groundRx,rocketRx,sniffedRx,cpuMean\n";
}
void print(std::ostream& os) const
{
os << timestamp << "," << groundRx << "," << rocketRx << ","
- << sniffedRx << "\n";
+ << sniffedRx << "," << cpuMean << "\n";
}
};
} // namespace Antennas
--
GitLab
From a4bacad2a0b690552c401df9f29e73a593281fc5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 8 May 2025 18:46:09 +0200
Subject: [PATCH 21/23] [ARP, submodules] Fixed SMA led, submodule for WIZ
stats
Fixed an issue with SMA for the RED led in wrong cases
skyward-boardcore: Added wiz5500stats for debugging reasons
---
src/Groundstation/Automated/SMA/SMA.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index a5064828e..09eaed190 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -328,7 +328,7 @@ State SMA::state_feedback(const Event& event)
case EV_ENTRY:
{
logStatus(SMAState::FEEDBACK);
- getModule<Leds>()->setOn(LedColor::RED);
+ getModule<Leds>()->setOff(LedColor::RED);
// Set the gains for the no feedback phase
if (!follower.setMaxGain(SMAConfig::YAW_GAIN_F,
--
GitLab
From b030e77239bbde5f2e5b4037d24df1e861054be9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Sat, 31 May 2025 14:34:07 +0200
Subject: [PATCH 22/23] [ARP] Removed old entrypoint and minor comment
- automated-antennas-entry removed since old unused entrypoint
- minor comment for the gain to tell Follower has another limitation.
---
.../Automated/Config/SMAConfig.h | 1 +
.../Automated/automated-antennas-entry.cpp | 204 ------------------
2 files changed, 1 insertion(+), 204 deletions(-)
delete mode 100644 src/Groundstation/Automated/automated-antennas-entry.cpp
diff --git a/src/Groundstation/Automated/Config/SMAConfig.h b/src/Groundstation/Automated/Config/SMAConfig.h
index acd709f01..8f2de625b 100644
--- a/src/Groundstation/Automated/Config/SMAConfig.h
+++ b/src/Groundstation/Automated/Config/SMAConfig.h
@@ -41,6 +41,7 @@ static constexpr float PITCH_GAIN_NF =
1.0; ///< Pitch gain for the no feedback states
// Feedback gains for the Follower
+// Note that the Follower applies another limitation to avoid gains over 1
static constexpr float YAW_GAIN_F = 0.1; ///< Yaw gain for the feedback states
static constexpr float PITCH_GAIN_F =
1.0; ///< Pitch gain for the feedback states
diff --git a/src/Groundstation/Automated/automated-antennas-entry.cpp b/src/Groundstation/Automated/automated-antennas-entry.cpp
deleted file mode 100644
index 3a022fc8c..000000000
--- a/src/Groundstation/Automated/automated-antennas-entry.cpp
+++ /dev/null
@@ -1,204 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Riccardo Musso, Emilio Corigliano, Niccolò Betto, Federico Lolli
- *
- * 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.
- */
-
-#include <Groundstation/Automated/Actuators/Actuators.h>
-#include <Groundstation/Automated/Buses.h>
-#include <Groundstation/Automated/Hub.h>
-#include <Groundstation/Automated/Leds/Leds.h>
-#include <Groundstation/Automated/Ports/Ethernet.h>
-#include <Groundstation/Automated/Radio/Radio.h>
-#include <Groundstation/Automated/SMA/SMA.h>
-#include <Groundstation/Automated/Sensors/Sensors.h>
-#include <Groundstation/Common/Ports/Serial.h>
-#include <Groundstation/LyraGS/BoardStatus.h>
-#include <common/Events.h>
-#include <diagnostic/PrintLogger.h>
-#include <events/EventBroker.h>
-#include <miosix.h>
-#include <scheduler/TaskScheduler.h>
-#include <utils/ButtonHandler/ButtonHandler.h>
-
-#include <thread>
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-using namespace Groundstation;
-using namespace Antennas;
-using namespace Common;
-using namespace Boardcore;
-using namespace miosix;
-
-GpioPin button = GpioPin(GPIOG_BASE, 10); ///< Emergency stop button
-
-/**
- * @brief Automated Antennas (SkyLink) entrypoint.
- * The entrypoint performs the following operations:
- * - Initializes software modules
- * -> Green LED is turned on when done
- * - Waits for the rocket to be powered on and acquire a GPS fix
- * -> Yellow LED is turned on when done
- * - Waits for the antenna to acquire a GPS fix
- * -> Orange LED is turned on when done
- * - Initializes the follower
- * - Starts the follower task
- */
-int main()
-{
- ModuleManager& modules = ModuleManager::getInstance();
- PrintLogger logger = Logging::getLogger("automated_antennas");
- bool ok = true;
-
- button.mode(Mode::INPUT_PULL_UP);
- // ButtonHandler
- ButtonHandler::getInstance().registerButtonCallback(
- button,
- [&](ButtonEvent bEvent)
- {
- if (bEvent == ButtonEvent::LONG_PRESS ||
- bEvent == ButtonEvent::VERY_LONG_PRESS)
- {
- ModuleManager::getInstance()
- .get<Actuators>()
- ->IRQemergencyStop();
- }
- });
- ButtonHandler::getInstance().start();
-
- TaskScheduler* scheduler_low = new TaskScheduler(0);
- TaskScheduler* scheduler_high = new TaskScheduler();
- Leds* leds = new Leds(scheduler_low);
- Hub* hub = new Hub();
- Buses* buses = new Buses();
- Serial* serial = new Serial();
- RadioMain* radio_main = new RadioMain();
- BoardStatus* board_status = new BoardStatus();
- Actuators* actuators = new Actuators();
- Sensors* sensors = new Sensors();
- SMA* sma = new SMA(scheduler_high);
- Ethernet* ethernet = new Ethernet();
-
- // Inserting Modules
- { // TODO remove this scope (improve readability)
- ok &= modules.insert(sma);
- ok &= modules.insert<HubBase>(hub);
- ok &= modules.insert(buses);
- ok &= modules.insert(serial);
- ok &= modules.insert(radio_main);
- ok &= modules.insert(board_status);
- ok &= modules.insert(actuators);
- ok &= modules.insert(sensors);
- ok &= modules.insert(ethernet);
- ok &= modules.insert(leds);
-
- // If insertion failed, stop right here
- if (!ok)
- {
- LOG_ERR(logger, "Failed to insert all modules!\n");
- leds->endlessBlink(LedColor::RED, LED_BLINK_FAST_PERIOD_MS);
- }
- else
- {
- LOG_DEBUG(logger, "All modules inserted successfully!\n");
- }
- }
-
- // Starting Modules
- bool init_fatal = false;
- bool init_error = false;
-
-#ifndef NO_SD_LOGGING
- if (!Logger::getInstance().start())
- {
- LOG_ERR(logger, "ERROR: Failed to start Logger\n");
- init_error = true;
- }
-#endif
- if (!scheduler_low->start())
- {
- LOG_ERR(logger, "ERROR: Failed to start Scheduler Low\n");
- init_error = true;
- }
- if (!scheduler_high->start())
- {
- LOG_ERR(logger, "FATAL: Failed to start Scheduler High\n");
- init_fatal = true;
- }
- if (!serial->start())
- {
- LOG_ERR(logger, "ERROR: Failed to start Serial\n");
- init_error = true;
- }
- if (!leds->start())
- {
- LOG_ERR(logger, "ERROR: Failed to start Leds\n");
- init_error = true;
- }
- if (!radio_main->start())
- {
- LOG_ERR(logger, "FATAL: Failed to start Main Radio\n");
- init_fatal = true;
- }
- if (!ethernet->start())
- {
- LOG_ERR(logger, "ERROR: Failed to start Ethernet\n");
- init_error = true;
- }
- if (!board_status->start())
- {
- LOG_ERR(logger, "ERROR: Failed to start Board Status\n");
- init_error = true;
- }
- if (!sensors->start())
- {
- LOG_ERR(logger, "ERROR: Failed to start Sensors\n");
- init_error = true;
- }
- if (!sma->start())
- {
- LOG_ERR(logger, "FATAL: Failed to start SMA\n");
- init_fatal = true;
- }
- actuators->start();
-
- LOG_INFO(logger, "Modules setup successful");
-
- if (board_status->isMainRadioPresent())
- LOG_DEBUG(logger, "Main radio is present\n");
-
- // If init fatal and sma not started, blink red endlessly
- if (init_fatal)
- {
- leds->endlessBlink(LedColor::RED, LED_BLINK_FAST_PERIOD_MS);
- } // If another module is in error
- else if (init_error)
- {
- EventBroker::getInstance().post(ARP_INIT_ERROR, TOPIC_ARP);
- } // If all modules are ok
- else
- {
- LOG_INFO(logger, "Starting ARP");
- EventBroker::getInstance().post(ARP_INIT_OK, TOPIC_ARP);
- }
-
- while (true)
- Thread::wait();
- return 0;
-}
--
GitLab
From 292e0acef4c81ee643a402f252cfda770e7ce27c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Sat, 31 May 2025 14:59:48 +0200
Subject: [PATCH 23/23] [ARP] Fix for MR, steppers test speed
- Stepper test speed: Now stepper test speed is reset since otherwise not moves at maximum but last used speed.
- Fix for MR: Authorship for LogSniffing, fixed time for sniffing for ROCKET_STATS_TM
---
.../Automated/Actuators/Actuators.h | 18 +++++++++---------
src/Groundstation/Automated/Hub.cpp | 8 ++++----
src/Groundstation/Automated/LogSniffing.h | 4 ++--
src/Groundstation/Automated/SMA/SMA.cpp | 6 +++++-
4 files changed, 20 insertions(+), 16 deletions(-)
diff --git a/src/Groundstation/Automated/Actuators/Actuators.h b/src/Groundstation/Automated/Actuators/Actuators.h
index 0d457b1af..18310d820 100644
--- a/src/Groundstation/Automated/Actuators/Actuators.h
+++ b/src/Groundstation/Automated/Actuators/Actuators.h
@@ -126,31 +126,31 @@ public:
}
}
-private:
- Boardcore::StepperPWM* getStepper(StepperList stepper)
+ const StepperConfig* getStepperConfig(StepperList stepper) const
{
switch (stepper)
{
case StepperList::STEPPER_X:
- return &stepperX;
+ return &Antennas::Config::stepperXConfig;
case StepperList::STEPPER_Y:
- return &stepperY;
+ return &Antennas::Config::stepperYConfig;
default:
- assert(false && "Non existent stepper");
+ assert(false && "Non existent stepperConfig");
return nullptr;
}
};
- const StepperConfig* getStepperConfig(StepperList stepper) const
+private:
+ Boardcore::StepperPWM* getStepper(StepperList stepper)
{
switch (stepper)
{
case StepperList::STEPPER_X:
- return &Antennas::Config::stepperXConfig;
+ return &stepperX;
case StepperList::STEPPER_Y:
- return &Antennas::Config::stepperYConfig;
+ return &stepperY;
default:
- assert(false && "Non existent stepperConfig");
+ assert(false && "Non existent stepper");
return nullptr;
}
};
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index 21d366c1e..8a0a408a6 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -300,8 +300,8 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
timestamp);
/* Messages older and within the discard interval are treated as old
* messages*/
- if (timestamp <= lastFlightTMTimestamp &&
- timestamp > lastFlightTMTimestamp - DISCARD_MSG_DELAY)
+ if (timestamp > lastFlightTMTimestamp - DISCARD_MSG_DELAY &&
+ timestamp <= lastFlightTMTimestamp)
return;
TRACE(
"[info][Radio/Sniffing] Hub: A FLIGHT_ROCKET_TM packet is valid "
@@ -332,8 +332,8 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
rocketST.timestamp);
/* Messages older and within the discard interval are treated as old
* messages*/
- if (rocketST.timestamp <= lastStatsTMTimestamp &&
- lastStatsTMTimestamp > rocketST.timestamp + DISCARD_MSG_DELAY)
+ if (rocketST.timestamp > lastStatsTMTimestamp - DISCARD_MSG_DELAY &&
+ rocketST.timestamp <= lastStatsTMTimestamp)
return;
TRACE(
"[info][Radio/Sniffing] Hub: A ROCKET_STAT_TM packet is valid, "
diff --git a/src/Groundstation/Automated/LogSniffing.h b/src/Groundstation/Automated/LogSniffing.h
index 4b10019fb..f80248704 100644
--- a/src/Groundstation/Automated/LogSniffing.h
+++ b/src/Groundstation/Automated/LogSniffing.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Davide Mor
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Matteo Pancotti
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 09eaed190..cbe0fd980 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -102,7 +102,11 @@ ActuationStatus SMA::moveStepperDeg(StepperList stepperId, float angle)
}
else
{
- return getModule<Actuators>()->moveDeg(stepperId, angle);
+ auto steppers = getModule<Actuators>();
+
+ const auto* config = steppers->getStepperConfig(stepperId);
+ steppers->setSpeed(stepperId, config->MAX_SPEED);
+ return steppers->moveDeg(stepperId, angle);
}
}
--
GitLab