diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 1d98ecb875b2149785bbb8a4a512dbcd243810d6..1e336464370c65fc526deaa9a185ba7024d31198 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -44,8 +44,8 @@ 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/BoardStatus.cpp src/boards/Groundstation/Base/Hub.cpp ) diff --git a/src/boards/Groundstation/Base/Hub.cpp b/src/boards/Groundstation/Base/Hub.cpp index 16ae7325912e24556f62f066f4e9e9dede0d0794..df638223157403f07744eee70bf5bdd3217f55e7 100644 --- a/src/boards/Groundstation/Base/Hub.cpp +++ b/src/boards/Groundstation/Base/Hub.cpp @@ -24,7 +24,7 @@ #include <Groundstation/Base/Ports/Ethernet.h> #include <Groundstation/Base/Radio/Radio.h> -#include <Groundstation/Base/Radio/RadioStatus.h> +#include <Groundstation/Base/BoardStatus.h> #include <Groundstation/Common/Config/GeneralConfig.h> #include <Groundstation/Common/Ports/Serial.h> @@ -34,7 +34,7 @@ using namespace Boardcore; void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) { - RadioStatus* status = ModuleManager::getInstance().get<RadioStatus>(); + BoardStatus* status = ModuleManager::getInstance().get<BoardStatus>(); bool send_ok = false; @@ -51,17 +51,22 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) } // If both of the sends went wrong, just send a nack - if (!send_ok) - { - sendNack(msg); - } + // This doesn't work well with multiple GS on the same ethernet network + // if (!send_ok) + // { + // sendNack(msg); + // } } void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) { + BoardStatus* status = ModuleManager::getInstance().get<BoardStatus>(); + Serial* serial = ModuleManager::getInstance().get<Serial>(); serial->sendMsg(msg); - Ethernet* ethernet = ModuleManager::getInstance().get<Ethernet>(); - ethernet->sendMsg(msg); + if (status->isEthernetPresent()) { + 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 9034f7173f705dfce0e6671018ba214525c0b13c..94e80dd040f070d4bdd8cbfe3637bf9bb9bb75f8 100644 --- a/src/boards/Groundstation/Base/Ports/Ethernet.cpp +++ b/src/boards/Groundstation/Base/Ports/Ethernet.cpp @@ -22,6 +22,7 @@ #include "Ethernet.h" +#include <Groundstation/Base/BoardStatus.h> #include <Groundstation/Base/Buses.h> #include <interfaces-impl/hwmapping.h> @@ -42,5 +43,17 @@ bool Ethernet::start() ethernet::cs::getPin(), ethernet::intr::getPin(), SPI::ClockDivider::DIV_64); - return EthernetBase::start(std::move(wiz5500)); + // 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; } \ No newline at end of file diff --git a/src/boards/Groundstation/Base/Radio/Radio.cpp b/src/boards/Groundstation/Base/Radio/Radio.cpp index fbd12ea8f014eece0dd00a1746e189432b684766..e27d656aca90bd6638d7ab0cdf14eaddca1a6ee3 100644 --- a/src/boards/Groundstation/Base/Radio/Radio.cpp +++ b/src/boards/Groundstation/Base/Radio/Radio.cpp @@ -24,7 +24,7 @@ #include <Groundstation/Base/Buses.h> #include <Groundstation/Base/Hub.h> -#include <Groundstation/Base/Radio/RadioStatus.h> +#include <Groundstation/Base/BoardStatus.h> #include <Groundstation/Common/Ports/Serial.h> #include <radio/SX1278/SX1278Frontends.h> @@ -84,7 +84,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) @@ -127,7 +127,7 @@ bool RadioPayload::start() // First check if the device is even connected bool present = sx1278->checkVersion(); - ModuleManager::getInstance().get<RadioStatus>()->setPayloadRadioPresent( + ModuleManager::getInstance().get<BoardStatus>()->setPayloadRadioPresent( present); if (present) diff --git a/src/boards/Groundstation/Base/Radio/RadioStatus.cpp b/src/boards/Groundstation/Base/Radio/RadioStatus.cpp deleted file mode 100644 index 12a1a2b15aa456cdeee3391c0a206c3a998b7329..0000000000000000000000000000000000000000 --- a/src/boards/Groundstation/Base/Radio/RadioStatus.cpp +++ /dev/null @@ -1,109 +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/Base/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 GroundstationBase; - -bool RadioStatus::isMainRadioPresent() { return main_radio_present; } -bool RadioStatus::isPayloadRadioPresent() { return payload_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; - } - - if (payload_radio_present) - { - tm.payload_radio_present = 1; - - auto stats = - ModuleManager::getInstance().get<RadioPayload>()->getStats(); - tm.payload_packet_tx_error_count = stats.send_errors; - tm.payload_tx_bitrate = - payload_tx_bitrate.update(stats.bits_tx_count); - tm.payload_packet_rx_success_count = stats.packet_rx_success_count; - tm.payload_packet_rx_drop_count = stats.packet_rx_drop_count; - tm.payload_rx_bitrate = - payload_rx_bitrate.update(stats.bits_rx_count); - tm.payload_rx_rssi = stats.rx_rssi; - tm.payload_rx_fei = stats.rx_fei; - - last_payload_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/boards/Groundstation/Base/Radio/RadioStatus.h b/src/boards/Groundstation/Base/Radio/RadioStatus.h deleted file mode 100644 index 49c818cfe5313279a8142f3a7f1a3d0525d10e0b..0000000000000000000000000000000000000000 --- a/src/boards/Groundstation/Base/Radio/RadioStatus.h +++ /dev/null @@ -1,116 +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. - */ - -#pragma once - -#include <ActiveObject.h> -#include <Groundstation/Common/Config/RadioConfig.h> -#include <Groundstation/Common/Radio/RadioBase.h> -#include <utils/collections/CircularBuffer.h> - -#include <utils/ModuleManager/ModuleManager.hpp> - -namespace GroundstationBase -{ - -/** - * @brief Utility to calculate the bitrate - */ -template <unsigned int WINDOW_SIZE, unsigned int PERIOD> -class BitrateCalculator -{ -public: - BitrateCalculator() {} - - /** - * @brief Update the calculator, should be called every PERIOD ms - */ - uint16_t update(uint32_t sample) - { - if (window.isFull()) - { - uint32_t last = window.pop(); - window.put(sample); - - uint16_t delta = sample - last; - - // window size is in ms, we want the result in s - return delta * 1000 / WINDOW_SIZE; - } - else - { - window.put(sample); - return 0; - } - } - -private: - Boardcore::CircularBuffer<uint32_t, WINDOW_SIZE / PERIOD> window; -}; - -/** - * @brief Class responsible for keeping track of radio status and metrics. - */ -class RadioStatus : public Boardcore::Module, private Boardcore::ActiveObject -{ -public: - RadioStatus() {} - - bool start(); - - /** - * @brief Check wether the main radio was found during boot. - */ - bool isMainRadioPresent(); - - /** - * @brief Check wether the payload radio was found during boot. - */ - bool isPayloadRadioPresent(); - - void setMainRadioPresent(bool present); - void setPayloadRadioPresent(bool present); - -private: - void run() override; - - Groundstation::RadioStats last_main_stats = {0}; - Groundstation::RadioStats last_payload_stats = {0}; - - BitrateCalculator<Groundstation::RADIO_BITRATE_WINDOW_SIZE, - Groundstation::RADIO_STATUS_PERIOD> - main_tx_bitrate; - BitrateCalculator<Groundstation::RADIO_BITRATE_WINDOW_SIZE, - Groundstation::RADIO_STATUS_PERIOD> - main_rx_bitrate; - BitrateCalculator<Groundstation::RADIO_BITRATE_WINDOW_SIZE, - Groundstation::RADIO_STATUS_PERIOD> - payload_tx_bitrate; - BitrateCalculator<Groundstation::RADIO_BITRATE_WINDOW_SIZE, - Groundstation::RADIO_STATUS_PERIOD> - payload_rx_bitrate; - - bool main_radio_present = false; - bool payload_radio_present = false; -}; - -} // namespace GroundstationBase \ 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 1475744ebf1b27f2d4cb87d0859f65307daee6a4..f6c627b234ded251bbd0d6f44bc914e269c69063 100644 --- a/src/boards/Groundstation/Common/Ports/EthernetBase.cpp +++ b/src/boards/Groundstation/Common/Ports/EthernetBase.cpp @@ -64,15 +64,17 @@ void EthernetBase::sendMsg(const mavlink_message_t& msg) } } +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 - if (!this->wiz5500->reset()) - { - return false; - } + this->wiz5500->reset(); // Setup ip and other stuff this->wiz5500->setSubnetMask(SUBNET); @@ -111,7 +113,9 @@ void EthernetBase::handleMsg(const mavlink_message_t& msg) ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len) { - return wiz5500->recv(0, pkt, max_len); + WizIp dst_ip; + uint16_t dst_port; + return wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port); } bool EthernetBase::send(uint8_t* pkt, size_t len) diff --git a/src/entrypoints/Groundstation/base-groundstation-entry.cpp b/src/entrypoints/Groundstation/base-groundstation-entry.cpp index 9115906e3d256d135bbe94fdfd1a0182c8e9ae0b..5ed02c045b276e7a3d974ece33b705f850597f5e 100644 --- a/src/entrypoints/Groundstation/base-groundstation-entry.cpp +++ b/src/entrypoints/Groundstation/base-groundstation-entry.cpp @@ -24,7 +24,7 @@ #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/Base/BoardStatus.h> #include <Groundstation/Common/Ports/Serial.h> #include <miosix.h> @@ -62,7 +62,7 @@ int main() Ethernet *ethernet = new Ethernet(); RadioMain *radio_main = new RadioMain(); RadioPayload *radio_payload = new RadioPayload(); - RadioStatus *radio_status = new RadioStatus(); + BoardStatus *board_status = new BoardStatus(); ModuleManager &modules = ModuleManager::getInstance(); @@ -74,7 +74,7 @@ int main() ok &= modules.insert(ethernet); ok &= modules.insert(radio_main); ok &= modules.insert(radio_payload); - ok &= modules.insert(radio_status); + ok &= modules.insert(board_status); // If insertion failed, stop right here if (!ok) @@ -109,22 +109,29 @@ int main() printf("[error] Failed to start payload radio!\n"); } - ok &= radio_status->start(); + ok &= board_status->start(); if (!ok) { - printf("[error] Failed to start radio status!\n"); + printf("[error] Failed to start board status!\n"); } - if (radio_status->isMainRadioPresent()) + if (board_status->isMainRadioPresent()) { + printf("Main radio detected!\n"); led2On(); } - if (radio_status->isPayloadRadioPresent()) + if (board_status->isPayloadRadioPresent()) { + printf("Payload radio detected!\n"); led3On(); } + if (board_status->isEthernetPresent()) + { + printf("Ethernet detected!\n"); + } + if (!ok) { errorLoop();