From 64eaadb332c5b3ae021bbf85468f71fd2cbbbf08 Mon Sep 17 00:00:00 2001 From: uncrustify Date: Thu, 16 Aug 2012 23:20:14 -0700 Subject: [PATCH] uncrustify libraries/AP_Math/location.cpp --- libraries/AP_Math/location.cpp | 114 ++++++++++++++++----------------- 1 file changed, 57 insertions(+), 57 deletions(-) diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 7035ce59da..9c9a6948b9 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -18,7 +18,7 @@ */ /* - this module deals with calculations involving struct Location + * this module deals with calculations involving struct Location */ #include @@ -29,17 +29,17 @@ static float longitude_scale(const struct Location *loc) { - static int32_t last_lat; - static float scale = 1.0; - if (labs(last_lat - loc->lat) < 100000) { - // we are within 0.01 degrees (about 1km) of the - // same latitude. We can avoid the cos() and return - // the same scale factor. - return scale; - } - scale = cos((fabs((float)loc->lat)/1.0e7) * 0.0174532925); - last_lat = loc->lat; - return scale; + static int32_t last_lat; + static float scale = 1.0; + if (labs(last_lat - loc->lat) < 100000) { + // we are within 0.01 degrees (about 1km) of the + // same latitude. We can avoid the cos() and return + // the same scale factor. + return scale; + } + scale = cos((fabs((float)loc->lat)/1.0e7) * 0.0174532925); + last_lat = loc->lat; + return scale; } @@ -48,13 +48,13 @@ static float longitude_scale(const struct Location *loc) // if one of the locations is invalid float get_distance(const struct Location *loc1, const struct Location *loc2) { - if (loc1->lat == 0 || loc1->lng == 0) - return -1; - if(loc2->lat == 0 || loc2->lng == 0) - return -1; - float dlat = (float)(loc2->lat - loc1->lat); - float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2); - return sqrt(sq(dlat) + sq(dlong)) * .01113195; + if (loc1->lat == 0 || loc1->lng == 0) + return -1; + if(loc2->lat == 0 || loc2->lng == 0) + return -1; + float dlat = (float)(loc2->lat - loc1->lat); + float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2); + return sqrt(sq(dlat) + sq(dlong)) * .01113195; } // return distance in centimeters to between two locations, or -1 if @@ -67,11 +67,11 @@ int32_t get_distance_cm(const struct Location *loc1, const struct Location *loc2 // return bearing in centi-degrees between two locations int32_t get_bearing_cd(const struct Location *loc1, const struct Location *loc2) { - int32_t off_x = loc2->lng - loc1->lng; - int32_t off_y = (loc2->lat - loc1->lat) / longitude_scale(loc2); - int32_t bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; - if (bearing < 0) bearing += 36000; - return bearing; + int32_t off_x = loc2->lng - loc1->lng; + int32_t off_y = (loc2->lat - loc1->lat) / longitude_scale(loc2); + int32_t bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + if (bearing < 0) bearing += 36000; + return bearing; } // see if location is past a line perpendicular to @@ -80,16 +80,16 @@ int32_t get_bearing_cd(const struct Location *loc1, const struct Location *loc2) // then this function returns true if we have flown past // the target waypoint bool location_passed_point(struct Location &location, - struct Location &point1, - struct Location &point2) + struct Location &point1, + struct Location &point2) { - // the 3 points form a triangle. If the angle between lines - // point1->point2 and location->point2 is greater than 90 - // degrees then we have passed the waypoint - Vector2f loc1(location.lat, location.lng); - Vector2f pt1(point1.lat, point1.lng); - Vector2f pt2(point2.lat, point2.lng); - float angle = (loc1 - pt2).angle(pt1 - pt2); + // the 3 points form a triangle. If the angle between lines + // point1->point2 and location->point2 is greater than 90 + // degrees then we have passed the waypoint + Vector2f loc1(location.lat, location.lng); + Vector2f pt1(point1.lat, point1.lng); + Vector2f pt2(point2.lat, point2.lng); + float angle = (loc1 - pt2).angle(pt1 - pt2); if (isinf(angle)) { // two of the points are co-located. // If location is equal to point2 then say we have passed the @@ -99,34 +99,34 @@ bool location_passed_point(struct Location &location, } return false; } else if (angle == 0) { - // if we are exactly on the line between point1 and - // point2 then we are past the waypoint if the - // distance from location to point1 is greater then - // the distance from point2 to point1 - return get_distance(&location, &point1) > - get_distance(&point2, &point1); + // if we are exactly on the line between point1 and + // point2 then we are past the waypoint if the + // distance from location to point1 is greater then + // the distance from point2 to point1 + return get_distance(&location, &point1) > + get_distance(&point2, &point1); - } - if (degrees(angle) > 90) { - return true; - } - return false; + } + if (degrees(angle) > 90) { + return true; + } + return false; } /* - extrapolate latitude/longitude given bearing and distance - thanks to http://www.movable-type.co.uk/scripts/latlong.html - - This function is precise, but costs about 1.7 milliseconds on an AVR2560 -*/ + * extrapolate latitude/longitude given bearing and distance + * thanks to http://www.movable-type.co.uk/scripts/latlong.html + * + * This function is precise, but costs about 1.7 milliseconds on an AVR2560 + */ void location_update(struct Location *loc, float bearing, float distance) { - float lat1 = radians(loc->lat*1.0e-7); - float lon1 = radians(loc->lng*1.0e-7); - float brng = radians(bearing); - float dr = distance/RADIUS_OF_EARTH; + float lat1 = radians(loc->lat*1.0e-7); + float lon1 = radians(loc->lng*1.0e-7); + float brng = radians(bearing); + float dr = distance/RADIUS_OF_EARTH; - float lat2 = asin(sin(lat1)*cos(dr) + + float lat2 = asin(sin(lat1)*cos(dr) + cos(lat1)*sin(dr)*cos(brng)); float lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1), cos(dr)-sin(lat1)*sin(lat2)); @@ -135,9 +135,9 @@ void location_update(struct Location *loc, float bearing, float distance) } /* - extrapolate latitude/longitude given distances north and east - This function costs about 80 usec on an AVR2560 -*/ + * extrapolate latitude/longitude given distances north and east + * This function costs about 80 usec on an AVR2560 + */ void location_offset(struct Location *loc, float ofs_north, float ofs_east) { if (ofs_north != 0 || ofs_east != 0) {