[gs] Added ethernet port

cmake/dependencies.cmake

@@ -45,6 +45,7 @@ set(MAIN_COMPUTER
 set(GROUNDSTATION_COMMON
     src/boards/Groundstation/Base/Radio/Radio.cpp
     src/boards/Groundstation/Base/Radio/RadioStatus.cpp
+    src/boards/Groundstation/Base/Ports/Ethernet.cpp
     src/boards/Groundstation/Base/Hub.cpp
 )
 
@@ -133,6 +134,7 @@ set(GS_COMPUTER
 
 set(GROUNDSTATION_BASE
     src/boards/Groundstation/Common/Ports/Serial.cpp
+    src/boards/Groundstation/Common/Ports/EthernetBase.cpp
     src/boards/Groundstation/Common/Radio/RadioBase.cpp
     src/boards/Groundstation/Common/HubBase.cpp
 )
\ No newline at end of file

src/boards/Groundstation/Base/Hub.cpp

@@ -24,6 +24,7 @@
 
 #include <Groundstation/Common/Config/GeneralConfig.h>
 #include <Groundstation/Common/Ports/Serial.h>
+#include <Groundstation/Base/Ports/Ethernet.h>
 #include <Groundstation/Base/Radio/Radio.h>
 #include <Groundstation/Base/Radio/RadioStatus.h>
 
@@ -62,5 +63,6 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
     Serial* serial = ModuleManager::getInstance().get<Serial>();
     serial->sendMsg(msg);
 
-    // TODO: Add UDP dispatch
+    Ethernet* ethernet = ModuleManager::getInstance().get<Ethernet>();
+    ethernet->sendMsg(msg);
 }
\ No newline at end of file

src/boards/Groundstation/Base/Ports/Ethernet.cpp

@@ -22,12 +22,10 @@
 
 #include "Ethernet.h"
 
-#include <Groundstation/Base/BoardStatus.h>
 #include <Groundstation/Base/Buses.h>
 #include <interfaces-impl/hwmapping.h>
 
 using namespace Groundstation;
-using namespace GroundstationBase;
 using namespace Boardcore;
 using namespace miosix;
 
@@ -39,23 +37,9 @@ void __attribute__((used)) MIOSIX_ETHERNET_IRQ()
 bool Ethernet::start()
 {
     std::unique_ptr<Wiz5500> wiz5500 = std::make_unique<Wiz5500>(
-        ModuleManager::getInstance().get<Buses>()->ethernet_bus,
+        ModuleManager::getInstance().get<Groundstation::Buses>()->ethernet_bus,
         ethernet::cs::getPin(), ethernet::intr::getPin(),
         SPI::ClockDivider::DIV_64);
 
-    // First check if the device is even connected
-    bool present = wiz5500->checkVersion();
-
-    ModuleManager::getInstance().get<BoardStatus>()->setEthernetPresent(
-        present);
-
-    if (present)
-    {
-        if (!EthernetBase::start(std::move(wiz5500)))
-        {
-            return false;
-        }
-    }
-
-    return true;
+    return EthernetBase::start(std::move(wiz5500));
 }
\ No newline at end of file

src/boards/Groundstation/Base/Ports/Ethernet.h

@@ -26,13 +26,13 @@
 
 #include <utils/ModuleManager/ModuleManager.hpp>
 
-namespace GroundstationBase
+namespace Groundstation
 {
 
-class Ethernet : public Groundstation::EthernetBase, public Boardcore::Module
+class Ethernet : public EthernetBase, public Boardcore::Module
 {
 public:
     [[nodiscard]] bool start();
 };
 
-}  // namespace GroundstationBase
+}  // namespace Groundstation
\ No newline at end of file

src/boards/Groundstation/Common/Config/EthernetConfig.h

@@ -22,10 +22,10 @@
 
 #pragma once
 
-#include <drivers/WIZ5500/WIZ5500.h>
-
 #include <cstdint>
 
