Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNAV: revert AP_Math class change

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-05 12:34:54 +10:00
parent 33555b7f12
commit c08b62f9e4
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -125,7 +125,7 @@ void AC_Circle::update()
_angle_total += angle_change;
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if (!AP_Math::is_zero(_radius)) {
if (!is_zero(_radius)) {
// calculate target position
Vector3f target;
target.x = _center.x + _radius * cosf(-_angle);
@ -179,7 +179,7 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result)
float dist = pythagorous2(vec.x, vec.y);
// if current location is exactly at the center of the circle return edge directly behind vehicle
if (AP_Math::is_zero(dist)) {
if (is_zero(dist)) {
result.x = _center.x - _radius * _ahrs.cos_yaw();
result.y = _center.y - _radius * _ahrs.sin_yaw();
result.z = _center.z;
@ -243,7 +243,7 @@ void AC_Circle::init_start_angle(bool use_heading)
} else {
// if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
const Vector3f &curr_pos = _inav.get_position();
if (AP_Math::is_equal(curr_pos.x,_center.x) && AP_Math::is_equal(curr_pos.y,_center.y)) {
if (is_equal(curr_pos.x,_center.x) && is_equal(curr_pos.y,_center.y)) {
_angle = wrap_PI(_ahrs.yaw-PI);
} else {
// get bearing from circle center to vehicle in radians

12
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -262,7 +262,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
float desired_speed = desired_vel.length();
if (!AP_Math::is_zero(desired_speed)) {
if (!is_zero(desired_speed)) {
Vector2f desired_vel_norm = desired_vel/desired_speed;
float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms;
@ -411,7 +411,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
_track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination
if (AP_Math::is_zero(_track_length)) {
if (is_zero(_track_length)) {
// avoid possible divide by zero
_pos_delta_unit.x = 0;
_pos_delta_unit.y = 0;
@ -682,15 +682,15 @@ void AC_WPNav::calculate_wp_leash_length()
}
// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if(AP_Math::is_zero(pos_delta_unit_z) && AP_Math::is_zero(pos_delta_unit_xy)){
if(is_zero(pos_delta_unit_z) && is_zero(pos_delta_unit_xy)){
_track_accel = 0;
_track_speed = 0;
_track_leash_length = WPNAV_LEASH_LENGTH_MIN;
}else if(AP_Math::is_zero(_pos_delta_unit.z)){
}else if(is_zero(_pos_delta_unit.z)){
_track_accel = _wp_accel_cms/pos_delta_unit_xy;
_track_speed = _wp_speed_cms/pos_delta_unit_xy;
_track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;
}else if(AP_Math::is_zero(pos_delta_unit_xy)){
}else if(is_zero(pos_delta_unit_xy)){
_track_accel = _wp_accel_z_cms/pos_delta_unit_z;
_track_speed = speed_z/pos_delta_unit_z;
_track_leash_length = leash_z/pos_delta_unit_z;
@ -954,7 +954,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length();
if (!AP_Math::is_zero(target_vel_length)) {
if (!is_zero(target_vel_length)) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}