fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:14:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

replace geo _wrap_pi with matrix::wrap_pi

Browse Source
This commit is contained in:
Daniel Agar 2018-06-09 16:22:47 -04:00 committed by Lorenz Meier
parent 5c64879688
commit a8b73bd139
5 changed files with 23 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
attitude_fw/ecl_wheel_controller.cpp
View File

@ -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;

143
geo/geo.cpp
View File

@ -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_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;
}

5
geo/geo.h
View File

@ -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);

7
l1/ecl_l1_pos_controller.cpp
View File

@ -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;

2
l1/ecl_l1_pos_controller.h
View File

@ -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.