From a8b73bd139dbad1bc7bc09b1142300aa0a2b0bb5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 9 Jun 2018 16:22:47 -0400 Subject: [PATCH] replace geo _wrap_pi with matrix::wrap_pi --- attitude_fw/ecl_wheel_controller.cpp | 5 +- geo/geo.cpp | 143 +++------------------------ geo/geo.h | 5 - l1/ecl_l1_pos_controller.cpp | 7 +- l1/ecl_l1_pos_controller.h | 2 +- 5 files changed, 23 insertions(+), 139 deletions(-) diff --git a/attitude_fw/ecl_wheel_controller.cpp b/attitude_fw/ecl_wheel_controller.cpp index 58f6e95e2f..c291f3f054 100644 --- a/attitude_fw/ecl_wheel_controller.cpp +++ b/attitude_fw/ecl_wheel_controller.cpp @@ -42,6 +42,9 @@ #include #include #include +#include + +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; diff --git a/geo/geo.cpp b/geo/geo.cpp index 6a98a1f230..9ba944c676 100644 --- a/geo/geo.cpp +++ b/geo/geo.cpp @@ -46,8 +46,12 @@ #include #include +#include #include +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(sin(d_lon) * cos(lat_next_rad)); const float x = static_cast(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_y = (double)radius * cos((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 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; -} diff --git a/geo/geo.h b/geo/geo.h index 2289e28331..bddad85ef2 100644 --- a/geo/geo.h +++ b/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); diff --git a/l1/ecl_l1_pos_controller.cpp b/l1/ecl_l1_pos_controller.cpp index eae0ee3248..010952070b 100644 --- a/l1/ecl_l1_pos_controller.cpp +++ b/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); - float eta = _target_bearing - _wrap_pi(current_heading); - eta = _wrap_pi(eta); + _target_bearing = _nav_bearing = wrap_pi(navigation_heading); + + float eta = wrap_pi(_target_bearing - wrap_pi(current_heading)); /* consequently the bearing error is exactly eta: */ _bearing_error = eta; diff --git a/l1/ecl_l1_pos_controller.h b/l1/ecl_l1_pos_controller.h index dd26a39150..5928c66658 100644 --- a/l1/ecl_l1_pos_controller.h +++ b/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.