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>
#include <stdio.h>

struct Connection {
    int sockfd;
    socklen_t addrlen;
    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;
}

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;

}

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