Merge branch 'main' into feat/airdrop-pathing-config

.devcontainer/devcontainer.json

@@ -1,11 +1,20 @@
 // 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"
+	// ],
+
+	// 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": {

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;

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) \

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
  * ---

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;

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;
 }

src/network/gcs.cpp

@@ -33,7 +33,9 @@ GCSServer::GCSServer(uint16_t port, std::shared_ptr<MissionState> 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);

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<std::pair<BottleDropIndex, std::chrono::milliseconds>> 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");
 

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();
+}

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++;
     }
 }