aera.c

Go to the documentation of this file.

// AERA extension for dump1090
//
// Copyright (C) 2013-14 by Andreas Lang <andreas.lang3@student.kit.edu>
// Version 1.1.1 - 04.07.2014
//
#include "dump1090.h"
#include "aera.h"
#include <errno.h>
//
// ===================== AERA extension  ===================
//
uint64_t mstime(void)
{
    struct timeval tv;
    uint64_t mst;

    gettimeofday(&tv, NULL);
    mst = ((uint64_t)tv.tv_sec)*1000;
    mst += round(tv.tv_usec/1000.0);
    return mst;
}

// Calculate GroundDistance with haversine formula
double aeraCalcGroundDistance(double lat1, double lon1, double lat2, double lon2)
{
    double latitudeArc  = (lat2 - lat1) * AERA_DEG_TO_RAD;
    double longitudeArc = (lon2 - lon1) * AERA_DEG_TO_RAD;
    double latitudeH = sin(latitudeArc * 0.5);
    latitudeH *= latitudeH;
    double lontitudeH = sin(longitudeArc * 0.5);
    lontitudeH *= lontitudeH;
    double tmp = cos(lat1*AERA_DEG_TO_RAD) * cos(lat2*AERA_DEG_TO_RAD);
    double arcInRadians = 2.0 * asin(sqrt(latitudeH + tmp*lontitudeH));
    return AERA_EARTH_RADIUS_IN_METERS*arcInRadians;
}

// Calculate CorrectedAltitude with curvature of the earth
double aeraCalcCorrectedAltitude(double alt1, double alt2, double groundDistance)
{
    if (alt1 == 0.0) {
        return 0.0;
    } else {
        return (alt1 - alt2) - sqrt((groundDistance * groundDistance) + (AERA_EARTH_RADIUS_IN_METERS * AERA_EARTH_RADIUS_IN_METERS)) + AERA_EARTH_RADIUS_IN_METERS;
    }
}

/*
// Calculate Zenith with haversine formula
double aeraCalcZenith(double lat1, double lon1, double alt1, double lat2, double lon2, double alt2)
{
    double latitudeArc  = (lat2 - lat1) * AERA_DEG_TO_RAD;
    double longitudeArc = (lon2 - lon1) * AERA_DEG_TO_RAD;
    double latitudeH = sin(latitudeArc * 0.5);
    latitudeH *= latitudeH;
    double lontitudeH = sin(longitudeArc * 0.5);
    lontitudeH *= lontitudeH;
    double tmp = cos(lat1*AERA_DEG_TO_RAD) * cos(lat2*AERA_DEG_TO_RAD);
    double arcInRadians = 2.0 * asin(sqrt(latitudeH + tmp*lontitudeH));
    double distanceInMeters = AERA_EARTH_RADIUS_IN_METERS*arcInRadians;
    return 90.0 - atan((alt1 - alt2) / distanceInMeters) / AERA_DEG_TO_RAD;
}

double aeraCalcDistance(double lat1, double lon1, double alt1, double lat2, double lon2, double alt2)
{
    double latitudeArc  = (lat2 - lat1) * AERA_DEG_TO_RAD;
    double longitudeArc = (lon2 - lon1) * AERA_DEG_TO_RAD;
    double latitudeH = sin(latitudeArc * 0.5);
    latitudeH *= latitudeH;
    double lontitudeH = sin(longitudeArc * 0.5);
    lontitudeH *= lontitudeH;
    double tmp = cos(lat1*AERA_DEG_TO_RAD) * cos(lat2*AERA_DEG_TO_RAD);
    double arcInRadians = 2.0 * asin(sqrt(latitudeH + tmp*lontitudeH));
    return sqrt(AERA_EARTH_RADIUS_IN_METERS*arcInRadians * AERA_EARTH_RADIUS_IN_METERS*arcInRadians + (alt1 - alt2)*(alt1 - alt2));
}
*/
double aeraCalcAzimuth(double lat1, double lon1, double lat2, double lon2)
{
    double lat1Arc = lat1 * AERA_DEG_TO_RAD;
    double lat2Arc = lat2 * AERA_DEG_TO_RAD;
    double longitudeArc = (lon1 - lon2) * AERA_DEG_TO_RAD;
    double z = sin(longitudeArc) * cos(lat2Arc);
    double n = cos(lat1Arc) * sin(lat2Arc) - sin(lat1Arc) * cos(lat2Arc) * cos(longitudeArc);
    return fmod(630. + atan2(z, n) / AERA_DEG_TO_RAD, 360);
}

