nav.cpp

00001     /*
00002     File:       nav.cpp
00003     Version:    0.1.0
00004     Date:       Feb. 28, 2017
00005     License:    GPL v2
00006     
00007     Navigation class
00008     
00009     ****************************************************************************
00010     This program is free software; you can redistribute it and/or modify
00011     it under the terms of the GNU General Public License as published by
00012     the Free Software Foundation; either version 2 of the License, or
00013     (at your option) any later version.
00014 
00015     This program is distributed in the hope that it will be useful,
00016     but WITHOUT ANY WARRANTY; without even the implied warranty of
00017     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018     GNU General Public License for more details.
00019 
00020     You should have received a copy of the GNU General Public License
00021     along with this program; if not, write to the Free Software
00022     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00023     ****************************************************************************
00024  */
00025        #include <math.h>
00026        #include "nav.h" 
00027        // Calculate the heading
00028         double NAV::CalculateDistance (double from_lat, double from_lon, double to_lat, double to_lon)
00029         {
00030             double R = 6371e3;
00031             double lat1 = DegreeToRadian(from_lat);
00032             double lat2 = DegreeToRadian(to_lat);
00033             double deltaLat = DegreeToRadian(to_lat - from_lat);
00034             double deltaLong = DegreeToRadian(to_lon - from_lon);
00035 
00036             double a = sin(deltaLat / 2) * sin(deltaLat / 2) + cos(lat1) * cos(lat2) * sin(deltaLong / 2) * sin(deltaLong / 2);
00037             double c = 2 * atan2(sqrt(a), sqrt(1 - a));
00038             double d = R * c;
00039 
00040             return d;
00041         }
00042         
00043         // Calculate bearing
00044         double NAV::CalculateBearing(double from_lat, double from_lon, double to_lat, double to_lon)
00045         {
00046             double lat1 = DegreeToRadian(from_lat);
00047             double lat2 = DegreeToRadian(to_lat);
00048             double long1 = DegreeToRadian(from_lon);
00049             double long2 = DegreeToRadian(to_lon);
00050 
00051             double y = sin(long2 - long1) * cos(lat2);
00052             double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(long2 - long1);
00053 
00054             double bearing = RadianToDegree(atan2(y, x));
00055 
00056             // Correct "wrap around" if necessary
00057             if (bearing < 0) bearing = 360.0 + bearing;
00058             if (bearing > 360.0) bearing = bearing - 360.0;
00059 
00060             return bearing;
00061         }
00062         
00063         /*
00064         This function takes as input the next gps coordinate that the boat is suppossed to achieve and calculates the compass
00065         heading to the waypoint
00066         Jeff Witten - 05/26/14 from Github VT-SailBOT/Navigation sailbot.c
00067         */
00068         int NAV::direction_to_next_point (double from_lat, double from_lon, double to_lat, double to_lon)
00069         {
00070             double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat);
00071             double dist_lon = PI/180*6367449*cos(from_lat);
00072             double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat;
00073             double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon;
00074             double difference = (lat_meter/lon_meter);
00075             double  degree = atan(difference);  
00076             degree = fabs(degree*180/PI);
00077             int angle = 0;
00078     
00079             if(to_lat >= from_lat && to_lon >= from_lon)           //Quadrant I
00080                 angle = 90 - degree;
00081             else if(to_lat <= from_lat && to_lon >= from_lon)          //Quadrant II
00082                 angle = 90 + degree;
00083             else if(to_lat <= from_lat && to_lon <= from_lon)          //Quadrant III
00084                 angle = 270 - degree;
00085             else if(to_lat >= from_lat && to_lon <= from_lon)          //Quadrant IV
00086                 angle = 270 + degree;
00087 
00088         return angle;
00089         }
00090         
00091         /*
00092         This function takes as input the gps coordinates of the boat and the gps coordinates of a waypoint that the boat desires to approach
00093         The function then sets a flag based on whether or not the boat is within a predetermined perimeter of the waypoint
00094         Approach:
00095         1.) Latitude and Longitude of San Francisco = 37.7844 N, 122.4167 W
00096         2.) At 40 degrees North: 1 degree latitude = 111.03 km, 1 degree longitude = 85.39 km
00097         3.) 111.03 = 85.39 * (1.30027) - used to correct approximately rectangular lat/lon grid to approximately square
00098         4.) Through unit analysis, 1 meter = 0.0000111509 degrees longitude 
00099         Jeff Witten - 03/27/14 from Github VT-SailBOT/Navigation sailbot.c
00100         */
00101         int NAV::point_proximity(double from_lat, double from_lon, double to_lat, double to_lon)
00102         {
00103         int number = 0;
00104         double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat);
00105         double dist_lon = PI/180*6367449*cos(from_lat);
00106         double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat;
00107         double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon;
00108         distance = sqrt(pow(lat_meter, 2) + pow(lon_meter,2));
00109 
00110         if (distance <= point_proximity_radius){
00111             number = 1;
00112         } else {
00113             number = 0;
00114         }    
00115             return number;
00116         }
00117         
00118         /**
00119  * \brief Calculate distance between two points
00120  * This function uses an algorithm for an oblate spheroid earth model.
00121  * The algorithm is described here: 
00122  * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
00123  * \return Distance in meters
00124  */
00125 double NAV::distance_ellipsoid(double from_lat,  double from_lon, double from_azimuth, double to_lat, double to_lon, double to_azimuth)
00126 {
00127     /* All variables */
00128     double f, a, b, sqr_a, sqr_b;
00129     double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
00130     double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
00131     int remaining_steps; 
00132     double sqr_u, A, B, delta_sigma;
00133  
00134     if ((from_lat == to_lat) && (from_lon == to_lon))
00135     { /* Identical points */
00136         if ( from_azimuth != 0 )
00137             from_azimuth = 0;
00138         if ( to_azimuth != 0 )
00139             to_azimuth = 0;
00140         return 0;    
00141     } /* Identical points */
00142 
00143     /* Earth geometry */
00144     f = NMEA_EARTH_FLATTENING;
00145     a = NMEA_EARTH_SEMIMAJORAXIS_M;
00146     b = (1 - f) * a;
00147     sqr_a = a * a;
00148     sqr_b = b * b;
00149 
00150     /* Calculation */
00151     L = to_lon - from_lon;
00152     phi1 = from_lat;
00153     phi2 = to_lat;
00154     U1 = atan((1 - f) * tan(phi1));
00155     U2 = atan((1 - f) * tan(phi2));
00156     sin_U1 = sin(U1);
00157     sin_U2 = sin(U2);
00158     cos_U1 = cos(U1);
00159     cos_U2 = cos(U2);
00160 
00161     /* Initialize iteration */
00162     sigma = 0;
00163     sin_sigma = sin(sigma);
00164     cos_sigma = cos(sigma);
00165     cos_2_sigmam = 0;
00166     sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
00167     sqr_cos_alpha = 0;
00168     lambda = L;
00169     sin_lambda = sin(lambda);                            
00170     cos_lambda = cos(lambda);                       
00171     delta_lambda = lambda;
00172     remaining_steps = 20; 
00173 
00174     while ((delta_lambda > 1e-12) && (remaining_steps > 0)) 
00175     { /* Iterate */
00176         /* Variables */
00177         double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; 
00178 
00179         /* Calculation */
00180         tmp1 = cos_U2 * sin_lambda;
00181         tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;  
00182         sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);                
00183         cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;   
00184         tan_sigma = sin_sigma / cos_sigma;                  
00185         sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;  
00186         cos_alpha = cos(asin(sin_alpha));                 
00187         sqr_cos_alpha = cos_alpha * cos_alpha;                     
00188         cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
00189         sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; 
00190         C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
00191         lambda_prev = lambda; 
00192         sigma = asin(sin_sigma); 
00193         lambda = L + 
00194             (1 - C) * f * sin_alpha
00195             * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));                                                
00196         delta_lambda = lambda_prev - lambda; 
00197         if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; 
00198         sin_lambda = sin(lambda);
00199         cos_lambda = cos(lambda);
00200         remaining_steps--; 
00201     }  /* Iterate */
00202 
00203     /* More calculation  */
00204     sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; 
00205     A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
00206     B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
00207     delta_sigma = B * sin_sigma * ( 
00208         cos_2_sigmam + B / 4 * ( 
00209         cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
00210         B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
00211         ));
00212 
00213     /* Calculate result */
00214     if ( from_azimuth != 0 )
00215     {
00216         double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
00217         from_azimuth = atan(tan_alpha_1);
00218     }
00219     if ( to_azimuth != 0 )
00220     {
00221         double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
00222         to_azimuth = atan(tan_alpha_2);
00223     }
00224 
00225     return b * A * (sigma - delta_sigma);
00226 }
00227         
00228         
00229         // Convert Degrees to Radians
00230         double NAV::DegreeToRadian(double angle)
00231         {
00232             return PI * angle / 180.0;
00233         }
00234         
00235        // Convert Radians to Degrees
00236         double NAV::RadianToDegree(double angle)
00237         {
00238             return angle * (180.0 / PI);
00239         }