From 0cec23f7f7cc932bcec10b02b5e6871d525367e9 Mon Sep 17 00:00:00 2001 From: PgSocks Date: Tue, 27 Dec 2022 21:14:50 -0600 Subject: [PATCH] Make two connections for SDK and device --- include/message.h | 2 +- src/client.h | 3 ++- src/connection.h | 32 +++++++++++++++++++++++++++++--- src/messages/led.c | 2 +- src/messages/sdk_connection.c | 2 +- src/messages/sdk_mode.c | 2 +- src/messages/set_wheel_speed.c | 2 +- src/robomaster.c | 12 +++++++----- 8 files changed, 43 insertions(+), 14 deletions(-) diff --git a/include/message.h b/include/message.h index 4d94dbb..184beaf 100644 --- a/include/message.h +++ b/include/message.h @@ -147,7 +147,7 @@ union Request { union Response { struct Header header; struct SetSdkConnectionResp sdkconn; - struct SetSdkModeReq sdkmode; + struct SetSdkModeResp sdkmode; struct SetSystemLedResp led; struct SetWheelSpeedResp wheel; }; diff --git a/src/client.h b/src/client.h index 6aba128..cab7fad 100644 --- a/src/client.h +++ b/src/client.h @@ -8,7 +8,8 @@ struct Client { int16_t seq; - struct Connection* connection; + struct Connection* sdk_conn; + struct Connection* dev_conn; }; diff --git a/src/connection.h b/src/connection.h index 1c18c9d..eca31f3 100644 --- a/src/connection.h +++ b/src/connection.h @@ -11,6 +11,7 @@ #include #include #include +#include struct Connection { int sockfd; @@ -21,27 +22,52 @@ struct Connection { inline static struct Connection* -connection_new() +connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip) { struct Connection* conn = malloc(sizeof(struct Connection)); memset(conn, 0, sizeof(struct Connection)); // Request a UDP socket conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0); + + if(source_port && source_ip) { + struct sockaddr_in loc_addr; + loc_addr.sin_family = AF_INET; + loc_addr.sin_port = htons(source_port); + loc_addr.sin_addr.s_addr = inet_addr(source_ip); + if(bind(conn->sockfd, (struct sockaddr*)&loc_addr, sizeof(loc_addr)) < 0) + { + perror("unable to bind local port"); + exit(EXIT_FAILURE); + } + } // Make the socket non-blocking int flags = fcntl(conn->sockfd, F_GETFL); fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK); // Set the address of the drone + memset(&conn->remote_addr, 0, sizeof(conn->remote_addr)); conn->addrlen = sizeof(conn->remote_addr); conn->remote_addr.sin_family = AF_INET; - conn->remote_addr.sin_port = htons(30030); - conn->remote_addr.sin_addr.s_addr = inet_addr("192.168.2.1"); + conn->remote_addr.sin_port = htons(dest_port); + conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip); return conn; } +inline +static +void +connection_set_remote(struct Connection* conn, unsigned int dest_port, const char* dest_ip) +{ + // Set the address of the drone + conn->addrlen = sizeof(conn->remote_addr); + conn->remote_addr.sin_family = AF_INET; + conn->remote_addr.sin_port = htons(dest_port); + conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip); +} + static inline void diff --git a/src/messages/led.c b/src/messages/led.c index 3d6cbdb..70fa9c2 100644 --- a/src/messages/led.c +++ b/src/messages/led.c @@ -28,6 +28,6 @@ set_system_led ( req.led.t2 = t2; req_finalize(session, 0x3F, SET_SYSTEM_LED_CMDID, sizeof(struct SetSystemLedReq), &req); - req_send(session->connection, &req, sizeof(struct SetSystemLedReq)); + req_send(session->dev_conn, &req, sizeof(struct SetSystemLedReq)); } diff --git a/src/messages/sdk_connection.c b/src/messages/sdk_connection.c index 5fc3b01..06dcf21 100644 --- a/src/messages/sdk_connection.c +++ b/src/messages/sdk_connection.c @@ -17,5 +17,5 @@ set_sdk_connection( req.sdkconn.ip_address = ip_address; req.sdkconn.port = port; req_finalize(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(struct SetSdkConnectionReq), &req); - req_send(session->connection, &req, sizeof(struct SetSdkConnectionReq)); + req_send(session->sdk_conn, &req, sizeof(struct SetSdkConnectionReq)); } diff --git a/src/messages/sdk_mode.c b/src/messages/sdk_mode.c index cef1fdc..f15f7e7 100644 --- a/src/messages/sdk_mode.c +++ b/src/messages/sdk_mode.c @@ -9,5 +9,5 @@ set_sdk_mode( union Request req = {0}; req.sdkmode.enable = enable; req_finalize(session, 0x3F, SET_SDK_MODE_CMDID, sizeof(struct SetSdkModeReq), &req); - req_send(session->connection, &req, sizeof(struct SetSdkModeReq)); + req_send(session->dev_conn, &req, sizeof(struct SetSdkModeReq)); } diff --git a/src/messages/set_wheel_speed.c b/src/messages/set_wheel_speed.c index c601eff..724b9fb 100644 --- a/src/messages/set_wheel_speed.c +++ b/src/messages/set_wheel_speed.c @@ -15,5 +15,5 @@ set_wheel_speed ( req.wheel.wheel_speed[2] = w3; req.wheel.wheel_speed[3] = w4; req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, sizeof(struct SetWheelSpeedReq), &req); - req_send(session->connection, &req, sizeof(struct SetWheelSpeedReq)); + req_send(session->sdk_conn, &req, sizeof(struct SetWheelSpeedReq)); } diff --git a/src/robomaster.c b/src/robomaster.c index 96eae25..ce8cef9 100644 --- a/src/robomaster.c +++ b/src/robomaster.c @@ -20,19 +20,22 @@ Client client_new() { } void client_connect(Client client) { - client->connection = connection_new(); + client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1"); + client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1"); set_sdk_connection(client, CONNECTION_WIFI_AP, 0, 10010); } void poll_message(Client client, union Message* resp) { + memset(resp, 0, sizeof(union Message)); + // Poll for messages static struct timeval timeout = {0, 0}; fd_set read_fds; FD_ZERO(&read_fds); - FD_SET(client->connection->sockfd, &read_fds); - int result = select(client->connection->sockfd + 1, &read_fds, NULL, NULL, NULL); + FD_SET(client->sdk_conn->sockfd, &read_fds); + int result = select(client->sdk_conn->sockfd + 1, &read_fds, NULL, NULL, NULL); // Check for socket polling errors if(result < 0) { @@ -47,8 +50,7 @@ void poll_message(Client client, union Message* resp) { } // Read a message from the socket - // TODO: Use union to make all messages same size - int recvb = recvfrom(client->connection->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->connection->remote_addr, &client->connection->addrlen); + int recvb = recvfrom(client->sdk_conn->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->sdk_conn->remote_addr, &client->sdk_conn->addrlen); // Check for socket read errors if(recvb < 0) {