From edcfbb680bb1c504d534b0ed61eb275bb2c91e95 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Thu, 18 Apr 2024 15:49:32 -0700 Subject: [PATCH 1/2] Feat/conn status (#156) * add /connection handler func * add err msg if gcs server fails to start up * implement connection route * fix lint * fix default value not being 0.0 for rcStrength --- .devcontainer/devcontainer.json | 5 +++- include/network/gcs_macros.hpp | 5 ++++ include/network/gcs_routes.hpp | 9 +++++++ include/network/mavlink.hpp | 1 + protos | 2 +- src/network/gcs.cpp | 9 ++++++- src/network/gcs_routes.cpp | 43 +++++++++++++++++++++++++++++++++ src/network/mavlink.cpp | 4 +++ 8 files changed, 75 insertions(+), 3 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 2f83d5b7..48852230 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,11 +1,14 @@ // See this page for reference of options: https://containers.dev/implementors/json_reference { "name": "Existing Dockerfile", - "image": "ghcr.io/tritonuas/obcpp:x86", + "image": "ghcr.io/tritonuas/obcpp:main", // enable when need to connect over USB to pixhawk // also: need to run obcpp with sudo or add tuas user to dialout group with // `sudo usermod -aG dialout tuas && newgrp && bash` // "runArgs": ["--device=/dev/ttyACM0"], + // "runArgs": [ + // "--network=host" + // ], "customizations": { "vscode": { diff --git a/include/network/gcs_macros.hpp b/include/network/gcs_macros.hpp index ae1d1a19..11303615 100644 --- a/include/network/gcs_macros.hpp +++ b/include/network/gcs_macros.hpp @@ -107,6 +107,11 @@ if (request.has_header("User-Agent")) \ LOG_F(INFO, "User-Agent: %s", request.get_header_value("User-Agent").c_str()); +#define LOG_REQUEST_TRACE(method, route) \ + VLOG_SCOPE_F(TRACE, "%s %s", method, route); \ + if (request.has_header("User-Agent")) \ + VLOG_F(TRACE, "User-Agent: %s", request.get_header_value("User-Agent").c_str()); + // One of the LOG_RESPONSE logging functions should be used to both log and // set the HTTP response #define LOG_RESPONSE_5(LOG_LEVEL, msg, response_code, body, mime) \ diff --git a/include/network/gcs_routes.hpp b/include/network/gcs_routes.hpp index 58fef3d1..f81a86f1 100644 --- a/include/network/gcs_routes.hpp +++ b/include/network/gcs_routes.hpp @@ -17,6 +17,15 @@ #include "ticks/tick.hpp" #include "ticks/path_gen.hpp" +/* + * GET /connection + * --- + * Returns information about the connection status of the OBC + * + * 200 OK: Successfully retrieved data + */ +DEF_GCS_HANDLE(Get, connections); + /* * GET /tick * --- diff --git a/include/network/mavlink.hpp b/include/network/mavlink.hpp index 86ed6eca..3e177202 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -64,6 +64,7 @@ class MavlinkClient { double airspeed_m_s(); double heading_deg(); mavsdk::Telemetry::FlightMode flight_mode(); + mavsdk::Telemetry::RcStatus get_conn_status(); private: mavsdk::Mavsdk mavsdk; diff --git a/protos b/protos index abe95730..c5841043 160000 --- a/protos +++ b/protos @@ -1 +1 @@ -Subproject commit abe95730634b146f39fb5127ee96bac511e9cbd5 +Subproject commit c5841043ba805c065997232b5a6ae281873d9969 diff --git a/src/network/gcs.cpp b/src/network/gcs.cpp index c890ff49..970c7c3c 100644 --- a/src/network/gcs.cpp +++ b/src/network/gcs.cpp @@ -33,7 +33,9 @@ GCSServer::GCSServer(uint16_t port, std::shared_ptr state) this->server_thread = std::thread([this, port]() { loguru::set_thread_name("gcs server"); LOG_F(INFO, "Starting GCS HTTP server on port %d", port); - this->server.listen("0.0.0.0", port); + if (!this->server.listen("0.0.0.0", port)) { + LOG_F(ERROR, "ERROR: GCS server stopped!"); + } LOG_F(INFO, "GCS Server stopped on port %d", port); }); } @@ -46,6 +48,11 @@ GCSServer::~GCSServer() { } void GCSServer::_bindHandlers() { + this->server.Get("/", [](const httplib::Request& request, httplib::Response& response) { + response.status = 200; + response.set_content("Fort-nite", "text/plain"); + }); + BIND_HANDLER(Get, connections); BIND_HANDLER(Get, tick); BIND_HANDLER(Get, mission); BIND_HANDLER(Post, mission); diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index b2825f72..fe619c67 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -16,6 +16,8 @@ #include "ticks/path_gen.hpp" #include "ticks/path_validate.hpp" +using namespace std::chrono_literals; // NOLINT + /* * This file defines all of the GCS handler functions for every route * the gcs server is listening on. @@ -34,6 +36,47 @@ * the LOG_RESPONSE macro will handle it for you. */ +DEF_GCS_HANDLE(Get, connections) { + LOG_REQUEST_TRACE("GET", "/connections"); + + std::list> lost_airdrop_conns; + if (state->getAirdrop() == nullptr) { + lost_airdrop_conns.push_back({BottleDropIndex::A, 99999ms}); + lost_airdrop_conns.push_back({BottleDropIndex::B, 99999ms}); + lost_airdrop_conns.push_back({BottleDropIndex::C, 99999ms}); + lost_airdrop_conns.push_back({BottleDropIndex::D, 99999ms}); + lost_airdrop_conns.push_back({BottleDropIndex::E, 99999ms}); + } else { + lost_airdrop_conns = state->getAirdrop()->getLostConnections(3s); + } + + mavsdk::Telemetry::RcStatus mav_conn; + + if (state->getMav() == nullptr) { + mav_conn.is_available = false; + mav_conn.signal_strength_percent = 0.0; + } else { + mav_conn = state->getMav()->get_conn_status(); + } + + // TODO: query the camera status + bool camera_good = false; + + OBCConnInfo info; + for (auto const& [bottle_index, ms_since_last_heartbeat] : lost_airdrop_conns) { + info.add_dropped_bottle_idx(bottle_index); + info.add_ms_since_ad_heartbeat(ms_since_last_heartbeat.count()); + } + info.set_mav_rc_good(mav_conn.is_available); + info.set_mav_rc_strength(mav_conn.signal_strength_percent); + info.set_camera_good(camera_good); + + std::string output; + google::protobuf::util::MessageToJsonString(info, &output); + + LOG_RESPONSE(TRACE, "Returning conn info", OK, output.c_str(), mime::json); +} + DEF_GCS_HANDLE(Get, tick) { LOG_REQUEST("GET", "/tick"); diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 26a1169c..ea2297bb 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -276,3 +276,7 @@ mavsdk::Telemetry::FlightMode MavlinkClient::flight_mode() { Lock lock(this->data_mut); return this->data.flight_mode; } + +mavsdk::Telemetry::RcStatus MavlinkClient::get_conn_status() { + return this->telemetry->rc_status(); +} From 23b193c6cae6fe17fda8d1410dff4c029bc069b1 Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Thu, 18 Apr 2024 15:50:42 -0700 Subject: [PATCH 2/2] Airdrop Connection Test (#155) * update airdrop protocol * fix lint * connection fix from 4/15/24 testing w/ Cody * remove testing stuff * fix lint * fix unit test * update protos --- .devcontainer/devcontainer.json | 6 ++++++ include/airdrop/packet.h | 3 ++- src/network/airdrop_client.cpp | 10 ++++++++++ tests/unit/airdrop_client/airdrop_client_test.cpp | 8 +++++--- 4 files changed, 23 insertions(+), 4 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 48852230..57467682 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -10,6 +10,12 @@ // "--network=host" // ], + // Enable network mode host if on Linux and testing airdrop connectivity + // "appPort": [ "45906:45906/udp", "45907:45907/udp" ], // port forward airdrop ports for local testing + // "runArgs": [ + // "--network=host" + // ], + "customizations": { "vscode": { "settings": { diff --git a/include/airdrop/packet.h b/include/airdrop/packet.h index ae7ac18f..71f3ad10 100644 --- a/include/airdrop/packet.h +++ b/include/airdrop/packet.h @@ -52,6 +52,7 @@ enum ad_packet_hdr { // Handshake to establish connection SET_MODE = 1, ACK_MODE = 2, + RESET_MODE = 3, // TODO: // Direct Drop DROP_NOW = 100, @@ -91,7 +92,6 @@ inline int validate_packet_as(enum ad_packet_hdr hdr, ad_packet_t packet) { } switch (packet.hdr) { - case HEARTBEAT: case ACK_MODE: return 1; case SET_MODE: @@ -102,6 +102,7 @@ inline int validate_packet_as(enum ad_packet_hdr hdr, ad_packet_t packet) { case REVOKE: case ACK_REVOKE: case ABOUT_TO_RELEASE: + case HEARTBEAT: return (packet.data >= BOTTLE_A && packet.data <= BOTTLE_E); default: return 0; diff --git a/src/network/airdrop_client.cpp b/src/network/airdrop_client.cpp index fcb4d385..23127e21 100644 --- a/src/network/airdrop_client.cpp +++ b/src/network/airdrop_client.cpp @@ -55,6 +55,8 @@ void AirdropClient::_establishConnection() { LOG_F(INFO, "Payload connection established in %s mode", (this->mode == DIRECT_DROP) ? "Direct" : "Indirect"); + send_ad_packet(this->socket, make_ad_packet(ad_packet_hdr::ACK_MODE, *this->mode)); + this->worker_future = std::async(std::launch::async, &AirdropClient::_receiveWorker, this); } @@ -173,6 +175,12 @@ void AirdropClient::_receiveWorker() { continue; // heartbeat, so we should not put it in the queue } + if (packet.hdr == SET_MODE) { + send_ad_packet(this->socket, make_ad_packet(ad_packet_hdr::ACK_MODE, *this->mode)); + LOG_F(INFO, "Received extra SET_MODE, reacking"); + continue; + } + Lock lock(this->recv_mut); this->recv_queue.emplace(packet); } @@ -188,5 +196,7 @@ bool AirdropClient::_parseHeartbeats(ad_packet_t packet) { // subtract 1 to get the index into the lastHeartbeat array. this->last_heartbeat[packet.data - 1] = getUnixTime_ms(); + LOG_F(INFO, "Packet heartbeat from %d", packet.data); + return true; } diff --git a/tests/unit/airdrop_client/airdrop_client_test.cpp b/tests/unit/airdrop_client/airdrop_client_test.cpp index a28a22a1..7e276790 100644 --- a/tests/unit/airdrop_client/airdrop_client_test.cpp +++ b/tests/unit/airdrop_client/airdrop_client_test.cpp @@ -85,10 +85,12 @@ TEST(AirdropClientTest, ObcSendToPayload) { }); ad_packet_t p = { 0 }; - while (num_received < NUM_TO_SEND) { + while (num_received < NUM_TO_SEND + 1) { recv_ad_packet(payload_socket, &p, sizeof(ad_packet_t)); - ASSERT_EQ(p.hdr, SIGNAL); - ASSERT_EQ(p.data, num_received); + if (num_received != 0) { // ignore first because will be ack_mode + ASSERT_EQ(p.hdr, SIGNAL); + ASSERT_EQ(p.data, num_received - 1); + } num_received++; } }