+#include <drivers/WIZ5500/WIZ5500.h>
+
 namespace Groundstation
 {
 
@@ -37,4 +37,4 @@ constexpr Boardcore::WizIp IP_BASE   = {192, 168, 1, 0};
 constexpr Boardcore::WizIp GATEWAY = {192, 168, 1, 1};
 constexpr Boardcore::WizIp SUBNET = {0, 0, 0, 0};
 
-}  // namespace Groundstation
+}
\ No newline at end of file

src/boards/Groundstation/Common/Ports/EthernetBase.cpp

@@ -20,27 +20,25 @@
  * THE SOFTWARE.
  */
 
-#include "EthernetBase.h"
+#include <random>
 
-#include <Groundstation/Common/Config/EthernetConfig.h>
 #include <Groundstation/Common/HubBase.h>
+#include <Groundstation/Common/Config/EthernetConfig.h>
 
-#include <random>
+#include "EthernetBase.h"
 
 using namespace Groundstation;
 using namespace Boardcore;
 using namespace miosix;
 
-WizIp Groundstation::genNewRandomIp()
-{
+WizIp Groundstation::genNewRandomIp() {
     WizIp ip = IP_BASE;
     ip.d = (rand() % 253) + 1; // Generate in range 1-254
 
     return ip;
 }
 
-WizMac Groundstation::genNewRandomMac()
-{
+WizMac Groundstation::genNewRandomMac() {
     WizMac mac = MAC_BASE;
     mac.e = (rand() % 253) + 1; // Generate in range 1-254
     mac.f = (rand() % 253) + 1; // Generate in range 1-254
@@ -50,7 +48,7 @@ WizMac Groundstation::genNewRandomMac()
 
 void EthernetBase::handleINTn()
 {
-    if (wiz5500)
+    if (started)
     {
         wiz5500->handleINTn();
     }
@@ -58,15 +56,12 @@ void EthernetBase::handleINTn()
 
 void EthernetBase::sendMsg(const mavlink_message_t& msg)
 {
-    if (mav_driver && mav_driver->isStarted())
-    {
-        mav_driver->enqueueMsg(msg);
-    }
+    if(started) {
+        uint8_t msg_buf[MAVLINK_NUM_NON_PAYLOAD_BYTES +
+                        MAVLINK_MAX_DIALECT_PAYLOAD_SIZE];
+        int msg_len = mavlink_msg_to_send_buffer(msg_buf, &msg);
+        wiz5500->send(0, msg_buf, msg_len, 100);
     }
-
-Boardcore::Wiz5500::PhyState EthernetBase::getState()
-{
-    return wiz5500->getPhyState();
 }
 
 bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
@@ -74,7 +69,10 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
     this->wiz5500 = std::move(wiz5500);
 
     // Reset the device
-    this->wiz5500->reset();
+    if (!this->wiz5500->reset())
+    {
+        return false;
+    }
 
     // Setup ip and other stuff
     this->wiz5500->setSubnetMask(SUBNET);
@@ -82,43 +80,59 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
     this->wiz5500->setSourceIp(genNewRandomIp());
     this->wiz5500->setSourceMac(genNewRandomMac());
 
-    this->wiz5500->setOnIpConflict(
-        [this]() { this->wiz5500->setSourceIp(genNewRandomIp()); });
+    this->wiz5500->setOnIpConflict([this]() {
+        this->wiz5500->setSourceIp(genNewRandomIp());
+    });
 
     // Ok now open the UDP socket
-    if (!this->wiz5500->openUdp(0, RECV_PORT, {255, 255, 255, 255}, SEND_PORT,
-                                500))
+    if (!this->wiz5500->openUdp(0, RECV_PORT, {255, 255, 255, 255}, SEND_PORT, 500))
     {
         return false;
     }
 
-    auto mav_handler = [this](EthernetMavDriver* channel,
-                              const mavlink_message_t& msg) { handleMsg(msg); };
-
-    mav_driver = std::make_unique<EthernetMavDriver>(this, mav_handler, 0, 10);
-
-    if (!mav_driver->start())
+    if (!ActiveObject::start())
     {
         return false;
     }
 
+    started = true;
     return true;
 }
 
-void EthernetBase::handleMsg(const mavlink_message_t& msg)
+void EthernetBase::run()
 {
-    // Dispatch the message through the hub.
-    ModuleManager::getInstance().get<HubBase>()->dispatchOutgoingMsg(msg);
-}
+    mavlink_message_t msg;
+    mavlink_status_t status;
+    uint8_t msg_buf[256];
 
-ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len)
+    while (!shouldStop())
     {
         WizIp dst_ip;
         uint16_t dst_port;
-    return wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port);
+        ssize_t rcv_len = this->wiz5500->recvfrom(0, msg_buf, sizeof(msg_buf),
+                                                  dst_ip, dst_port, 500);
+
+        if (rcv_len == -1)
+        {
+            // Avoid spin looping, failure are highly likely to happen in
+            // sequence
+            Thread::sleep(100);
         }
+        else
+        {
+            for (ssize_t i = 0; i < rcv_len; i++)
+            {
+                uint8_t parse_result = mavlink_parse_char(
+                    MAVLINK_COMM_0, msg_buf[i], &msg, &status);
 
-bool EthernetBase::send(uint8_t* pkt, size_t len)
+                if (parse_result == 1)
                 {
-    return wiz5500->send(0, pkt, len, 100);
+                    // Dispatch the message through the hub.
+                    ModuleManager::getInstance()
+                        .get<HubBase>()
+                        ->dispatchOutgoingMsg(msg);
+                }
+            }
+        }
+    }
 }
\ No newline at end of file

src/boards/Groundstation/Common/Ports/Serial.cpp

@@ -35,11 +35,13 @@ bool Serial::start()
         return false;
     }
 
