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