Major formatting/whitespace cleanup

1 files changed, 69 insertions, 51 deletions

diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index bc467bfa3..789368fbd 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
 {
 // This function returns the distance to the nearest point on the track line.  Distance is positive if current
-// position is right of the track and negative if left of the track as seen from a point on the track line 
+// position is right of the track and negative if left of the track as seen from a point on the track line
 // headed towards the end point.
 
 	crosstrack_error_s return_var;
@@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now,
 	float bearing_end;
 	float bearing_track;
 	float bearing_diff;
-	
+
 	return_var.error = true;		// Set error flag, cleared when valid result calculated.
 	return_var.past_end = false;
 	return_var.distance = 0.0f;
 	return_var.bearing = 0.0f;
-	
+
 	// Return error if arguments are bad
-	if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
-	
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
+
 	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
 	bearing_diff = bearing_track - bearing_end;
 	bearing_diff = _wrapPI(bearing_diff);
-	
+
 	// Return past_end = true if past end point of line
-	if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
+	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
 		return_var.past_end = true;
 		return_var.error = false;
 		return return_var;
 	}
-	
+
 	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-	return_var.distance = (dist_to_end)*sin(bearing_diff);
-	if(sin(bearing_diff) >=0 ) {
+	return_var.distance = (dist_to_end) * sin(bearing_diff);
+
+	if (sin(bearing_diff) >= 0) {
 		return_var.bearing = _wrapPI(bearing_track - M_PI_2_F);
+
 	} else {
 		return_var.bearing = _wrapPI(bearing_track + M_PI_2_F);
 	}
+
 	return_var.error = false;
-	
+
 	return return_var;
 
 }
 
 
-__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, 
-									float radius, float arc_start_bearing, float arc_sweep)
+__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
+		float radius, float arc_start_bearing, float arc_sweep)
 {
 	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
 	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
@@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
 	float	bearing_sector_end;
 	float	bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
 	bool	in_sector;
-	
+
 	return_var.error = true;		// Set error flag, cleared when valid result calculated.
 	return_var.past_end = false;
 	return_var.distance = 0.0f;
 	return_var.bearing = 0.0f;
-	
+
 	// Return error if arguments are bad
-	if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
-	
-	
-	if(arc_sweep >= 0) {
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
+
+
+	if (arc_sweep >= 0) {
 		bearing_sector_start = arc_start_bearing;
 		bearing_sector_end = arc_start_bearing + arc_sweep;
-		if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
+		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
 	} else {
 		bearing_sector_end = arc_start_bearing;
 		bearing_sector_start = arc_start_bearing - arc_sweep;
-		if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
+
+		if (bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
 	}
+
 	in_sector = false;
-	
+
 	// Case where sector does not span zero
-	if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
-	
+	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+
 	// Case where sector does span zero
-	if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true;
-	
+	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+
 	// If in the sector then calculate distance and bearing to closest point
-	if(in_sector) {
+	if (in_sector) {
 		return_var.past_end = false;
 		float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
-		
-		if(dist_to_center <= radius) {
+
+		if (dist_to_center <= radius) {
 			return_var.distance = radius - dist_to_center;
 			return_var.bearing = bearing_now + M_PI_F;
+
 		} else {
 			return_var.distance = dist_to_center - radius;
 			return_var.bearing = bearing_now;
 		}
-		
-	// If out of the sector then calculate dist and bearing to start or end point
+
+		// If out of the sector then calculate dist and bearing to start or end point
+
 	} else {
-	
-	// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude) 
-	// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
-	// calculate the position of the start and end points.  We should not be doing this often
-	// as this function generally will not be called repeatedly when we are out of the sector.
-	
-	// TO DO - this is messed up and won't compile
+
+		// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude)
+		// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
+		// calculate the position of the start and end points.  We should not be doing this often
+		// as this function generally will not be called repeatedly when we are out of the sector.
+
+		// TO DO - this is messed up and won't compile
 		float start_disp_x = radius * sin(arc_start_bearing);
 		float start_disp_y = radius * cos(arc_start_bearing);
-		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep));
-		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep));
-		float lon_start = lon_now + start_disp_x/111111.0d;
-		float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d;
-		float lon_end = lon_now + end_disp_x/111111.0d;
-		float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d;
+		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
+		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
+		float lon_start = lon_now + start_disp_x / 111111.0d;
+		float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
+		float lon_end = lon_now + end_disp_x / 111111.0d;
+		float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
 		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
 		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-		if(dist_to_start < dist_to_end) {
+
+		if (dist_to_start < dist_to_end) {
 			return_var.distance = dist_to_start;
 			return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+
 		} else {
 			return_var.past_end = true;
 			return_var.distance = dist_to_end;
@@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
 		}
 
 	}
-	
+
 	return_var.bearing = _wrapPI(return_var.bearing);
 	return_var.error = false;
-	return return_var;	
+	return return_var;
 }
 
 float _wrapPI(float bearing)
@@ -227,21 +238,25 @@ float _wrapPI(float bearing)
 	while (bearing > M_PI_F) {
 		bearing = bearing - M_TWOPI_F;
 	}
+
 	while (bearing <=  -M_PI_F) {
 		bearing = bearing + M_TWOPI_F;
 	}
+
 	return bearing;
 }
-		
+
 float _wrap2PI(float bearing)
 {
 
 	while (bearing >= M_TWOPI_F) {
 		bearing = bearing - M_TWOPI_F;
 	}
+
 	while (bearing <  0.0f) {
 		bearing = bearing + M_TWOPI_F;
 	}
+
 	return bearing;
 }
 
@@ -251,23 +266,26 @@ float _wrap180(float bearing)
 	while (bearing > 180.0f) {
 		bearing = bearing - 360.0f;
 	}
+
 	while (bearing <=  -180.0f) {
 		bearing = bearing + 360.0f;
 	}
+
 	return bearing;
 }
-		
+
 float _wrap360(float bearing)
 {
 
 	while (bearing >= 360.0f) {
 		bearing = bearing - 360.0f;
 	}
+
 	while (bearing <  0.0f) {
 		bearing = bearing + 360.0f;
 	}
+
 	return bearing;
 }
-		
 
-	
\ No newline at end of file
+