diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 2f83d5b7..2cd7abd9 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -7,6 +7,12 @@ // `sudo usermod -aG dialout tuas && newgrp && bash` // "runArgs": ["--device=/dev/ttyACM0"], + // 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 243fb38e..71f3ad10 100644 --- a/include/airdrop/packet.h +++ b/include/airdrop/packet.h @@ -4,7 +4,7 @@ #include /********************************************************************************************** - * LAST UPDATED: 2024/02/29 + * LAST UPDATED: 2024/04/15 by Tyler Lentz * * This file defines the packet definitions for Airdrop communications. It is used in two * separate repositories: @@ -27,18 +27,23 @@ #define AD_OBC_PORT 45906 // OBC listens on this port, so payloads send to this #define AD_PAYLOAD_PORT 45907 // Payloads listen on this port, so OBC sends to this -// All of the values in here are 1 byte long, which means we do not have to worry about -// the endianness of the information as it is sent over the wire. If we ever add values more -// than one byte long, we'll have to worry about this. - -// gcc specific packing syntax. Technically this should already be packed because it is just -// two one-byte values, but just making sure. +// Normal packet, which applies for every kind of packet that is not an ad_latlng_packet struct __attribute__((packed)) ad_packet { uint8_t hdr; uint8_t data; }; typedef struct ad_packet ad_packet_t; +// Separate kind of packet sent by OBC to Payloads to specify drop location +// for indirect drop +struct __attribute__((packed)) ad_latlng_packet { + uint8_t hdr; + uint8_t bottle; + double lat; + double lng; +}; +typedef struct ad_latlng_packet ad_latlng_packet_t; + enum ad_packet_hdr { // Continually sent by the payloads to confirm they are still operational // and can talk to the OBC @@ -47,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, @@ -56,6 +62,8 @@ enum ad_packet_hdr { ACK_SIGNAL = 201, REVOKE = 202, ACK_REVOKE = 203, + SEND_LATLNG = 204, // special packet format! + ACK_LATLNG = 205, // Direct & Indirect ABOUT_TO_RELEASE = 255, @@ -84,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: @@ -95,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/include/network/airdrop_client.hpp b/include/network/airdrop_client.hpp index 037d8c81..305907e0 100644 --- a/include/network/airdrop_client.hpp +++ b/include/network/airdrop_client.hpp @@ -24,6 +24,7 @@ class AirdropClient { ~AirdropClient(); bool send(ad_packet_t packet); + bool send(ad_latlng_packet_t packet); // Receives oldest packet since last receive() call, ignoring any // HEARTBEAT packets as those are parsed by the client itself // and exposed through the TODO function. diff --git a/include/network/airdrop_sockets.h b/include/network/airdrop_sockets.h index 0ebb8a3e..aa5223bc 100644 --- a/include/network/airdrop_sockets.h +++ b/include/network/airdrop_sockets.h @@ -67,6 +67,7 @@ ad_int_result_t set_socket_nonblocking(int sock_fd); // Either returns an error string or the number of bytes sent. // IMPORTANT: must have previously called set_send_thread from curr thread. ad_int_result_t send_ad_packet(ad_socket_t socket, ad_packet_t packet); +ad_int_result_t send_ad_latlng_packet(ad_socket_t socket, ad_latlng_packet_t packet); // Receive packet and place into buf, which is of length buf_len. // Either returns an error string or the number of bytes read. diff --git a/protos b/protos index b7d3e9b7..c5841043 160000 --- a/protos +++ b/protos @@ -1 +1 @@ -Subproject commit b7d3e9b74b38046602d4631072e6777b0b59b273 +Subproject commit c5841043ba805c065997232b5a6ae281873d9969 diff --git a/src/network/airdrop_client.cpp b/src/network/airdrop_client.cpp index 7e444739..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); } @@ -72,6 +74,20 @@ bool AirdropClient::send(ad_packet_t packet) { return true; } +bool AirdropClient::send(ad_latlng_packet_t packet) { + set_send_thread(); + + auto res = send_ad_latlng_packet(this->socket, packet); + if (res.is_err) { + LOG_F(ERROR, "%s", res.data.err); + return false; + } + + // TODO: helper to go from packet -> str + LOG_F(INFO, "Sent airdrop latlng packet: %hhu %hhu", packet.hdr, packet.bottle); + return true; +} + std::optional AirdropClient::receive() { Lock lock(this->recv_mut); @@ -159,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); } @@ -174,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/src/network/airdrop_sockets.c b/src/network/airdrop_sockets.c index 12a3014e..9e38829c 100644 --- a/src/network/airdrop_sockets.c +++ b/src/network/airdrop_sockets.c @@ -118,6 +118,26 @@ ad_int_result_t send_ad_packet(ad_socket_t socket, ad_packet_t packet) { AD_RETURN_SUCC_RESULT(int, bytes_sent); } +ad_int_result_t send_ad_latlng_packet(ad_socket_t socket, ad_latlng_packet_t packet) { + struct sockaddr_in SEND_ADDR = { + .sin_family = AF_INET, + .sin_port = htons(socket.send_port), + .sin_addr = INADDR_BROADCAST, + .sin_zero = {0}, + }; + + static char err[AD_ERR_LEN]; + int bytes_sent = sendto(socket.fd, (void*) &packet, sizeof(ad_latlng_packet_t), 0, + (struct sockaddr*) &SEND_ADDR, sizeof(SEND_ADDR)); + + if (bytes_sent < 0) { + snprintf(&err[0], AD_ERR_LEN, "send failed: %s", strerror_l(errno, _send_locale)); + AD_RETURN_ERR_RESULT(int, err); + } + + AD_RETURN_SUCC_RESULT(int, bytes_sent); +} + ad_int_result_t recv_ad_packet(ad_socket_t socket, void* buf, size_t buf_len) { static char err[AD_ERR_LEN]; struct sockaddr_in RECV_ADDR = { diff --git a/src/network/mock/airdrop_sockets.c b/src/network/mock/airdrop_sockets.c index e59a5c18..e29035dc 100644 --- a/src/network/mock/airdrop_sockets.c +++ b/src/network/mock/airdrop_sockets.c @@ -70,6 +70,11 @@ ad_int_result_t send_ad_packet(ad_socket_t socket, ad_packet_t packet) { AD_RETURN_SUCC_RESULT(int, sizeof(ad_packet_t)); } +ad_int_result_t send_ad_latlng_packet(ad_socket_t socket, ad_latlng_packet_t packet) { + // TODO: Mock sending ad_latlng_packets + fprintf(stderr, "ERR: haven't yet implemented sending ad latlng packets in mock"); +} + ad_int_result_t recv_ad_packet(ad_socket_t socket, void* buf, size_t buf_len) { static char err[1] = ""; 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++; } }