diff --git a/CMakeLists.txt b/CMakeLists.txt
index 56712bb3afe44c8461c32c036cd918ac2b1537b2..670a5267d89485f34359194a6a46640f52f24ff7 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/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 2f96687df87daa93c68bb634bba780abc776c704..16ba691682e42b64f0e2fb07bac74023c28b3d86 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/scripts/logdecoder/logdecoder.cpp b/scripts/logdecoder/logdecoder.cpp
index 72530d5fdc0dc6aa41944d872011ceecb9822974..d6ca802b9ccb2f320ef4f80df17dc5ef3d1dc313 100644
--- a/scripts/logdecoder/logdecoder.cpp
+++ b/scripts/logdecoder/logdecoder.cpp
@@ -21,8 +21,11 @@
*/
#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>
#include <Main/PinHandler/PinData.h>
#include <Main/Sensors/SensorsData.h>
#include <Main/StateMachines/ABKController/ABKControllerData.h>
@@ -46,6 +49,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 +138,16 @@ 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>();
+ ds.registerType<Antennas::LogSniffing>();
+ ds.registerType<Antennas::HubData>();
}
// cppcheck-suppress passedByValue
diff --git a/src/Groundstation/Automated/Actuators/Actuators.h b/src/Groundstation/Automated/Actuators/Actuators.h
index 0d457b1af7b15c5b796d715ad9ca2758f13a5ba8..18310d820724c3f478f58b16136921560581875c 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/Config/SMAConfig.h b/src/Groundstation/Automated/Config/SMAConfig.h
index e22f0d193040846badd9b80a333e262c0ec8b063..8f2de625b950a0352a2e3166f8bc360c528683a5 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,17 @@ 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
+// 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
+
} // namespace SMAConfig
} // namespace Antennas
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index adc96f2c6ac6e0f43b20e1b10e2cede944e2b877..8a0a408a614d356a8bb6133344302482f7bcf12c 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>
@@ -44,6 +45,13 @@ using namespace miosix;
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");
LyraGS::BoardStatus* status = getModule<LyraGS::BoardStatus>();
LyraGS::RadioMain* radioMain = getModule<LyraGS::RadioMain>();
@@ -60,6 +68,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 +248,28 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
}
}
}
+
+ // In case the message is spoofed from ethernet by another groundstation
+ if (msg.msgid == MAVLINK_MSG_ID_ROCKET_FLIGHT_TM ||
+ msg.msgid == MAVLINK_MSG_ID_ROCKET_STATS_TM)
+ {
+ TRACE(
+ "[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
+ */
+ dispatchIncomingMsg(msg);
+ LogSniffing sniffing = {TimestampTimer::getTimestamp(), 1};
+ Logger::getInstance().log(sniffing);
+
+ logHubData.timestamp = TimestampTimer::getTimestamp();
+ logHubData.sniffedRx = logHubData.sniffedRx + 1;
+ logHubData.cpuMean = CpuMeter::getCpuStats().mean;
+
+ Logger::getInstance().log(logHubData);
+ }
}
void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
@@ -250,11 +281,33 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
(void)serial;
#endif
+ logHubData.timestamp = TimestampTimer::getTimestamp();
+ logHubData.rocketRx = logHubData.rocketRx + 1;
+ logHubData.cpuMean = CpuMeter::getCpuStats().mean;
+
+ Logger::getInstance().log(logHubData);
+
// Extracting NAS rocket state
if (msg.msgid == MAVLINK_MSG_ID_ROCKET_FLIGHT_TM)
{
mavlink_rocket_flight_tm_t rocketTM;
mavlink_msg_rocket_flight_tm_decode(&msg, &rocketTM);
+ uint64_t timestamp = rocketTM.timestamp;
+ TRACE(
+ "[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
+ * messages*/
+ if (timestamp > lastFlightTMTimestamp - DISCARD_MSG_DELAY &&
+ timestamp <= lastFlightTMTimestamp)
+ return;
+ 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),
Eigen::Matrix<float, 13, 1>(
@@ -273,9 +326,25 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
{
mavlink_rocket_stats_tm_t rocketST;
mavlink_msg_rocket_stats_tm_decode(&msg, &rocketST);
+ TRACE(
+ "[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
+ * messages*/
+ if (rocketST.timestamp > lastStatsTMTimestamp - DISCARD_MSG_DELAY &&
+ rocketST.timestamp <= lastStatsTMTimestamp)
+ return;
+ 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
+ // [lat, lon, alt] are needed
GPSData gpsState;
- gpsState = getRocketOrigin();
-
+ getRocketOrigin(gpsState);
gpsState.gpsTimestamp = rocketST.timestamp;
gpsState.latitude = rocketST.ref_lat;
gpsState.longitude = rocketST.ref_lon;
@@ -300,25 +369,32 @@ 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()
+{
+ Lock<FastMutex> lock(nasStateMutex);
+ return hasNewNasSet;
+}
void Hub::setRocketNasState(const NASState& newRocketNasState)
{
Lock<FastMutex> lock(nasStateMutex);
- flagNasSet = true;
+ hasNewNasSet = true;
+ rocketNasSet = true;
lastRocketNasState = newRocketNasState;
}
@@ -326,4 +402,5 @@ void Hub::setRocketOrigin(const GPSData& newRocketCoordinates)
{
Lock<FastMutex> lock(coordinatesMutex);
lastRocketCoordinates = newRocketCoordinates;
+ originReceived = true;
}
diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h
index 9130dd058466db23751f9c511fd702c40ef3a5de..a61b174e89b8bbab7788287d2c11da112aef3c93 100644
--- a/src/Groundstation/Automated/Hub.h
+++ b/src/Groundstation/Automated/Hub.h
@@ -22,6 +22,8 @@
#pragma once
+#include <Groundstation/Automated/HubData.h>
+#include <Groundstation/Automated/LogSniffing.h>
#include <Groundstation/Automated/SMA/SMA.h>
#include <Groundstation/Common/HubBase.h>
#include <Groundstation/LyraGS/BoardStatus.h>
@@ -42,6 +44,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.
*/
@@ -72,14 +81,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:
/**
@@ -93,10 +105,16 @@ 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 = 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 0000000000000000000000000000000000000000..375b112c973c9ab9d3421abbe1aca438f2ab23ca
--- /dev/null
+++ b/src/Groundstation/Automated/HubData.h
@@ -0,0 +1,54 @@
+/* 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;
+ float cpuMean = 0;
+
+ static std::string header()
+ {
+ return "timestamp,groundRx,rocketRx,sniffedRx,cpuMean\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << groundRx << "," << rocketRx << ","
+ << sniffedRx << "," << cpuMean << "\n";
+ }
+};
+} // namespace Antennas
diff --git a/src/Groundstation/Automated/LogSniffing.h b/src/Groundstation/Automated/LogSniffing.h
new file mode 100644
index 0000000000000000000000000000000000000000..f8024870465438b6b7783f6d8943954d2f14022d
--- /dev/null
+++ b/src/Groundstation/Automated/LogSniffing.h
@@ -0,0 +1,46 @@
+/* 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
+ * 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 Antennas
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 57b1707431eac53f15799160dfeb1d7feca1c915..cbe0fd980e7e2a445d69c55096c38da716407d16 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -94,14 +94,19 @@ 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;
}
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);
}
}
@@ -138,41 +143,50 @@ void SMA::setFatal() { fatalInit = true; };
void SMA::update()
{
GPSData rocketCoordinates, antennaCoordinates;
- VN300Data vn300Data;
-
- Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
- auto* sensors = getModule<Sensors>();
- rocketCoordinates = hub->getRocketOrigin();
-
- // 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)
+ VN300Data data;
+ auto steppers = getModule<Actuators>();
+
+ 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 (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
- 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)
{
@@ -195,6 +209,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:
@@ -211,24 +226,26 @@ void SMA::update()
}
break;
}
+
// in active state, update the follower and propagator inner states
case SMAState::ACTIVE:
+ case SMAState::ACTIVE_NF:
{
// retrieve the last NAS Rocket state
- if (hub->hasNasSet())
- {
- NASState nasState = hub->getRocketNasState();
+ 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);
- }
+
propagator.update(); // step the propagator
PropagatorState predicted = propagator.getState();
// update the follower with the propagated state
follower.setLastRocketNasState(predicted.getNasState());
- follower.setLastAntennaAttitude(vn300Data);
+ follower.setLastAntennaAttitude(data);
follower.update(); // step the follower
FollowerState follow = follower.getState();
@@ -238,7 +255,6 @@ void SMA::update()
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);
@@ -262,77 +278,20 @@ void SMA::update()
"limit. Error: ",
actuation, "\n");
}
-
- break;
}
- case SMAState::ACTIVE_NF:
- {
- VN300Data fakeAttitudeData;
-
- // retrieve the last NAS Rocket state
- if (hub->hasNasSet())
- {
- NASState nasState = hub->getRocketNasState();
+ break;
- // 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.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
- 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)
+ case SMAState::CALIBRATE:
+ {
+ if (!sensors->isCalibrating())
{
- LOG_ERR(
- logger,
- "Step antenna - STEPPER_X could not move or reached move "
- "limit. Error: ",
- actuation, "\n");
+ EventBroker::getInstance().post(ARP_CAL_DONE, TOPIC_ARP);
+ LOG_DEBUG(logger, "Exit from calibration\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;
}
+ break;
default:
- {
break;
- }
}
}
@@ -373,13 +332,19 @@ State SMA::state_feedback(const Event& event)
case EV_ENTRY:
{
logStatus(SMAState::FEEDBACK);
- getModule<Leds>()->setOn(LedColor::YELLOW);
+ getModule<Leds>()->setOff(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:
@@ -409,13 +374,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:
@@ -702,6 +673,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;
}
@@ -783,7 +757,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 2bc290d9a150501f70d4b64d8aa3fb163cf72891..b73b2c66ad6c45b089b1c678d99d6da5a771cb0c 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);
@@ -108,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);
/**
diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp
index 866231a74dfb27b41b52d496cde6decf28920177..0fe17cf13e091b16f95e3e3c06a71893bd637508 100644
--- a/src/Groundstation/Automated/Sensors/Sensors.cpp
+++ b/src/Groundstation/Automated/Sensors/Sensors.cpp
@@ -63,9 +63,33 @@ 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)
+ {
+ 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());
}
VN300Data Sensors::getVN300LastSample() { return vn300->getLastSample(); }
diff --git a/src/Groundstation/Automated/Sensors/Sensors.h b/src/Groundstation/Automated/Sensors/Sensors.h
index 87ce9a237b67bfc8b1923e3bace41de0a20a61b4..43d06d76b209563e578ee2d1e184a35954be4796 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
diff --git a/src/Groundstation/Automated/automated-antennas-entry.cpp b/src/Groundstation/Automated/automated-antennas-entry.cpp
deleted file mode 100644
index 3a022fc8cb07f136522f48d9634cc54bc626b7a7..0000000000000000000000000000000000000000
--- 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;
-}
diff --git a/src/Groundstation/Automated/test-actuators.cpp b/src/Groundstation/Automated/test-actuators.cpp
deleted file mode 100644
index a92629b1535cecff7f8819344f433e9e6247de02..0000000000000000000000000000000000000000
--- 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 c33b71cdde30e8d197e6e4ca81e12f2c697e5838..0000000000000000000000000000000000000000
--- 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 a77712da36b4483cfa9ae33f823ea4fa89d41a0d..0000000000000000000000000000000000000000
--- 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 7a69211c5b49a6e90d374849c7ae77f73840eb8e..0000000000000000000000000000000000000000
--- 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)
- ;
-}
diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp
index f183a9a64b728425db6bc3a3c2982159b5e80874..76e66d89a1a3e7ce7d47cd129a48152a8e690fc2 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,9 +65,9 @@ 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);
+ this->wiz5500 = wiz5500;
// Reset the device
this->wiz5500->reset();
@@ -82,7 +82,21 @@ 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;
+ // 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
{
@@ -108,6 +122,15 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
if (!mav_driver->start())
return false;
+ // In case of sniffing mode initialize and start the EthernetSniffer
+ if (sniffOtherGs)
+ {
+ getModule<EthernetSniffer>()->init(1, SEND_PORT, RECV_PORT);
+ if (!getModule<EthernetSniffer>()->start(this->wiz5500))
+ return false;
+ }
+
+ TRACE("[info] Ethernet module started correctly\n");
return true;
}
@@ -126,5 +149,6 @@ ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len)
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 5f997f6caed74ecbfaf0dc24cc7353dcd116cd52..57e6622e50e7be804694ae030ecf4afc25df5104 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>
@@ -34,19 +35,23 @@
namespace Groundstation
{
+// Timeout for the port receive
+static constexpr uint16_t RECEIVE_PORT_TIMEOUT_MS = 500;
+
Boardcore::WizIp genNewRandomIp();
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() {};
- 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();
@@ -55,7 +60,8 @@ public:
Boardcore::Wiz5500::PhyState getState();
protected:
- bool start(std::unique_ptr<Boardcore::Wiz5500> wiz5500);
+ bool start(std::shared_ptr<Boardcore::Wiz5500> wiz5500);
+ std::shared_ptr<Boardcore::Wiz5500> wiz5500;
private:
/**
@@ -68,10 +74,11 @@ 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;
+ 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 0000000000000000000000000000000000000000..e08e1c538c3f842536cf6097bde48d7572b11d7e
--- /dev/null
+++ b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
@@ -0,0 +1,95 @@
+/* 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;
+ this->srcPort = srcPort;
+ this->dstPort = dstPort;
+}
+
+bool EthernetSniffer::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
+{
+ this->wiz5500 = wiz5500;
+
+ TRACE("[info] Opening sniffing UDP socket\n");
+ // We open the UDP socket for sniffing
+ if (!this->wiz5500->openUdp(portNr, srcPort, {255, 255, 255, 255}, dstPort,
+ 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, 0, 10);
+
+ if (!mav_driver->start())
+ return false;
+
+ TRACE("[info] EthernetSniffer 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 0000000000000000000000000000000000000000..7f4ea7a74340f2bd367d761b11c254f6117ca1e0
--- /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/BoardStatus.cpp b/src/Groundstation/LyraGS/BoardStatus.cpp
index 0b7f6bb91c95ca19ae1e31f94340a0b79ffbfee5..b65ac904bdbabe308b7975ad6bf181dac7180195 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 3eb286799a41a297744808a0244e435ecfa7f4c6..4d789f71a6896bb22002d07071417d4f08f5a805 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>
diff --git a/src/Groundstation/LyraGS/Ports/Ethernet.cpp b/src/Groundstation/LyraGS/Ports/Ethernet.cpp
index 6b3e3270fc12fa4186473c77f74e6cf6f1c58d41..6013c835a97d4865c89e5e161228ab1a9ab59198 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/Ports/Ethernet.h b/src/Groundstation/LyraGS/Ports/Ethernet.h
index a267a7abe1cef7bc863076659560f706ab3e3167..4899208287603a70beb44f845202a6c594a01523 100644
--- a/src/Groundstation/LyraGS/Ports/Ethernet.h
+++ b/src/Groundstation/LyraGS/Ports/Ethernet.h
@@ -36,7 +36,10 @@ class EthernetGS : public Boardcore::InjectableWithDeps<
{
public:
EthernetGS() : Super{} {}
- EthernetGS(bool randomIp, uint8_t ipOffset) : Super{randomIp, ipOffset} {}
+ EthernetGS(bool randomIp, uint8_t ipOffset, bool sniffing)
+ : Super{randomIp, ipOffset, sniffing}
+ {
+ }
[[nodiscard]] bool start();
void sendMsg(const mavlink_message_t& msg);
void handleINTn();
diff --git a/src/Groundstation/LyraGS/Radio/RadioData.h b/src/Groundstation/LyraGS/Radio/RadioData.h
new file mode 100644
index 0000000000000000000000000000000000000000..5edb3ce4b88e6c0e72b235222a9d0aec2e1bfd47
--- /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
diff --git a/src/Groundstation/LyraGS/lyra-gs-entry.cpp b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
index 869328463a5abc32c44df6001423c7d8c5337773..959829b5abefca09b73ea510ec24fce021c7cd6b 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;
@@ -93,6 +104,8 @@ void errorLoop()
}
}
+static bool constexpr ethernetSniffing = true;
+
/**
* @brief Lyra GS entrypoint.
* This entrypoint performs the following operations:
@@ -129,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");
@@ -140,8 +161,9 @@ 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);
+ 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);
@@ -160,6 +182,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);
@@ -343,7 +366,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 +374,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 +382,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)