From 3be859fa58cac61e7199e0a91cd5c9857e4ae22d Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Wed, 2 Oct 2024 15:17:06 +0200
Subject: [PATCH] [BoardStatus] Updated mavlink library and created BoardStatus
 to send ARP_TM periodically

---
 cmake/dependencies.cmake                      |   2 +-
 .../Groundstation/Automated/BoardStatus.cpp   | 136 ++++++++++++++++++
 .../{Radio/RadioStatus.h => BoardStatus.h}    |  21 ++-
 src/boards/Groundstation/Automated/Hub.cpp    |   2 +-
 .../Groundstation/Automated/Radio/Radio.cpp   |   4 +-
 .../Automated/Radio/RadioStatus.cpp           |  89 ------------
 .../Automated/automated-antennas-entry.cpp    |  10 +-
 7 files changed, 155 insertions(+), 109 deletions(-)
 create mode 100644 src/boards/Groundstation/Automated/BoardStatus.cpp
 rename src/boards/Groundstation/Automated/{Radio/RadioStatus.h => BoardStatus.h} (84%)
 delete mode 100644 src/boards/Groundstation/Automated/Radio/RadioStatus.cpp

diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 57ac1915f..b4b996bff 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -140,8 +140,8 @@ set(GROUNDSTATION_BASE
 )
 
 set(GROUNDSTATION_AUTOMATED
+    src/boards/Groundstation/Automated/BoardStatus.cpp
     src/boards/Groundstation/Automated/Radio/Radio.cpp
-    src/boards/Groundstation/Automated/Radio/RadioStatus.cpp
     src/boards/Groundstation/Automated/Follower/Follower.cpp
     src/boards/Groundstation/Automated/Ports/Ethernet.cpp
     src/boards/Groundstation/Automated/Hub.cpp
diff --git a/src/boards/Groundstation/Automated/BoardStatus.cpp b/src/boards/Groundstation/Automated/BoardStatus.cpp
new file mode 100644
index 000000000..df329bf44
--- /dev/null
+++ b/src/boards/Groundstation/Automated/BoardStatus.cpp
@@ -0,0 +1,136 @@
+/* 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.
+ */
+
+#include "BoardStatus.h"
+
+#include <Groundstation/Automated/Actuators/Actuators.h>
+#include <Groundstation/Automated/Follower/Follower.h>
+#include <Groundstation/Automated/Ports/Ethernet.h>
+#include <Groundstation/Automated/Radio/Radio.h>
+#include <Groundstation/Automated/Sensors/Sensors.h>
+#include <Groundstation/Common/Config/GeneralConfig.h>
+#include <Groundstation/Common/HubBase.h>
+#include <common/Mavlink.h>
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Antennas;
+using namespace Boardcore;
+using namespace Groundstation;
+
+bool BoardStatus::isMainRadioPresent() { return main_radio_present; }
+bool BoardStatus::isEthernetPresent() { return ethernet_present; }
+
+bool BoardStatus::start()
+{
+    if (!ActiveObject::start())
+    {
+        return false;
+    }
+
+    return true;
+}
+
+void BoardStatus::setMainRadioPresent(bool present)
+{
+    main_radio_present = present;
+}
+
+void BoardStatus::setEthernetPresent(bool present)
+{
+    ethernet_present = present;
+}
+
+void BoardStatus::run()
+{
+    while (!shouldStop())
+    {
+        miosix::Thread::sleep(Groundstation::RADIO_STATUS_PERIOD);
+
+        auto vn300 =
+            ModuleManager::getInstance().get<Sensors>()->getVN300LastSample();
+
+        Actuators *actuators = ModuleManager::getInstance().get<Actuators>();
+        AntennaAngles targetAngles =
+            ModuleManager::getInstance().get<Follower>()->getTargetAngles();
+
+        mavlink_arp_tm_t tm = {0};
+        tm.timestamp    = TimestampTimer::getTimestamp(); /*< [us] Timestamp*/
+        tm.yaw          = vn300.yaw;          /*< [deg] Current Yaw*/
+        tm.pitch        = vn300.pitch;        /*< [deg] Current Pitch*/
+        tm.roll         = vn300.roll;         /*< [deg] Current Roll*/
+        tm.target_yaw   = targetAngles.yaw;   /*< [deg] Target Yaw*/
+        tm.target_pitch = targetAngles.pitch; /*< [deg] Target Pitch*/
+        tm.stepperX_pos = actuators->getCurrentDegPosition(
+            Actuators::StepperList::HORIZONTAL); /*< [deg] StepperX target pos*/
+        tm.stepperX_delta = actuators->getDeltaAngleDeg(
+            Actuators::StepperList::HORIZONTAL); /*< [deg] StepperX target delta
+                                                    deg*/
+        tm.stepperX_speed = actuators->getSpeed(
+            Actuators::StepperList::HORIZONTAL); /*< [rps] StepperX Speed*/
+        tm.stepperY_pos = actuators->getCurrentDegPosition(
+            Actuators::StepperList::VERTICAL); /*< [deg] StepperY target pos*/
+        tm.stepperY_delta = actuators->getDeltaAngleDeg(
+            Actuators::StepperList::VERTICAL); /*< [deg] StepperY target delta
+                                                  deg*/
+        tm.stepperY_speed = actuators->getSpeed(
+            Actuators::StepperList::VERTICAL); /*< [rps] StepperY Speed*/
+        tm.gps_latitude  = vn300.latitude;     /*< [deg] Latitude*/
+        tm.gps_longitude = vn300.longitude;    /*< [deg] Longitude*/
+        tm.gps_height    = vn300.altitude;     /*< [m] Altitude*/
+        tm.gps_fix       = vn300.fix_ins;      /*<  Wether the GPS has a FIX*/
+
+        tm.battery_voltage = -420.0;
+
+        if (main_radio_present)
+        {
+            tm.main_radio_present = 1;
+
+            auto stats =
+                ModuleManager::getInstance().get<RadioMain>()->getStats();
+            tm.main_packet_tx_error_count = stats.send_errors;
+            tm.main_tx_bitrate = main_tx_bitrate.update(stats.bits_tx_count);
+            tm.main_packet_rx_success_count = stats.packet_rx_success_count;
+            tm.main_packet_rx_drop_count    = stats.packet_rx_drop_count;
+            tm.main_rx_bitrate = main_rx_bitrate.update(stats.bits_rx_count);
+            tm.main_rx_rssi    = stats.rx_rssi;
+            
+            last_main_stats = stats;
+        }
+
+        if (ethernet_present)
+        {
+            auto stats =
+                ModuleManager::getInstance().get<Ethernet>()->getState();
+
+            tm.ethernet_present = 1;
+            tm.ethernet_status  = (stats.link_up ? 1 : 0) |
+                                 (stats.full_duplex ? 2 : 0) |
+                                 (stats.based_100mbps ? 4 : 0);
+        }
+
+        mavlink_message_t msg;
+        mavlink_msg_arp_tm_encode(Groundstation::GS_SYSTEM_ID,
+                                  Groundstation::GS_COMPONENT_ID, &msg, &tm);
+
+        ModuleManager::getInstance().get<HubBase>()->dispatchIncomingMsg(msg);
+    }
+};
diff --git a/src/boards/Groundstation/Automated/Radio/RadioStatus.h b/src/boards/Groundstation/Automated/BoardStatus.h
similarity index 84%
rename from src/boards/Groundstation/Automated/Radio/RadioStatus.h
rename to src/boards/Groundstation/Automated/BoardStatus.h
index 81282807e..60d57a2c7 100644
--- a/src/boards/Groundstation/Automated/Radio/RadioStatus.h
+++ b/src/boards/Groundstation/Automated/BoardStatus.h
@@ -54,7 +54,7 @@ public:
             uint16_t delta = sample - last;
 
             // window size is in ms, we want the result in s
-            return delta * 1000 / Groundstation::RADIO_BITRATE_WINDOW_SIZE;
+            return delta * 1000 / WINDOW_SIZE;
         }
         else
         {
@@ -70,10 +70,10 @@ private:
 /**
  * @brief Class responsible for keeping track of radio status and metrics.
  */
-class RadioStatus : public Boardcore::Module, private Boardcore::ActiveObject
+class BoardStatus : public Boardcore::Module, protected Boardcore::ActiveObject
 {
 public:
-    RadioStatus() {}
+    BoardStatus() {}
 
     bool start();
 
@@ -83,18 +83,17 @@ public:
     bool isMainRadioPresent();
 
     /**
-     * @brief Check wether the payload radio was found during boot.
+     * @brief Check wether the ethernet was found during boot.
      */
-    bool isPayloadRadioPresent();
+    bool isEthernetPresent();
 
     void setMainRadioPresent(bool present);
-    void setPayloadRadioPresent(bool present);
+    void setEthernetPresent(bool present);
 
-private:
+protected:
     void run() override;
 
-    Groundstation::RadioStats last_main_stats    = {0};
-    Groundstation::RadioStats last_payload_stats = {0};
+    Groundstation::RadioStats last_main_stats = {0};
 
     BitrateCalculator<Groundstation::RADIO_BITRATE_WINDOW_SIZE,
                       Groundstation::RADIO_STATUS_PERIOD>
@@ -103,8 +102,8 @@ private:
                       Groundstation::RADIO_STATUS_PERIOD>
         main_rx_bitrate;
 
-    bool main_radio_present    = false;
-    bool payload_radio_present = false;
+    bool main_radio_present = false;
+    bool ethernet_present   = false;
 };
 
 }  // namespace Antennas
