MQTT-Gateway/readref.c

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <inttypes.h>
#include <math.h>

#include <mosquitto.h>
#include "network.h"
#include "mqtt.h"

#include "config.h"

#define MQTT_RREF_REQUEST_LENGTH_ID 3
#define MQTT_RREF_REQUEST_LENGTH_FREQ 3
#define MQTT_RREF_REQUEST_LENGTH_SEPARATOR 1
#define MQTT_RREF_REQUEST_SEPARATOR ' '

const int MQTT_RREF_REQUEST_START_ID = 0;
const int MQTT_RREF_REQUEST_START_FREQ = MQTT_RREF_REQUEST_START_ID + MQTT_RREF_REQUEST_LENGTH_ID + MQTT_RREF_REQUEST_LENGTH_SEPARATOR;
const int MQTT_RREF_REQUEST_START_RREF = MQTT_RREF_REQUEST_START_FREQ + MQTT_RREF_REQUEST_LENGTH_FREQ + MQTT_RREF_REQUEST_LENGTH_SEPARATOR;

const int MQTT_RREF_REQUEST_MIN_LENGTH = MQTT_RREF_REQUEST_LENGTH_ID + MQTT_RREF_REQUEST_LENGTH_FREQ + (2 * MQTT_RREF_REQUEST_LENGTH_SEPARATOR) + 1; // at least one char for rref

int xPlaneSocket;
struct mosquitto *mqtt;

struct valueCacheElement
{
    float value;
    bool force;
};

struct valueCacheElement *valueCache = NULL;
int valueCacheCount = 0;

#define XPLANE_RREF_MAX_LENGTH 400
struct dref_struct_in
{
    int32_t dref_freq;
    int32_t dref_sender_index; // the index the customer is using to define this dataref
    char dref_string[XPLANE_RREF_MAX_LENGTH];
};

struct dref_struct_out
{
    int32_t dref_sender_index;
    float dref_flt_value;
};

int *strtoint(char *in, size_t start, size_t n)
{
    int *ret = malloc(sizeof(int));
    *ret = 0;
    int offset = start + n - 1;

    if (offset < 0)
    {
        return NULL;
    }

    for (int i = 0; i < n; i++)
    {
        int position = offset - i;
        if (in[position] >= '0' && in[position] <= '9')
        {
            *ret += (in[position] - '0') * pow(10, i);
        }
        else
        {
            return NULL;
        }
    }

    return ret;
}

void requestRrefFromXPlane(const struct mosquitto_message *message)
{
    int *index, *frequency;

    if (message->payloadlen >= MQTT_RREF_REQUEST_MIN_LENGTH)
    {
        index = strtoint(message->payload, MQTT_RREF_REQUEST_START_ID, MQTT_RREF_REQUEST_LENGTH_ID);
        frequency = strtoint(message->payload, MQTT_RREF_REQUEST_START_FREQ, MQTT_RREF_REQUEST_LENGTH_FREQ);

        if (index == NULL || frequency == NULL)
        {
            return;
        }

        if (*index < valueCacheCount)
        {
            valueCache[*index].force = true;
        }

        // prepare struct to send, make sure padding is with NULL bytes
        struct dref_struct_in drefToRead = {
            .dref_freq = *frequency,
            .dref_sender_index = *index};
        memset(drefToRead.dref_string, 0, sizeof(drefToRead.dref_string));
        strncpy(drefToRead.dref_string, &((char *)message->payload)[MQTT_RREF_REQUEST_START_RREF], message->payloadlen - MQTT_RREF_REQUEST_START_RREF);

        sendToXPlane(xPlaneSocket, DEST_SERVER, DEST_PORT, "RREF", (char *)&drefToRead, sizeof(drefToRead));
    }
}

void sendCommandToXPlane(const struct mosquitto_message *message)
{
    char *command;

    if (message->payloadlen > 0)
    {
        sendToXPlane(xPlaneSocket, DEST_SERVER, DEST_PORT, "CMND", (char *)message->payload, message->payloadlen);
    }
}

