A GPS serial interrupt service routine that has an on the fly nmea parser. Works with a STM32F411RE and a Adafruit GPS logger.
Dependents: Bicycl_Computer_NUCLEO-F411RE Bicycl_Computer_NUCLEO-L476RG
Fork of GPS by
main.cpp
#include "mbed.h" #include "GPSISR.h" #define PIN_RX_GPS PA_12 //GPS Shield RX pin #define PIN_TX_GPS PA_11 //GPS Shield TX pin Serial pc(USBTX, USBRX); // Set up serial interrupe service handler for gps characters. GPS MyGPS(PIN_TX_GPS,PIN_RX_GPS, 9600); int main() { while (1) { if (MyGPS.dataready()) { MyGPS.read(); pc.printf("NMEA has valid data"); pc.printf("Sats : %d \n", MyGPS.buffer.satellites); pc.printf("%d-%d-%d\n", MyGPS.buffer.month, MyGPS.buffer.day, MyGPS.buffer.year); pc.printf("%d:%d:%d\n", MyGPS.buffer.hours, MyGPS.buffer.minutes, MyGPS.buffer.seconds); } else { pc.printf("NMEA has no valid data"); } } }
nav.cpp@6:f0c13bb7d266, 2017-03-01 (annotated)
- Committer:
- trevieze
- Date:
- Wed Mar 01 15:36:05 2017 +0000
- Revision:
- 6:f0c13bb7d266
- Parent:
- 5:c5f700c1e1af
Math error in GPS speed calculation from Knots to Kilometer. Should be ; 1Kn = 1.85Km
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
trevieze | 5:c5f700c1e1af | 1 | /* |
trevieze | 5:c5f700c1e1af | 2 | File: nav.cpp |
trevieze | 5:c5f700c1e1af | 3 | Version: 0.1.0 |
trevieze | 5:c5f700c1e1af | 4 | Date: Feb. 28, 2017 |
trevieze | 5:c5f700c1e1af | 5 | License: GPL v2 |
trevieze | 5:c5f700c1e1af | 6 | |
trevieze | 5:c5f700c1e1af | 7 | Navigation class |
trevieze | 5:c5f700c1e1af | 8 | |
trevieze | 5:c5f700c1e1af | 9 | **************************************************************************** |
trevieze | 5:c5f700c1e1af | 10 | This program is free software; you can redistribute it and/or modify |
trevieze | 5:c5f700c1e1af | 11 | it under the terms of the GNU General Public License as published by |
trevieze | 5:c5f700c1e1af | 12 | the Free Software Foundation; either version 2 of the License, or |
trevieze | 5:c5f700c1e1af | 13 | (at your option) any later version. |
trevieze | 5:c5f700c1e1af | 14 | |
trevieze | 5:c5f700c1e1af | 15 | This program is distributed in the hope that it will be useful, |
trevieze | 5:c5f700c1e1af | 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
trevieze | 5:c5f700c1e1af | 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
trevieze | 5:c5f700c1e1af | 18 | GNU General Public License for more details. |
trevieze | 5:c5f700c1e1af | 19 | |
trevieze | 5:c5f700c1e1af | 20 | You should have received a copy of the GNU General Public License |
trevieze | 5:c5f700c1e1af | 21 | along with this program; if not, write to the Free Software |
trevieze | 5:c5f700c1e1af | 22 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
trevieze | 5:c5f700c1e1af | 23 | **************************************************************************** |
trevieze | 5:c5f700c1e1af | 24 | */ |
trevieze | 5:c5f700c1e1af | 25 | #include <math.h> |
trevieze | 5:c5f700c1e1af | 26 | #include "nav.h" |
trevieze | 5:c5f700c1e1af | 27 | // Calculate the heading |
trevieze | 5:c5f700c1e1af | 28 | double NAV::CalculateDistance (double from_lat, double from_lon, double to_lat, double to_lon) |
trevieze | 5:c5f700c1e1af | 29 | { |
trevieze | 5:c5f700c1e1af | 30 | double R = 6371e3; |
trevieze | 5:c5f700c1e1af | 31 | double lat1 = DegreeToRadian(from_lat); |
trevieze | 5:c5f700c1e1af | 32 | double lat2 = DegreeToRadian(to_lat); |
trevieze | 5:c5f700c1e1af | 33 | double deltaLat = DegreeToRadian(to_lat - from_lat); |
trevieze | 5:c5f700c1e1af | 34 | double deltaLong = DegreeToRadian(to_lon - from_lon); |
trevieze | 5:c5f700c1e1af | 35 | |
trevieze | 5:c5f700c1e1af | 36 | double a = sin(deltaLat / 2) * sin(deltaLat / 2) + cos(lat1) * cos(lat2) * sin(deltaLong / 2) * sin(deltaLong / 2); |
trevieze | 5:c5f700c1e1af | 37 | double c = 2 * atan2(sqrt(a), sqrt(1 - a)); |
trevieze | 5:c5f700c1e1af | 38 | double d = R * c; |
trevieze | 5:c5f700c1e1af | 39 | |
trevieze | 5:c5f700c1e1af | 40 | return d; |
trevieze | 5:c5f700c1e1af | 41 | } |
trevieze | 5:c5f700c1e1af | 42 | |
trevieze | 5:c5f700c1e1af | 43 | // Calculate bearing |
trevieze | 5:c5f700c1e1af | 44 | double NAV::CalculateBearing(double from_lat, double from_lon, double to_lat, double to_lon) |
trevieze | 5:c5f700c1e1af | 45 | { |
trevieze | 5:c5f700c1e1af | 46 | double lat1 = DegreeToRadian(from_lat); |
trevieze | 5:c5f700c1e1af | 47 | double lat2 = DegreeToRadian(to_lat); |
trevieze | 5:c5f700c1e1af | 48 | double long1 = DegreeToRadian(from_lon); |
trevieze | 5:c5f700c1e1af | 49 | double long2 = DegreeToRadian(to_lon); |
trevieze | 5:c5f700c1e1af | 50 | |
trevieze | 5:c5f700c1e1af | 51 | double y = sin(long2 - long1) * cos(lat2); |
trevieze | 5:c5f700c1e1af | 52 | double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(long2 - long1); |
trevieze | 5:c5f700c1e1af | 53 | |
trevieze | 5:c5f700c1e1af | 54 | double bearing = RadianToDegree(atan2(y, x)); |
trevieze | 5:c5f700c1e1af | 55 | |
trevieze | 5:c5f700c1e1af | 56 | // Correct "wrap around" if necessary |
trevieze | 5:c5f700c1e1af | 57 | if (bearing < 0) bearing = 360.0 + bearing; |
trevieze | 5:c5f700c1e1af | 58 | if (bearing > 360.0) bearing = bearing - 360.0; |
trevieze | 5:c5f700c1e1af | 59 | |
trevieze | 5:c5f700c1e1af | 60 | return bearing; |
trevieze | 5:c5f700c1e1af | 61 | } |
trevieze | 5:c5f700c1e1af | 62 | |
trevieze | 5:c5f700c1e1af | 63 | /* |
trevieze | 5:c5f700c1e1af | 64 | This function takes as input the next gps coordinate that the boat is suppossed to achieve and calculates the compass |
trevieze | 5:c5f700c1e1af | 65 | heading to the waypoint |
trevieze | 5:c5f700c1e1af | 66 | Jeff Witten - 05/26/14 from Github VT-SailBOT/Navigation sailbot.c |
trevieze | 5:c5f700c1e1af | 67 | */ |
trevieze | 5:c5f700c1e1af | 68 | int NAV::direction_to_next_point (double from_lat, double from_lon, double to_lat, double to_lon) |
trevieze | 5:c5f700c1e1af | 69 | { |
trevieze | 5:c5f700c1e1af | 70 | double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat); |
trevieze | 5:c5f700c1e1af | 71 | double dist_lon = PI/180*6367449*cos(from_lat); |
trevieze | 5:c5f700c1e1af | 72 | double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat; |
trevieze | 5:c5f700c1e1af | 73 | double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon; |
trevieze | 5:c5f700c1e1af | 74 | double difference = (lat_meter/lon_meter); |
trevieze | 5:c5f700c1e1af | 75 | double degree = atan(difference); |
trevieze | 5:c5f700c1e1af | 76 | degree = fabs(degree*180/PI); |
trevieze | 5:c5f700c1e1af | 77 | int angle = 0; |
trevieze | 5:c5f700c1e1af | 78 | |
trevieze | 5:c5f700c1e1af | 79 | if(to_lat >= from_lat && to_lon >= from_lon) //Quadrant I |
trevieze | 5:c5f700c1e1af | 80 | angle = 90 - degree; |
trevieze | 5:c5f700c1e1af | 81 | else if(to_lat <= from_lat && to_lon >= from_lon) //Quadrant II |
trevieze | 5:c5f700c1e1af | 82 | angle = 90 + degree; |
trevieze | 5:c5f700c1e1af | 83 | else if(to_lat <= from_lat && to_lon <= from_lon) //Quadrant III |
trevieze | 5:c5f700c1e1af | 84 | angle = 270 - degree; |
trevieze | 5:c5f700c1e1af | 85 | else if(to_lat >= from_lat && to_lon <= from_lon) //Quadrant IV |
trevieze | 5:c5f700c1e1af | 86 | angle = 270 + degree; |
trevieze | 5:c5f700c1e1af | 87 | |
trevieze | 5:c5f700c1e1af | 88 | return angle; |
trevieze | 5:c5f700c1e1af | 89 | } |
trevieze | 5:c5f700c1e1af | 90 | |
trevieze | 5:c5f700c1e1af | 91 | /* |
trevieze | 5:c5f700c1e1af | 92 | This function takes as input the gps coordinates of the boat and the gps coordinates of a waypoint that the boat desires to approach |
trevieze | 5:c5f700c1e1af | 93 | The function then sets a flag based on whether or not the boat is within a predetermined perimeter of the waypoint |
trevieze | 5:c5f700c1e1af | 94 | Approach: |
trevieze | 5:c5f700c1e1af | 95 | 1.) Latitude and Longitude of San Francisco = 37.7844 N, 122.4167 W |
trevieze | 5:c5f700c1e1af | 96 | 2.) At 40 degrees North: 1 degree latitude = 111.03 km, 1 degree longitude = 85.39 km |
trevieze | 5:c5f700c1e1af | 97 | 3.) 111.03 = 85.39 * (1.30027) - used to correct approximately rectangular lat/lon grid to approximately square |
trevieze | 5:c5f700c1e1af | 98 | 4.) Through unit analysis, 1 meter = 0.0000111509 degrees longitude |
trevieze | 5:c5f700c1e1af | 99 | Jeff Witten - 03/27/14 from Github VT-SailBOT/Navigation sailbot.c |
trevieze | 5:c5f700c1e1af | 100 | */ |
trevieze | 5:c5f700c1e1af | 101 | int NAV::point_proximity(double from_lat, double from_lon, double to_lat, double to_lon) |
trevieze | 5:c5f700c1e1af | 102 | { |
trevieze | 5:c5f700c1e1af | 103 | int number = 0; |
trevieze | 5:c5f700c1e1af | 104 | double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat); |
trevieze | 5:c5f700c1e1af | 105 | double dist_lon = PI/180*6367449*cos(from_lat); |
trevieze | 5:c5f700c1e1af | 106 | double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat; |
trevieze | 5:c5f700c1e1af | 107 | double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon; |
trevieze | 5:c5f700c1e1af | 108 | distance = sqrt(pow(lat_meter, 2) + pow(lon_meter,2)); |
trevieze | 5:c5f700c1e1af | 109 | |
trevieze | 5:c5f700c1e1af | 110 | if (distance <= point_proximity_radius){ |
trevieze | 5:c5f700c1e1af | 111 | number = 1; |
trevieze | 5:c5f700c1e1af | 112 | } else { |
trevieze | 5:c5f700c1e1af | 113 | number = 0; |
trevieze | 5:c5f700c1e1af | 114 | } |
trevieze | 5:c5f700c1e1af | 115 | return number; |
trevieze | 5:c5f700c1e1af | 116 | } |
trevieze | 5:c5f700c1e1af | 117 | |
trevieze | 5:c5f700c1e1af | 118 | /** |
trevieze | 5:c5f700c1e1af | 119 | * \brief Calculate distance between two points |
trevieze | 5:c5f700c1e1af | 120 | * This function uses an algorithm for an oblate spheroid earth model. |
trevieze | 5:c5f700c1e1af | 121 | * The algorithm is described here: |
trevieze | 5:c5f700c1e1af | 122 | * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf |
trevieze | 5:c5f700c1e1af | 123 | * \return Distance in meters |
trevieze | 5:c5f700c1e1af | 124 | */ |
trevieze | 5:c5f700c1e1af | 125 | double NAV::distance_ellipsoid(double from_lat, double from_lon, double from_azimuth, double to_lat, double to_lon, double to_azimuth) |
trevieze | 5:c5f700c1e1af | 126 | { |
trevieze | 5:c5f700c1e1af | 127 | /* All variables */ |
trevieze | 5:c5f700c1e1af | 128 | double f, a, b, sqr_a, sqr_b; |
trevieze | 5:c5f700c1e1af | 129 | double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; |
trevieze | 5:c5f700c1e1af | 130 | double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; |
trevieze | 5:c5f700c1e1af | 131 | int remaining_steps; |
trevieze | 5:c5f700c1e1af | 132 | double sqr_u, A, B, delta_sigma; |
trevieze | 5:c5f700c1e1af | 133 | |
trevieze | 5:c5f700c1e1af | 134 | if ((from_lat == to_lat) && (from_lon == to_lon)) |
trevieze | 5:c5f700c1e1af | 135 | { /* Identical points */ |
trevieze | 5:c5f700c1e1af | 136 | if ( from_azimuth != 0 ) |
trevieze | 5:c5f700c1e1af | 137 | from_azimuth = 0; |
trevieze | 5:c5f700c1e1af | 138 | if ( to_azimuth != 0 ) |
trevieze | 5:c5f700c1e1af | 139 | to_azimuth = 0; |
trevieze | 5:c5f700c1e1af | 140 | return 0; |
trevieze | 5:c5f700c1e1af | 141 | } /* Identical points */ |
trevieze | 5:c5f700c1e1af | 142 | |
trevieze | 5:c5f700c1e1af | 143 | /* Earth geometry */ |
trevieze | 5:c5f700c1e1af | 144 | f = NMEA_EARTH_FLATTENING; |
trevieze | 5:c5f700c1e1af | 145 | a = NMEA_EARTH_SEMIMAJORAXIS_M; |
trevieze | 5:c5f700c1e1af | 146 | b = (1 - f) * a; |
trevieze | 5:c5f700c1e1af | 147 | sqr_a = a * a; |
trevieze | 5:c5f700c1e1af | 148 | sqr_b = b * b; |
trevieze | 5:c5f700c1e1af | 149 | |
trevieze | 5:c5f700c1e1af | 150 | /* Calculation */ |
trevieze | 5:c5f700c1e1af | 151 | L = to_lon - from_lon; |
trevieze | 5:c5f700c1e1af | 152 | phi1 = from_lat; |
trevieze | 5:c5f700c1e1af | 153 | phi2 = to_lat; |
trevieze | 5:c5f700c1e1af | 154 | U1 = atan((1 - f) * tan(phi1)); |
trevieze | 5:c5f700c1e1af | 155 | U2 = atan((1 - f) * tan(phi2)); |
trevieze | 5:c5f700c1e1af | 156 | sin_U1 = sin(U1); |
trevieze | 5:c5f700c1e1af | 157 | sin_U2 = sin(U2); |
trevieze | 5:c5f700c1e1af | 158 | cos_U1 = cos(U1); |
trevieze | 5:c5f700c1e1af | 159 | cos_U2 = cos(U2); |
trevieze | 5:c5f700c1e1af | 160 | |
trevieze | 5:c5f700c1e1af | 161 | /* Initialize iteration */ |
trevieze | 5:c5f700c1e1af | 162 | sigma = 0; |
trevieze | 5:c5f700c1e1af | 163 | sin_sigma = sin(sigma); |
trevieze | 5:c5f700c1e1af | 164 | cos_sigma = cos(sigma); |
trevieze | 5:c5f700c1e1af | 165 | cos_2_sigmam = 0; |
trevieze | 5:c5f700c1e1af | 166 | sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; |
trevieze | 5:c5f700c1e1af | 167 | sqr_cos_alpha = 0; |
trevieze | 5:c5f700c1e1af | 168 | lambda = L; |
trevieze | 5:c5f700c1e1af | 169 | sin_lambda = sin(lambda); |
trevieze | 5:c5f700c1e1af | 170 | cos_lambda = cos(lambda); |
trevieze | 5:c5f700c1e1af | 171 | delta_lambda = lambda; |
trevieze | 5:c5f700c1e1af | 172 | remaining_steps = 20; |
trevieze | 5:c5f700c1e1af | 173 | |
trevieze | 5:c5f700c1e1af | 174 | while ((delta_lambda > 1e-12) && (remaining_steps > 0)) |
trevieze | 5:c5f700c1e1af | 175 | { /* Iterate */ |
trevieze | 5:c5f700c1e1af | 176 | /* Variables */ |
trevieze | 5:c5f700c1e1af | 177 | double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; |
trevieze | 5:c5f700c1e1af | 178 | |
trevieze | 5:c5f700c1e1af | 179 | /* Calculation */ |
trevieze | 5:c5f700c1e1af | 180 | tmp1 = cos_U2 * sin_lambda; |
trevieze | 5:c5f700c1e1af | 181 | tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; |
trevieze | 5:c5f700c1e1af | 182 | sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); |
trevieze | 5:c5f700c1e1af | 183 | cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; |
trevieze | 5:c5f700c1e1af | 184 | tan_sigma = sin_sigma / cos_sigma; |
trevieze | 5:c5f700c1e1af | 185 | sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; |
trevieze | 5:c5f700c1e1af | 186 | cos_alpha = cos(asin(sin_alpha)); |
trevieze | 5:c5f700c1e1af | 187 | sqr_cos_alpha = cos_alpha * cos_alpha; |
trevieze | 5:c5f700c1e1af | 188 | cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; |
trevieze | 5:c5f700c1e1af | 189 | sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; |
trevieze | 5:c5f700c1e1af | 190 | C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); |
trevieze | 5:c5f700c1e1af | 191 | lambda_prev = lambda; |
trevieze | 5:c5f700c1e1af | 192 | sigma = asin(sin_sigma); |
trevieze | 5:c5f700c1e1af | 193 | lambda = L + |
trevieze | 5:c5f700c1e1af | 194 | (1 - C) * f * sin_alpha |
trevieze | 5:c5f700c1e1af | 195 | * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); |
trevieze | 5:c5f700c1e1af | 196 | delta_lambda = lambda_prev - lambda; |
trevieze | 5:c5f700c1e1af | 197 | if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; |
trevieze | 5:c5f700c1e1af | 198 | sin_lambda = sin(lambda); |
trevieze | 5:c5f700c1e1af | 199 | cos_lambda = cos(lambda); |
trevieze | 5:c5f700c1e1af | 200 | remaining_steps--; |
trevieze | 5:c5f700c1e1af | 201 | } /* Iterate */ |
trevieze | 5:c5f700c1e1af | 202 | |
trevieze | 5:c5f700c1e1af | 203 | /* More calculation */ |
trevieze | 5:c5f700c1e1af | 204 | sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; |
trevieze | 5:c5f700c1e1af | 205 | A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); |
trevieze | 5:c5f700c1e1af | 206 | B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); |
trevieze | 5:c5f700c1e1af | 207 | delta_sigma = B * sin_sigma * ( |
trevieze | 5:c5f700c1e1af | 208 | cos_2_sigmam + B / 4 * ( |
trevieze | 5:c5f700c1e1af | 209 | cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - |
trevieze | 5:c5f700c1e1af | 210 | B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) |
trevieze | 5:c5f700c1e1af | 211 | )); |
trevieze | 5:c5f700c1e1af | 212 | |
trevieze | 5:c5f700c1e1af | 213 | /* Calculate result */ |
trevieze | 5:c5f700c1e1af | 214 | if ( from_azimuth != 0 ) |
trevieze | 5:c5f700c1e1af | 215 | { |
trevieze | 5:c5f700c1e1af | 216 | double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); |
trevieze | 5:c5f700c1e1af | 217 | from_azimuth = atan(tan_alpha_1); |
trevieze | 5:c5f700c1e1af | 218 | } |
trevieze | 5:c5f700c1e1af | 219 | if ( to_azimuth != 0 ) |
trevieze | 5:c5f700c1e1af | 220 | { |
trevieze | 5:c5f700c1e1af | 221 | double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); |
trevieze | 5:c5f700c1e1af | 222 | to_azimuth = atan(tan_alpha_2); |
trevieze | 5:c5f700c1e1af | 223 | } |
trevieze | 5:c5f700c1e1af | 224 | |
trevieze | 5:c5f700c1e1af | 225 | return b * A * (sigma - delta_sigma); |
trevieze | 5:c5f700c1e1af | 226 | } |
trevieze | 5:c5f700c1e1af | 227 | |
trevieze | 5:c5f700c1e1af | 228 | |
trevieze | 5:c5f700c1e1af | 229 | // Convert Degrees to Radians |
trevieze | 5:c5f700c1e1af | 230 | double NAV::DegreeToRadian(double angle) |
trevieze | 5:c5f700c1e1af | 231 | { |
trevieze | 5:c5f700c1e1af | 232 | return PI * angle / 180.0; |
trevieze | 5:c5f700c1e1af | 233 | } |
trevieze | 5:c5f700c1e1af | 234 | |
trevieze | 5:c5f700c1e1af | 235 | // Convert Radians to Degrees |
trevieze | 5:c5f700c1e1af | 236 | double NAV::RadianToDegree(double angle) |
trevieze | 5:c5f700c1e1af | 237 | { |
trevieze | 5:c5f700c1e1af | 238 | return angle * (180.0 / PI); |
trevieze | 5:c5f700c1e1af | 239 | } |