diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 2bc679a566b467a3d245be3657414d17a934bb12..1d98ecb875b2149785bbb8a4a512dbcd243810d6 100644 --- a/cmake/dependencies.cmake +++ b/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 diff --git a/src/boards/Groundstation/Base/Hub.cpp b/src/boards/Groundstation/Base/Hub.cpp index abea1b3d18ec813f01da72d0991a93492bbebc09..13db82be7f483aa0f4e7874cbbc2aca52334d9ec 100644 --- a/src/boards/Groundstation/Base/Hub.cpp +++ b/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 diff --git a/src/boards/Groundstation/Base/Ports/Ethernet.cpp b/src/boards/Groundstation/Base/Ports/Ethernet.cpp index 875c083cfee7d3869e1c0ede395c2ccda207b131..cc99a1eca6bb4d18862c837c305fec5486f536d6 100644 --- a/src/boards/Groundstation/Base/Ports/Ethernet.cpp +++ b/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 diff --git a/src/boards/Groundstation/Base/Ports/Ethernet.h b/src/boards/Groundstation/Base/Ports/Ethernet.h index 4a8283b1915e0600dff19e94d191038b88d9274a..fb3988478cd282631e1c3417cb01e826eaf68101 100644 --- a/src/boards/Groundstation/Base/Ports/Ethernet.h +++ b/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 \ No newline at end of file +} // namespace Groundstation \ No newline at end of file diff --git a/src/boards/Groundstation/Common/Config/EthernetConfig.h b/src/boards/Groundstation/Common/Config/EthernetConfig.h index ddb9872a061c453c240f1d61c6b77cb189b333ea..49edccd5a1415bf0e2f2096c1ca8dabafa3c25aa 100644 --- a/src/boards/Groundstation/Common/Config/EthernetConfig.h +++ b/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 { @@ -33,8 +33,8 @@ constexpr uint16_t RECV_PORT = 42070; constexpr uint16_t SEND_PORT = 42069; constexpr Boardcore::WizMac MAC_BASE = {0x69, 0x69, 0x69, 0x69, 0, 0}; -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}; +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 +} \ No newline at end of file diff --git a/src/boards/Groundstation/Common/Ports/EthernetBase.cpp b/src/boards/Groundstation/Common/Ports/EthernetBase.cpp index f6c627b234ded251bbd0d6f44bc914e269c69063..40b0122985dfdf2a00a4dff9f5c1c8f9aba8ec0b 100644 --- a/src/boards/Groundstation/Common/Ports/EthernetBase.cpp +++ b/src/boards/Groundstation/Common/Ports/EthernetBase.cpp @@ -20,37 +20,35 @@ * 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 + 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 + mac.e = (rand() % 253) + 1; // Generate in range 1-254 + mac.f = (rand() % 253) + 1; // Generate in range 1-254 return mac; } void EthernetBase::handleINTn() { - if (wiz5500) + if (started) { wiz5500->handleINTn(); } @@ -58,23 +56,23 @@ 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) { 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) -{ - // Dispatch the message through the hub. - ModuleManager::getInstance().get<HubBase>()->dispatchOutgoingMsg(msg); -} - -ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len) +void EthernetBase::run() { - WizIp dst_ip; - uint16_t dst_port; - return wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port); -} + mavlink_message_t msg; + mavlink_status_t status; + uint8_t msg_buf[256]; -bool EthernetBase::send(uint8_t* pkt, size_t len) -{ - return wiz5500->send(0, pkt, len, 100); + while (!shouldStop()) + { + WizIp dst_ip; + uint16_t 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); + + if (parse_result == 1) + { + // Dispatch the message through the hub. + ModuleManager::getInstance() + .get<HubBase>() + ->dispatchOutgoingMsg(msg); + } + } + } + } } \ No newline at end of file diff --git a/src/boards/Groundstation/Common/Ports/Serial.cpp b/src/boards/Groundstation/Common/Ports/Serial.cpp index 017a6783c912653b6ea902f59d82b0c3fccbf873..de46f3577e51eeacf386d9704d2a1fb937d25dde 100644 --- a/src/boards/Groundstation/Common/Ports/Serial.cpp +++ b/src/boards/Groundstation/Common/Ports/Serial.cpp @@ -35,18 +35,21 @@ bool Serial::start() return false; } + started = true; return true; } void Serial::sendMsg(const mavlink_message_t &msg) { - Lock<FastMutex> l(mutex); - 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); - - auto serial = miosix::DefaultConsole::instance().get(); - serial->writeBlock(msg_buf, msg_len, 0); + if(started) { + Lock<FastMutex> l(mutex); + 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); + + auto serial = miosix::DefaultConsole::instance().get(); + serial->writeBlock(msg_buf, msg_len, 0); + } } void Serial::run() diff --git a/src/boards/Groundstation/Common/Ports/Serial.h b/src/boards/Groundstation/Common/Ports/Serial.h index 46845b0cdff9778ebb60766376ee5dd37a06833e..a0318823ca8a5ad4ed0c4d4c46aa6d784912a455 100644 --- a/src/boards/Groundstation/Common/Ports/Serial.h +++ b/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; }; diff --git a/src/entrypoints/Groundstation/base-groundstation-entry.cpp b/src/entrypoints/Groundstation/base-groundstation-entry.cpp index 48d92498d742f90724c6f38871faf1630ebfcd48..8afa9771f86c3de2373eb5effb361664e5946c8e 100644 --- a/src/entrypoints/Groundstation/base-groundstation-entry.cpp +++ b/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) {