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;

#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 onMessage(struct mosquitto *mqtt, void *obj, const struct mosquitto_message *message)
{
    int *id, *frequency;
    char rref[XPLANE_RREF_MAX_LENGTH];

    printf("Received message %.*s on topic %s\n", message->payloadlen, (char *)message->payload, message->topic);

    if (strcmp(message->topic, "/xplane/meta/rref") == 0)
    {
        printf("found rref request\n");
        if (message->payloadlen >= MQTT_RREF_REQUEST_MIN_LENGTH)
        {
            id = 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 (id == NULL || frequency == NULL) {
                return;
            }

            // prepare struct to send, make sure padding is with NULL bytes
            struct dref_struct_in drefToRead = {
                .dref_freq = *frequency,
                .dref_sender_index = *id
            };
            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));
        }
    }
    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_USER, 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));

                        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));
                    }
                }
                else
                {
                    exit(EXIT_FAILURE);
                }
            }
            else
            {
                // unknown to me
            }
        }
        else
        {
            exit(EXIT_FAILURE);
        }

        mosquitto_loop(mqtt, 100, 1);
    }

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