\ No newline at end of file
diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index 905608ec5..049464c4f 100644
--- a/src/boards/Groundstation/Automated/Hub.cpp
+++ b/src/boards/Groundstation/Automated/Hub.cpp
@@ -22,10 +22,10 @@
 
 #include "Hub.h"
 
+#include <Groundstation/Automated/BoardStatus.h>
 #include <Groundstation/Automated/Follower/Follower.h>
 #include <Groundstation/Automated/Ports/Ethernet.h>
 #include <Groundstation/Automated/Radio/Radio.h>
-#include <Groundstation/Automated/Radio/RadioStatus.h>
 #include <Groundstation/Common/Config/GeneralConfig.h>
 #include <Groundstation/Common/Ports/Serial.h>
 #include <algorithms/NAS/NASState.h>
diff --git a/src/boards/Groundstation/Automated/Radio/Radio.cpp b/src/boards/Groundstation/Automated/Radio/Radio.cpp
index ee543417a..f3747d6b0 100644
--- a/src/boards/Groundstation/Automated/Radio/Radio.cpp
+++ b/src/boards/Groundstation/Automated/Radio/Radio.cpp
@@ -22,9 +22,9 @@
 
 #include "Radio.h"
 
+#include <Groundstation/Automated/BoardStatus.h>
 #include <Groundstation/Automated/Buses.h>
 #include <Groundstation/Automated/Hub.h>
