diff --git a/CMakeLists.txt b/CMakeLists.txt index 463aec2..0f4debf 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/src/client.h b/src/client.h index cab7fad..b15da75 100644 --- a/src/client.h +++ b/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]; + }; }; diff --git a/src/connection.c b/src/connection.c new file mode 100644 index 0000000..35ed64c --- /dev/null +++ b/src/connection.c @@ -0,0 +1,82 @@ +#include "message.h" +#include "client.h" +#include "crc.h" +#include "connection.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +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; + +} + diff --git a/src/connection.h b/src/connection.h index eca31f3..ca16aee 100644 --- a/src/connection.h +++ b/src/connection.h @@ -2,16 +2,14 @@ #include "message.h" #include "client.h" -#include "crc.h" -#include -#include -#include +#include #include -#include -#include -#include -#include + +// 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); diff --git a/src/robomaster.c b/src/robomaster.c index ce8cef9..d144e23 100644 --- a/src/robomaster.c +++ b/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); } } diff --git a/src/robomastersh.c b/src/robomastersh.c index 593fbea..7e5a105 100644 --- a/src/robomastersh.c +++ b/src/robomastersh.c @@ -1,10 +1,42 @@ #include "robomaster.h" +#include + 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; }