void onMessage(struct mosquitto *mqtt, void *obj, const struct mosquitto_message *message)
{
    printf("Received message %.*s on topic %s\n", message->payloadlen, (char *)message->payload, message->topic);

    if (strcmp(message->topic, "/xplane/meta/rref") == 0)
    {
        requestRrefFromXPlane(message);
    }
    else if (strcmp(message->topic, "/xplane/meta/cmnd") == 0)
    {
        sendCommandToXPlane(message);
    }
    else
    {
        fprintf(stderr, "Unkown topic %s\n", message->topic);
    }
}

int main()
{
    // network
    xPlaneSocket = createSocket(SRC_PORT);
    if (xPlaneSocket < 0)
    {
        exit(EXIT_FAILURE);
    }

    // mqtt
    mqtt = connectMqtt(MQTT_USER1, MQTT_PASS, onMessage);
    if (mqtt == NULL)
    {
        exit(EXIT_FAILURE);
    }

    // subscribe meta channel
    recvMqtt(mqtt, "/xplane/meta/#");

    // read from network
    char msg[DGRAM_MSG_LENGTH + 1];
    char payload[RECV_BUFFER];

    char mqttTopic[100], mqttPayload[512];
    while (1)
    {
        int payloadBytes = recvFromXPlane(xPlaneSocket, msg, payload, RECV_BUFFER);
        if (payloadBytes >= 0)
        {
            if (strcmp(msg, "RREF") == 0)
            {
                printf("Received message type %s\n", msg);
                printf("Received message data (%d) ", payloadBytes);
                for (int i = 0; i < payloadBytes; i++)
                {
                    printf("%02x", payload[i]);
                }
                printf("\n");

                // continue if payloadBytes is valid (1 byte + 8 bytes each dref)
                if ((payloadBytes - 1) % sizeof(struct dref_struct_out) == 0)
                {
                    // loop throught all provided drefs
                    for (int i = 1; i < payloadBytes; i += sizeof(struct dref_struct_out))
                    {
                        memset(mqttTopic, 0, sizeof(mqttTopic));
                        memset(mqttPayload, 0, sizeof(mqttPayload));

                        struct dref_struct_out drefRead;
                        memcpy(&drefRead, &payload[i], sizeof(struct dref_struct_out));

                        // add new element to array
                        if (valueCache == NULL)
                        {
                            valueCacheCount = drefRead.dref_sender_index + 10;
                            valueCache = calloc(valueCacheCount, sizeof(struct valueCacheElement));
                        }
                        else if (drefRead.dref_sender_index >= valueCacheCount)
                        {
                            valueCacheCount = drefRead.dref_sender_index + 10;
                            valueCache = reallocarray(valueCache, valueCacheCount, sizeof(struct valueCacheElement));
                        }
                        else
                        {
                            // size of array is sufficient
                        }

                        if (valueCache[drefRead.dref_sender_index].force || valueCache[drefRead.dref_sender_index].value != drefRead.dref_flt_value)
                        {
                            valueCache[drefRead.dref_sender_index].value = drefRead.dref_flt_value;
                            printf("%d: %f\n", drefRead.dref_sender_index, drefRead.dref_flt_value);

                            sprintf(mqttTopic, "/xplane/rref/%d", drefRead.dref_sender_index);
                            sprintf(mqttPayload, "%f", drefRead.dref_flt_value);

                            sendMqtt(mqtt, mqttTopic, mqttPayload, strlen(mqttPayload));
                            valueCache[drefRead.dref_sender_index].force = false;
                        }
                        else
                        {
                            // neither changed nor forced
                        }
                    }
                }
                else
                {
                    exit(EXIT_FAILURE);
                }
            }
            else
            {
                // unknown to me
            }
        }
        else
        {
            exit(EXIT_FAILURE);
        }

        mosquitto_loop(mqtt, 0, 1);
    }

    // bye bye (and no, I don't care about error anymore; I'll exit anyway?!)
    disconnectMqtt(mqtt);
    closeSocket(xPlaneSocket);
    exit(EXIT_SUCCESS);
}