-#include <Groundstation/Automated/Radio/RadioStatus.h>
 #include <Groundstation/Common/Ports/Serial.h>
 #include <interfaces-impl/hwmapping.h>
 #include <radio/SX1278/SX1278Frontends.h>
@@ -72,7 +72,7 @@ bool RadioMain::start()
     // First check if the device is even connected
     bool present = sx1278->checkVersion();
 
-    ModuleManager::getInstance().get<RadioStatus>()->setMainRadioPresent(
+    ModuleManager::getInstance().get<BoardStatus>()->setMainRadioPresent(
         present);
 
     if (present)
diff --git a/src/boards/Groundstation/Automated/Radio/RadioStatus.cpp b/src/boards/Groundstation/Automated/Radio/RadioStatus.cpp
deleted file mode 100644
index 164e2a590..000000000
--- a/src/boards/Groundstation/Automated/Radio/RadioStatus.cpp
+++ /dev/null
@@ -1,89 +0,0 @@
-/* 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.
- */
-
-#include "RadioStatus.h"
-
-#include <Groundstation/Automated/Radio/Radio.h>
-#include <Groundstation/Common/Config/GeneralConfig.h>
-#include <Groundstation/Common/HubBase.h>
-#include <common/Mavlink.h>
-#include <drivers/timer/TimestampTimer.h>
-
-using namespace Boardcore;
-using namespace Groundstation;
-using namespace Antennas;
-
-bool RadioStatus::isMainRadioPresent() { return main_radio_present; }
-
-bool RadioStatus::start()
-{
-    if (!ActiveObject::start())
-    {
-        return false;
-    }
-
-    return true;
-}
-
-void RadioStatus::setMainRadioPresent(bool present)
-{
-    main_radio_present = present;
-}
-
-void RadioStatus::setPayloadRadioPresent(bool present)
-{
-    payload_radio_present = present;
-}
-
-void RadioStatus::run()
-{
-    while (!shouldStop())
-    {
-        miosix::Thread::sleep(RADIO_STATUS_PERIOD);
-
-        mavlink_receiver_tm_t tm = {0};
-        tm.timestamp             = TimestampTimer::getTimestamp();
-
-        if (main_radio_present)
-        {
-            tm.main_radio_present = 1;
-
-            auto stats =
-                ModuleManager::getInstance().get<RadioMain>()->getStats();
-            tm.main_packet_tx_error_count = stats.send_errors;
-            tm.main_tx_bitrate = main_tx_bitrate.update(stats.bits_tx_count);
-            tm.main_packet_rx_success_count = stats.packet_rx_success_count;
-            tm.main_packet_rx_drop_count    = stats.packet_rx_drop_count;
-            tm.main_rx_bitrate = main_rx_bitrate.update(stats.bits_rx_count);
-            tm.main_rx_rssi    = stats.rx_rssi;
-            tm.main_rx_fei     = stats.rx_fei;
-
-            last_main_stats = stats;
-        }
-
-        mavlink_message_t msg;
-        mavlink_msg_receiver_tm_encode(GS_SYSTEM_ID, GS_COMPONENT_ID, &msg,
-                                       &tm);
-
-        ModuleManager::getInstance().get<HubBase>()->dispatchIncomingMsg(msg);
-    }
-}
\ No newline at end of file
diff --git a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
index 56e5ba753..610761925 100644
--- a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
+++ b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
@@ -26,8 +26,8 @@
 #include <Groundstation/Automated/Hub.h>
 #include <Groundstation/Automated/Ports/Ethernet.h>
 #include <Groundstation/Automated/Radio/Radio.h>
-#include <Groundstation/Automated/Radio/RadioStatus.h>
 #include <Groundstation/Automated/Sensors/Sensors.h>
+#include <Groundstation/Automated/BoardStatus.h>
 #include <Groundstation/Common/Ports/Serial.h>
 #include <diagnostic/CpuMeter/CpuMeter.h>
 #include <diagnostic/PrintLogger.h>
@@ -172,7 +172,7 @@ int main()
     Buses *buses              = new Buses();
     Serial *serial            = new Serial();
     RadioMain *radio_main     = new RadioMain();
-    RadioStatus *radio_status = new RadioStatus();
+    BoardStatus *board_status = new BoardStatus();
     Actuators *actuators      = new Actuators();
     Sensors *sensors          = new Sensors();
     Follower *follower        = new Follower();
@@ -185,7 +185,7 @@ int main()
         ok &= modules.insert(buses);
         ok &= modules.insert(serial);
         ok &= modules.insert(radio_main);
-        ok &= modules.insert(radio_status);
+        ok &= modules.insert(board_status);
         ok &= modules.insert(actuators);
         ok &= modules.insert(sensors);
         ok &= modules.insert(ethernet);
@@ -211,7 +211,7 @@ int main()
         START_MODULE("Serial", [&] { return serial->start(); });
         START_MODULE("Main Radio", [&] { return radio_main->start(); });
         START_MODULE("Ethernet", [&] { return ethernet->start(); });
-        START_MODULE("Radio Status", [&] { return radio_status->start(); });
+        START_MODULE("Board Status", [&] { return board_status->start(); });
         START_MODULE("Sensors", [&] { return sensors->start(); });
         actuators->start();
     }
@@ -220,7 +220,7 @@ int main()
     led1On();
     LOG_INFO(logger, "Modules setup successful");
 
-    if (radio_status->isMainRadioPresent())
+    if (board_status->isMainRadioPresent())
     {
         LOG_DEBUG(logger, "Main radio is present\n");
     }
-- 
GitLab