+    started = true;
     return true;
 }
 
 void Serial::sendMsg(const mavlink_message_t &msg)
 {
+    if(started) {
         Lock<FastMutex> l(mutex);
         uint8_t msg_buf[MAVLINK_NUM_NON_PAYLOAD_BYTES +
                         MAVLINK_MAX_DIALECT_PAYLOAD_SIZE];
@@ -48,6 +50,7 @@ void Serial::sendMsg(const mavlink_message_t &msg)
         auto serial = miosix::DefaultConsole::instance().get();
         serial->writeBlock(msg_buf, msg_len, 0);
     }
+}
 
 void Serial::run()
 {

src/boards/Groundstation/Common/Ports/Serial.h

@@ -46,13 +46,13 @@ public:
      */
     void sendMsg(const mavlink_message_t& msg);
 
-protected:
+private:
     /**
      * @brief Internal run method
      */
     void run() override;
 
-private:
+    bool started = false;
     miosix::FastMutex mutex;
 };
 

src/entrypoints/Groundstation/base-groundstation-entry.cpp

@@ -22,6 +22,7 @@
 
 #include <Groundstation/Base/Buses.h>
 #include <Groundstation/Base/Hub.h>
+#include <Groundstation/Base/Ports/Ethernet.h>
 #include <Groundstation/Base/Radio/Radio.h>
 #include <Groundstation/Base/Radio/RadioStatus.h>
 #include <Groundstation/Common/Ports/Serial.h>
@@ -57,6 +58,7 @@ int main()
     Hub *hub                    = new Hub();
     Buses *buses                = new Buses();
     Serial *serial              = new Serial();
+    Ethernet *ethernet          = new Ethernet();
     RadioMain *radio_main       = new RadioMain();
     RadioPayload *radio_payload = new RadioPayload();
     RadioStatus *radio_status   = new RadioStatus();
@@ -68,6 +70,7 @@ int main()
     ok &= modules.insert<HubBase>(hub);
     ok &= modules.insert(buses);
     ok &= modules.insert(serial);
+    ok &= modules.insert(ethernet);
     ok &= modules.insert(radio_main);
     ok &= modules.insert(radio_payload);
     ok &= modules.insert(radio_status);
@@ -87,6 +90,12 @@ int main()
         printf("[error] Failed to start serial!\n");
     }
 
+    ok &= ethernet->start();
+    if (!ok)
+    {
+        printf("[error] Failed to start ethernet!\n");
+    }
+
     ok &= radio_main->start();
     if (!ok)
     {