replace geo _wrap_pi with matrix::wrap_pi

attitude_fw/ecl_wheel_controller.cpp

@@ -42,6 +42,9 @@
 #include <float.h>
 #include <geo/geo.h>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+using matrix::wrap_pi;
 
 ECL_WheelController::ECL_WheelController() :
 	ECL_Controller("wheel")
@@ -112,7 +115,7 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
 	}
 
 	/* Calculate the error */
-	float yaw_error = _wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
+	float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
 
 	/*  Apply P controller: rate setpoint from current error and time constant */
 	_rate_setpoint =  yaw_error / _tc;

geo/geo.cpp

@@ -46,8 +46,12 @@
 #include <ecl.h>
 
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 #include <cfloat>
 
+using matrix::wrap_pi;
+using matrix::wrap_2pi;
+
 /*
  * Azimuthal Equidistant Projection
  * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
@@ -291,7 +295,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
 
 	} else {
 		float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
-		heading = _wrap_2pi(heading + M_PI_F);
+		heading = wrap_2pi(heading + M_PI_F);
 		waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
 	}
 }
@@ -299,7 +303,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
 void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
 					double *lat_target, double *lon_target)
 {
-	bearing = _wrap_2pi(bearing);
+	bearing = wrap_2pi(bearing);
 	double radius_ratio = (double)fabs((double)dist) / CONSTANTS_RADIUS_OF_EARTH;
 
 	double lat_start_rad = math::radians(lat_start);
@@ -327,7 +331,7 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne
 	const float y = static_cast<float>(sin(d_lon) * cos(lat_next_rad));
 	const float x = static_cast<float>(cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
 
-	return _wrap_pi(atan2f(y, x));
+	return wrap_pi(atan2f(y, x));
 }
 
 void
@@ -390,7 +394,7 @@ int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat
 
 	float bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	float bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
-	float bearing_diff = _wrap_pi(bearing_track - bearing_end);
+	float bearing_diff = wrap_pi(bearing_track - bearing_end);
 
 	// Return past_end = true if past end point of line
 	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
@@ -402,10 +406,10 @@ int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat
 	crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
 
 	if (sinf(bearing_diff) >= 0) {
-		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
+		crosstrack_error->bearing = wrap_pi(bearing_track - M_PI_2_F);
 
 	} else {
-		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
+		crosstrack_error->bearing = wrap_pi(bearing_track + M_PI_2_F);
 	}
 
 	return_value = 0;
@@ -491,8 +495,8 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_
 
 		double start_disp_x = (double)radius * sin((double)arc_start_bearing);
 		double start_disp_y = (double)radius * cos((double)arc_start_bearing);
-		double end_disp_x = (double)radius * sin((double)_wrap_pi(arc_start_bearing + arc_sweep));
+		double end_disp_x = (double)radius * sin((double)wrap_pi(arc_start_bearing + arc_sweep));
-		double end_disp_y = (double)radius * cos((double)_wrap_pi(arc_start_bearing + arc_sweep));
+		double end_disp_y = (double)radius * cos((double)wrap_pi(arc_start_bearing + arc_sweep));
 		double lon_start = lon_now + start_disp_x / 111111.0;
 		double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
 		double lon_end = lon_now + end_disp_x / 111111.0;
@@ -511,8 +515,9 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_
 		}
 	}
 
-	crosstrack_error->bearing = _wrap_pi(crosstrack_error->bearing);
+	crosstrack_error->bearing = wrap_pi(crosstrack_error->bearing);
 	return_value = 0;
+
 	return return_value;
 }
 
@@ -553,123 +558,3 @@ float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
 
 	return sqrtf(dx * dx + dy * dy + dz * dz);
 }
-
-float _wrap_pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!ISFINITE(bearing)) {
-		return bearing;
-	}
-
-	int c = 0;
-
-	while (bearing >= M_PI_F) {
-		bearing -= (2 * M_PI_F);
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	c = 0;
-
-	while (bearing < -M_PI_F) {
-		bearing += (2 * M_PI_F);
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	return bearing;
-}
-
-float _wrap_2pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!ISFINITE(bearing)) {
-		return bearing;
-	}
-
-	int c = 0;
-
-	while (bearing >= (2 * M_PI_F)) {
-		bearing -= (2 * M_PI_F);
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	c = 0;
-
-	while (bearing < 0.0f) {
-		bearing += (2 * M_PI_F);
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	return bearing;
-}
-
-float _wrap_180(float bearing)
-{
-	/* value is inf or NaN */
-	if (!ISFINITE(bearing)) {
-		return bearing;
-	}
-
-	int c = 0;
-
-	while (bearing >= 180.0f) {
-		bearing -= 360.0f;
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	c = 0;
-
-	while (bearing < -180.0f) {
-		bearing += 360.0f;
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	return bearing;
-}
-
-float _wrap_360(float bearing)
-{
-	/* value is inf or NaN */
-	if (!ISFINITE(bearing)) {
-		return bearing;
-	}
-
-	int c = 0;
-
-	while (bearing >= 360.0f) {
-		bearing -= 360.0f;
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	c = 0;
-
-	while (bearing < 0.0f) {
-		bearing += 360.0f;
-
-		if (c++ > 3) {
-			return NAN;
-		}
-	}
-
-	return bearing;
-}

geo/geo.h

@@ -301,8 +301,3 @@ float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float a
 float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
 		float x_next, float y_next, float z_next,
 		float *dist_xy, float *dist_z);
-
-float _wrap_180(float bearing);
-float _wrap_360(float bearing);
-float _wrap_pi(float bearing);
-float _wrap_2pi(float bearing);

l1/ecl_l1_pos_controller.cpp

@@ -43,6 +43,7 @@
 #include "ecl_l1_pos_controller.h"
 
 using matrix::Vector2f;
+using matrix::wrap_pi;
 
 float ECL_L1_Pos_Controller::nav_roll()
 {
@@ -292,9 +293,9 @@ void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float cur
 	 * (and no crosstrack correction occurs),
 	 * target and navigation bearing become the same
 	 */
-	_target_bearing = _nav_bearing = _wrap_pi(navigation_heading);
+	_target_bearing = _nav_bearing = wrap_pi(navigation_heading);
-	float eta = _target_bearing - _wrap_pi(current_heading);
+
-	eta = _wrap_pi(eta);
+	float eta = wrap_pi(_target_bearing - wrap_pi(current_heading));
 
 	/* consequently the bearing error is exactly eta: */
 	_bearing_error = eta;

l1/ecl_l1_pos_controller.h

@@ -76,7 +76,7 @@ public:
 	 *
 	 * @return bearing angle (-pi..pi, in NED frame)
 	 */
-	float nav_bearing() { return _wrap_pi(_nav_bearing); }
+	float nav_bearing() { return matrix::wrap_pi(_nav_bearing); }
 
 	/**
 	 * Get lateral acceleration demand.