struct aircraft *interactiveFindAircraft2(uint32_t addr) {
    struct aircraft *a = Modes.aircrafts;

    while(a) {
        if (a->addr == addr) return (a);
        a = a->next;
    }
    return (NULL);
}

void aera(struct modesMessage *mm)
{
    uint32_t airplaneAddr; 
    double airplaneLat;              // Coordinates of Airplane
    double airplaneLon;              // Coordinates of Airplane
    double airplaneAltitude;         // Coordinates of Airplane
    double groundDistance;
    double correctedAltitude;
    double zenith;
    double azimuth;
    double distance;
    int heading = -1;                // Heading of Airplane (-1 if there is no information)
    int velocity = -1;               // Velocity of Airplane (-1 if there is no information)
    int lengthHex;
    char time[30];
    char hex[256];
    char logMessage[256];
    // char socketMessage[40];
    struct timeval timeVal;
    time_t curtime;
    uint32_t gpsSec, uSec;
    uint64_t curtimeWithmSec;
    struct aircraft *aircraft;

    //printf("msgtype :%d\n", mm->msgtype);
    if ((mm->msgtype == 17 || mm->msgtype == 18) && mm->metype >= 9 && mm->metype <= 18 && (mm->bFlags & MODES_ACFLAGS_LATLON_VALID)) { // Airborne position Baro

        int sentToSocket = 0;

        // Initializing time in different formats
        gettimeofday(&timeVal, 0x0);
        curtime = timeVal.tv_sec;
        gpsSec = timeVal.tv_sec - AERA_UTC_TO_GPS;
        uSec = (uint32_t)(timeVal.tv_usec);
        strftime(time, 30, "%Y %m %d %H %M %S", gmtime(&curtime));

        curtimeWithmSec = ((uint64_t)curtime)*1000;
        curtimeWithmSec += round(uSec/1000.0);
        
        // Get ID of airplane
        airplaneAddr = mm->addr;

        // Get heading and velocity for that airplane
        aircraft = interactiveFindAircraft2(airplaneAddr);
        if (aircraft) {                                   // If it's an aircraft with that ID....
            heading = 90 - aircraft->track;               // Change coordinate system
            if (heading < 0) {                            // from N = 0, E = 90, S = 180, W = 270
                heading += 360;                           // to E = 0, N = 90, W = 180, S = 270
            }
            velocity = aircraft->speed;    // speed is in knots
        }
        
        // Read airplane position from modesMessage
        airplaneLat = mm->fLat;
        airplaneLon = mm->fLon;
        airplaneAltitude = mm->altitude * 0.3048; //in meters

        // Calculate zenith, azimuth and distance of airplane relative to AERA
        groundDistance = aeraCalcGroundDistance(airplaneLat, airplaneLon, AERA_STATION_LATITUDE, AERA_STATION_LONGITUDE);
        correctedAltitude = aeraCalcCorrectedAltitude(airplaneAltitude, AERA_STATION_ALTITUDE, groundDistance);
        zenith = 90.0 - atan(correctedAltitude / groundDistance) / AERA_DEG_TO_RAD;
        azimuth = aeraCalcAzimuth(airplaneLat, airplaneLon, AERA_STATION_LATITUDE, AERA_STATION_LONGITUDE);
        distance = sqrt((groundDistance * groundDistance) + (correctedAltitude * correctedAltitude));

        if (Modes.aera_net && zenith <= 80.) { // write data to socket if zenith <= 80. degrees
            // Format output message for socket
            /*
              AIRPLANE = 8 bytes
              Airplane Adress = 4 bytes (unsigned int)
              GPSsecond = 4 bytes (unsigned int)
              microsecond = 4 bytes (unsigned int)
              latitude = 4 bytes (float)
              longitude = 4 bytes (float)
              altutude = 4 bytes (float)
              zenith = 4 bytes (float)
              azimuth = 4 bytes (float)
              distance = 4 bytes (float)
              heading = 4 bytes (int)
              velocity = 4 bytes (int)
              = total of 52 bytes
            */

            float airplaneLatF = (float)(airplaneLat);
            float airplaneLonF = (float)(airplaneLon);
            float airplaneAltitudeF = (float)(airplaneAltitude);
            float zenithF = (float)(zenith);
            float azimuthF = (float)(azimuth);
            float distanceF = (float)(distance);

            unsigned char socketMessage[52];
            strncpy((char*)socketMessage,"AIRPLANE",8);
            *(unsigned int*)&socketMessage[8] = htonl(airplaneAddr);
            *(unsigned int*)&socketMessage[12] = htonl(gpsSec);
            *(unsigned int*)&socketMessage[16] = htonl(uSec);
            unsigned int tmp = *(unsigned int*)&airplaneLatF;
            *(unsigned int*)&socketMessage[20] = htonl(tmp);
            tmp = *(unsigned int*)&airplaneLonF;
            *(unsigned int*)&socketMessage[24] = htonl(tmp);
            tmp = *(unsigned int*)&airplaneAltitudeF;
            *(unsigned int*)&socketMessage[28] = htonl(tmp);
            tmp = *(unsigned int*)&zenithF;
            *(unsigned int*)&socketMessage[32] = htonl(tmp);
            tmp = *(unsigned int*)&azimuthF;
            *(unsigned int*)&socketMessage[36] = htonl(tmp);
            tmp = *(unsigned int*)&distanceF;
            *(unsigned int*)&socketMessage[40] = htonl(tmp);
            *(int*)&socketMessage[44] = htonl(heading);
            *(int*)&socketMessage[48] = htonl(velocity);


            // Write messages to socket
            sentToSocket = aeraWriteToSocket(aeraSocketFD, socketMessage, 52);

            // if (sentToSocket == 0) aeraWriteLogfile("ERROR writing to socket:\n");
        }
        // Format output message for logfile
        lengthHex = 0;
        int j;
        for (j = 0; j < mm->msgbits/8; j++) lengthHex += sprintf(hex + lengthHex, "%02x", mm->msg[j]);
        sprintf(logMessage, "%llu *%s; %s %u %06u %06x %3.12f %3.12f %5.2f %f %f %f %d %d %d\n", curtimeWithmSec, hex, time, gpsSec, uSec, airplaneAddr, airplaneLat, airplaneLon, airplaneAltitude, zenith, azimuth, distance, heading, velocity, sentToSocket);

        aeraWriteLogfile(logMessage);
    }

    return;
}

