diff --git a/include/robomaster.h b/include/robomaster.h index 369b234..96729e0 100644 --- a/include/robomaster.h +++ b/include/robomaster.h @@ -12,4 +12,6 @@ typedef struct Client* Client; typedef struct Message* Message; Client client_new(void* buffer, size_t size); +void client_connect(Client client); +Message poll_message(Client client); diff --git a/src/client.h b/src/client.h index 47008e2..5507151 100644 --- a/src/client.h +++ b/src/client.h @@ -1,5 +1,7 @@ #pragma once +#include "connection.h" + struct Client { void* buffer; @@ -10,6 +12,8 @@ struct Client { int16_t seq; + struct Connection* connection; + }; static inline uint8_t host2byte(uint8_t host, uint8_t index) { diff --git a/src/connection.c b/src/connection.c new file mode 100644 index 0000000..ca75962 --- /dev/null +++ b/src/connection.c @@ -0,0 +1,78 @@ + +#include "robomaster.h" +#include "client.h" +#include "connection.h" +#include "message.h" + +static struct Connection* connection_new() +{ + struct Connection* conn = calloc(sizeof(struct Connection)); + + // Request a UDP socket + conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0); + + // 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 + conn->addrlen = sizeof(conn->remote_addr); + conn->dest_addr.sin_family = AF_INET; + conn->dest_addr.sin_port = htons(30030); + conn->dest_addr.sin_addr.s_addr = inet_addr("192.168.2.1"); + + return conn; +} + +void client_connect(Client client) { + client->connection = connection_new(); + + sendto(client->connection->sockfd, client->buffer, session_size(session), 0, (struct sockaddr*)&client->connection->dest_addr, client->connection->addrlen); +} + +// TODO: Use union to make all messages same size +void send_message(Client client, Message message, int length) { + sendto(client->connection->sockfd, message, length, 0, (struct sockaddr*)&client->connection->dest_addr, client->connection->addrlen); +} + +Message poll_message(Client client) { + + // Poll for messages + static const struct timeval timeout = {-1, 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, &timeout); + + // Check for socket polling errors + if(result < 0) { + perror("message polling failed"); + exit(EXIT_FAILURE); + } + + // Skip if nothing was received yet + // TODO: Make a static "empty" message or something + if (result == 0) { + return NULL; + } + + // Read a message from the socket + // TODO: Use union to make all messages same size + // NOTE: This is never freed, so it's a memory leak + void* buffer = malloc(1024); + int recvb = recvfrom(client->connection->sockfd, buffer, sizeof(client->max_size), 0, (struct sockaddr*)&client->connection->dest_addr, &client->connection->addrlen); + + // Check for socket read errors + if(recvb < 0) { + perror("reading socket failed"); + exit(EXIT_FAILURE); + } + + // Check for message errors + if(message_validate(buffer)) { + perror("invalid message"); + exit(EXIT_FAILURE); + } + + return (Message)buffer; +} diff --git a/src/connection.h b/src/connection.h new file mode 100644 index 0000000..de824ba --- /dev/null +++ b/src/connection.h @@ -0,0 +1,8 @@ +#pragma once + +struct Connection { + int sockfd; + socklen_t addrlen; + sockaddr_in remote_addr; +}; + diff --git a/src/message.h b/src/message.h index e1a4f08..612fb8f 100644 --- a/src/message.h +++ b/src/message.h @@ -95,3 +95,23 @@ message_new(struct Client* session, uint8_t cmdset, uint8_t cmdid, size_t length } +enum MESSAGEERR { + MESSAGEERR_NONE, + MESSAGEERR_HEADERCRC, + MESSAGEERR_FOOTERCRC +} + +static +inline +enum MESSAGEERR +message_validate(const struct Message* message) { + + uint16_t length = (message->header->length_h[2] & 0x3) * 0xFF + message->header->length_l[1]; + + if(message->header.crc != crc8(message, 3)) + return MESSAGEERR_HEADERCRC; + + if(message->header.crc != crc16(message, length - sizeof(struct Footer))) + return MESSAGEERR_FOOTERCRC; + +}