File content as of revision 32:263d39ea6d3e:

#include "Config.h"

double Initial_Bearing;
double Final_Bearing;
double D_IMU_Y_north;

string decodeCommandGET(string cmd) {
    if (cmd == "IMU_Y") {
        return(IMU_Y);
    } else if (cmd == "IMU_P") {
        return(IMU_P);
    } else if (cmd == "IMU_R") {
        return(IMU_R);
    } else if (cmd == "GPS_Quality") {
        return(GPS_Quality);
    } else if (cmd == "GPS_UTC") {
        return(GPS_UTC);
    } else if (cmd == "GPS_Latitude") {
        return(GPS_Latitude);
    } else if (cmd == "GPS_Longitude") {
        return(GPS_Longitude);
    } else if (cmd == "GPS_Altitude") {
        return(GPS_Altitude);
    } else if (cmd == "GPS_Num_Satellite") {
        return(GPS_Num_Satellite);
    } else if (cmd == "GPS_HDOP") {
        return(GPS_HDOP);
    } else if (cmd == "GPS_VDOP") {
        return(GPS_VDOP);
    } else if (cmd == "GPS_PDOP") {
        return(GPS_PDOP);
    } else if (cmd == "GPS_Date") {
        return(GPS_Date);
    } else if (cmd == "GPS_VelocityKnot") {
        return(GPS_VelocityKnot);
    } else if (cmd == "GPS_VelocityKph") {
        return(GPS_VelocityKph);
    } else {
        return("Not valid command, example: @GET=GPS_Quality");
    }
}



double Deg2Rad(double degree) {
    return degree*DEG2RAD_RATIO;
}

double Rad2Degree(double radian) {
    return radian*RAD2DEG_RATIO;
}

double getDistance(int task_id) {
    double cur_Latitude = Deg2Rad(D_GPS_Latitude);
    double cur_Logntitude = D_GPS_Longitude;
    double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
    double dest_Longitude = Longitude_Path[task_id-1];
    if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
        return -1;
    } else {
        return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)))*EARTH_RADIUS;
    }
}


double getAngle(int task_id) {
    double cur_Latitude = Deg2Rad(D_GPS_Latitude);
    double cur_Logntitude = D_GPS_Longitude;
    double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
    double dest_Longitude = Longitude_Path[task_id-1];
    if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
        return -361;
    } else {
        Initial_Bearing = Rad2Degree(atan2(cos(cur_Latitude)*sin(dest_Latitude)-sin(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)),
                          sin(Deg2Rad(dest_Longitude-cur_Logntitude))*cos(dest_Latitude)));
        Final_Bearing = ((int)(Initial_Bearing+180))%360;
        D_IMU_Y_north = (D_IMU_Y<=0)?(D_IMU_Y + 180):(D_IMU_Y - 180);
        double out =  (Initial_Bearing - D_IMU_Y_north< -180)?(Initial_Bearing - D_IMU_Y_north + 360):
                      (Initial_Bearing - D_IMU_Y_north>  180)?(Initial_Bearing - D_IMU_Y_north - 360):(Initial_Bearing - D_IMU_Y_north);
        return out;
    }
}