Read from multiple sockets

CMakeLists.txt

@@ -17,8 +17,8 @@ add_library(robomaster
     src/messages/led.c
     src/messages/sdk_mode.c
     src/messages/set_wheel_speed.c
+    src/connection.c
     src/robomaster.c
-    src/robomastersh.c
 )
 
 target_include_directories(robomaster

src/client.h

@@ -8,8 +8,13 @@ struct Client {
 
    int16_t seq;
 
-   struct Connection* sdk_conn;
-   struct Connection* dev_conn;
+   union {
+       struct {
+           struct Connection* sdk_conn;
+           struct Connection* dev_conn;
+       };
+       struct Connection *conns[2];
+   };
 
 };
 

src/connection.c

@@ -0,0 +1,82 @@
+#include "message.h"
+#include "client.h"
+#include "crc.h"
+#include "connection.h"
+
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <sys/select.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <string.h>
+#include <fcntl.h>
+#include <stdio.h>
+
+int max_fd = -1;
+
+struct Connection*
+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(dest_port);
+    conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip);
+
+    // File descriptors are numbers that count up sequentially,
+    // so save the last one as the greatest file descriptor.
+    // This is needed for polling the sockets later.
+    max_fd = conn->sockfd;
+
+    return conn;
+}
+
+void
+req_send(struct Connection* conn, union Request* req, size_t length) {
+    sendto(conn->sockfd, req, length, 0, (struct sockaddr*)&conn->remote_addr, conn->addrlen);
+}
+
+void
+req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, size_t length, union Request* req) {
+
+    req->header.preamble = 0x55;
+    req->header.length_l = length & 0xFF;
+    req->header.length_h = ((length >> 8) & 0x3) | 4;
+    req->header.crc = crc8(req, 3);
+    req->header.seq_id = client->seq++;
+    req->header.sender = client->hostbyte;
+    // TODO: Figure out what this is supposed to be
+    req->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX);
+    req->header.ack_needed = true;
+    req->header.cmdset = cmdset;
+    req->header.cmdid = cmdid;
+
+    struct Footer* footer = (void*)req + length - sizeof(struct Footer);
+    uint16_t crc = crc16(req, length - sizeof(struct Footer));
+    footer->crc = crc;
+
+}
+

src/connection.h

@@ -2,16 +2,14 @@
 
 #include "message.h"
 #include "client.h"
-#include "crc.h"
 
-#include <stdlib.h>
-#include <sys/socket.h>
-#include <sys/select.h>
+#include <stddef.h>
 #include <netinet/in.h>
-#include <arpa/inet.h>
-#include <string.h>
-#include <fcntl.h>
-#include <stdio.h>
+
+// The greated file descriptor is needed for polling the sockets.
+// It needs to be global for the whole process.
+// TODO: Should probably just move the polling function to connection.c
+extern int max_fd;
 
 struct Connection {
     int sockfd;
@@ -19,82 +17,12 @@ struct Connection {
     struct sockaddr_in remote_addr;
 };
 
-inline
-static
 struct Connection*
-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(dest_port);
-    conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip);
-
-    return conn;
-}
+connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip);
 
-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
-req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, size_t length, union Request* req) {
-
-    req->header.preamble = 0x55;
-    req->header.length_l = length & 0xFF;
-    req->header.length_h = ((length >> 8) & 0x3) | 4;
-    req->header.crc = crc8(req, 3);
-    req->header.seq_id = client->seq++;
-    req->header.sender = client->hostbyte;
-    // TODO: Figure out what this is supposed to be
-    req->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX);
-    req->header.ack_needed = true;
-    req->header.cmdset = cmdset;
-    req->header.cmdid = cmdid;
-
-    struct Footer* footer = (void*)req + length - sizeof(struct Footer);
-    uint16_t crc = crc16(req, length - sizeof(struct Footer));
-    footer->crc = crc;
-
-}
+req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, size_t length, union Request* req);
 
-inline
-static
 void
-req_send(struct Connection* conn, union Request* req, size_t length) {
-    sendto(conn->sockfd, req, length, 0, (struct sockaddr*)&conn->remote_addr, conn->addrlen);
-}
+req_send(struct Connection* conn, union Request* req, size_t length);
 

src/robomaster.c

@@ -35,7 +35,8 @@ void poll_message(Client client, union Message* resp) {
     fd_set read_fds;
     FD_ZERO(&read_fds);
     FD_SET(client->sdk_conn->sockfd, &read_fds);
-    int result = select(client->sdk_conn->sockfd + 1, &read_fds, NULL, NULL, NULL);
+    FD_SET(client->dev_conn->sockfd, &read_fds);
+    int result = select(max_fd + 1, &read_fds, NULL, NULL, NULL);
 
     // Check for socket polling errors
     if(result < 0) {
@@ -45,23 +46,32 @@ void poll_message(Client client, union Message* resp) {
 
     // Skip if nothing was received yet
     // TODO: Make a static "empty" message or something
-    if (result == 0) {
+    if (result == 0)
         return;
-    }
 
-    // Read a message from the socket
-    int recvb = recvfrom(client->sdk_conn->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->sdk_conn->remote_addr, &client->sdk_conn->addrlen);
+    // Read a message from the sockets
+    for(int i = 0; i < 2; i++)
+    {
 
-    // Check for socket read errors
-    if(recvb < 0) {
-        perror("reading socket failed");
-	exit(EXIT_FAILURE);
-    }
+        if(!FD_ISSET(client->conns[i]->sockfd, &read_fds))
+            continue;
+
+        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) {
+            perror("reading socket failed");
+            exit(EXIT_FAILURE);
+        }
+
+        // Check for message errors
+        if(message_validate(resp) != MESSAGEERR_NONE) {
+            perror("invalid message");
+            exit(EXIT_FAILURE);
+        }
+
+	return;
 
-    // Check for message errors
-    if(message_validate(resp) != MESSAGEERR_NONE) {
-        perror("invalid message");
-	exit(EXIT_FAILURE);
     }
 
 }

src/robomastersh.c

@@ -1,10 +1,42 @@
 #include "robomaster.h"
 
+#include <stdio.h>
+
 int main(int argc, char* argv[])
 {
     Client client = client_new();
     client_connect(client);
 
+    union Message resp;
+    poll_message(client, &resp);
+    if(resp.header.cmdid != SET_SDK_CONNECTION_CMDID || resp.resp.sdkconn.retcode) {
+        fprintf(stderr, "Could not set SDK connection\n");
+        return 1;
+    }
+
+    set_sdk_mode(client, true);
+    //poll_message(client, &resp);
+    //if(resp.header.cmdid == SET_SDK_MODE_CMDID || resp.resp.sdkmode.retcode) {
+    //    fprintf(stderr, "Could not set SDK mode\n");
+    //    return 1;
+    //}
+
+    set_system_led (
+    		client,
+    		0xFF,
+    		0x00,
+    		0xFF,
+    		LEDCOMP_ALL,
+    		0xFFFF,
+    		LEDEFFECT_ON,
+    		100,
+    		100 );
+    //poll_message(client, &resp);
+    //if(resp.header.cmdid == SET_SYSTEM_LED_CMDID || resp.resp.led.retcode) {
+    //    fprintf(stderr, "Could not set LED color\n");
+    //    return 1;
+    //}
+
     return 0;
 }