void aeraWriteLogfile(char *logMessage)
{
    FILE *file = NULL;
    time_t curtime = time(NULL);
    char logFile[128];

    strftime(logFile, 128, "%Y%m%d.log", gmtime(&curtime));

    file = fopen(logFile, "a");
    fputs(logMessage, file);
    fclose (file);
}

int aeraConnectToSocket(int *aeraSocketFD, char host[], int port)
{
    signal(SIGPIPE, SIG_IGN);
    int ret = 1;

    struct sockaddr_in serv_addr;
    struct hostent *server;
    *aeraSocketFD = socket(AF_INET, SOCK_STREAM, 0);

    if (*aeraSocketFD < 0) {
//      printf("ERROR opening socket\n");
        ret = 0;
    }

    server = gethostbyname(host);

    if (server == 0x0) {
//      printf("ERROR, no such host\n");
        ret = 0;
    }

    bzero((char *) &serv_addr, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    bcopy((char*)server->h_addr,(char*)&serv_addr.sin_addr.s_addr,server->h_length);
    serv_addr.sin_port = htons(port);

    if ( connect(*aeraSocketFD,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0) {
//      printf("ERROR connecting to socket\n");
        ret = 0;
    }

    return(ret);
}

int aeraReconnectToSocket(int *aeraSocketFD)
{
    time_t current_time;
    current_time = time(NULL);

    close(*aeraSocketFD);
    if (current_time - aeraSocketReconnectTime > 10) { // Try to reconnect after 10s
        int ret = aeraConnectToSocket(aeraSocketFD, Modes.aera_net, Modes.aera);
        aeraSocketReconnectTime = time(NULL);
        return(ret);
    } else {
        return(0);
    }
}

int aeraWriteToSocket(int *aeraSocketFD, unsigned char *socketMessage, int length)
{
    int error;
    int ret = 1;

    error = write(*aeraSocketFD, socketMessage, length);
    if (error != length) { // retry
//      printf("ERROR RECONNECT\n");
        ret = aeraReconnectToSocket(aeraSocketFD);
        if (ret == 1) {
            error = write(*aeraSocketFD, socketMessage, length);
            if (error != length) {
                ret = 0;
            }
        }
    }
    return(ret);
}