Make two connections for SDK and device

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

src/client.h

@@ -8,7 +8,8 @@ struct Client {
 
    int16_t seq;
 
-   struct Connection* connection;
+   struct Connection* sdk_conn;
+   struct Connection* dev_conn;
 
 };
 

src/connection.h

@@ -11,6 +11,7 @@
 #include <arpa/inet.h>
 #include <string.h>
 #include <fcntl.h>
+#include <stdio.h>
 
 struct Connection {
     int sockfd;
@@ -21,7 +22,7 @@ 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));
@@ -29,19 +30,44 @@ connection_new()
     // 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_port = htons(dest_port);
-    conn->remote_addr.sin_addr.s_addr = inet_addr("192.168.2.1");
+    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

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

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

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

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

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);
+    FD_SET(client->sdk_conn->sockfd, &read_fds);
-    int result = select(client->connection->sockfd + 1, &read_fds, NULL, NULL, NULL);
+    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->sdk_conn->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->sdk_conn->remote_addr, &client->sdk_conn->addrlen);
-    int recvb = recvfrom(client->connection->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->connection->remote_addr, &client->connection->addrlen);
 
     // Check for socket read errors
     if(recvb < 0) {