robomaster-sdk/src/connection.h

#pragma once

#include "message.h"
#include "client.h"
#include "crc.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>

struct Connection {
    int sockfd;
    socklen_t addrlen;
    struct sockaddr_in remote_addr;
};

inline
static
struct Connection*
connection_new()
{
    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);
 
    // 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->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");

    return conn;
}

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;

}

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