analyzer_util.cpp

#include "analyzer_util.h"

#define _USE_MATH_DEFINES
#include <math.h>
#include <sys/time.h>


void format_timestamp(char *buf, const uint8_t buflen, const uint64_t T)
{
    struct tm *tmp;
    time_t t = T / 1000000;
    tmp = localtime(&t);
    ::strftime(buf, buflen, "%Y%m%d%H%M%S", tmp);
}

uint64_t now() {
    struct timeval tv;
    gettimeofday(&tv,NULL);
    return tv.tv_sec*(uint64_t)1000000 + tv.tv_usec;
}

double vec_len(double vec[3]) {
    return sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
}

double earthradius() // in metres
{
    return 6371000.0f;
}

double wrap_valid_longitude(const double longitude)
{
    if (longitude < 180.0f) {
        return longitude;
    }

    return 180 - (longitude - 180);
    // return (((longitude + 180.0) % 360.0) -180.0);
}

// http://www.movable-type.co.uk/scripts/latlong.html
void gps_newpos(const double orig_lat, const double orig_lon, const double bearing, const double distance, double &dest_lat, double &dest_lon)
{
    double origin_lat_rad = deg_to_rad(orig_lat);
    double origin_lon_rad = deg_to_rad(orig_lon);
    double bearing_rad = deg_to_rad(bearing);

    double dr = distance/earthradius();

    dest_lat = asin(sin(origin_lat_rad)*cos(dr) +
                    cos(origin_lat_rad)*sin(dr)*cos(bearing_rad));
    dest_lon = origin_lon_rad + atan2(sin(bearing_rad)*sin(dr)*cos(origin_lat_rad),
                                      cos(dr)-sin(origin_lat_rad)*sin(origin_lat_rad));
    dest_lat = rad_to_deg(dest_lat);
    dest_lon = wrap_valid_longitude(rad_to_deg(dest_lon));
}

// origin_lat in degrees
// origin_lon in degrees
// bearing in degrees
// distance in metres
void gps_offset(double orig_lat, double orig_lon, double east, double north, double &dest_lat, double &dest_lon)
{
    double bearing = rad_to_deg(atan2(east, north));
    double distance = sqrt(east*east + north*north);
    gps_newpos(orig_lat, orig_lon, bearing, distance, dest_lat, dest_lon);
}


double altitude_from_pressure_delta(
    double gnd_abs_press,
    double gnd_temp,
    double press_abs,
    double temp UNUSED)
{
    double scaling = press_abs/gnd_abs_press;
    return 153.8462 * (gnd_temp + 273.15) * (1.0 - exp(0.190259 * log(scaling)));
}

bool strieq(const char *x, const char *y)
{
    const char *xp = &x[0];
    const char *yp = &y[0];
    while (true) {
        if (*xp == '\0') {
            if (*yp == '\0') {
                return true;
            }
            return false;
        }
        if (*yp == '\0') {
            return false;
        }
        char xc = tolower(*xp);
        char yc = tolower(*yp);
        if (xc != yc) {
            return false;
        }
        xp++;
        yp++;
    }
}