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

AP_WPNav: compiler warnings: apply is_zero(float) or is_equal(float)

Browse Source
This commit is contained in:
Tom Pittenger 2015-05-02 02:29:04 -07:00 committed by Andrew Tridgell
parent 98ca790cb9
commit 913d00f525
2 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -1,6 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h> #include <AP_HAL.h>
#include <AC_Circle.h> #include <AC_Circle.h>
#include <AP_Math.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -124,7 +125,7 @@ void AC_Circle::update()
_angle_total += angle_change; _angle_total += angle_change;
// if the circle_radius is zero we are doing panorama so no need to update loiter target // if the circle_radius is zero we are doing panorama so no need to update loiter target
if (_radius != 0.0f) { if (!AP_Math::is_zero(_radius)) {
// calculate target position // calculate target position
Vector3f target; Vector3f target;
target.x = _center.x + _radius * cosf(-_angle); target.x = _center.x + _radius * cosf(-_angle);
@ -178,7 +179,7 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result)
float dist = pythagorous2(vec.x, vec.y); float dist = pythagorous2(vec.x, vec.y);
// if current location is exactly at the center of the circle return edge directly behind vehicle // if current location is exactly at the center of the circle return edge directly behind vehicle
if (dist == 0) { if (AP_Math::is_zero(dist)) {
result.x = _center.x - _radius * _ahrs.cos_yaw(); result.x = _center.x - _radius * _ahrs.cos_yaw();
result.y = _center.y - _radius * _ahrs.sin_yaw(); result.y = _center.y - _radius * _ahrs.sin_yaw();
result.z = _center.z; result.z = _center.z;
@ -242,7 +243,7 @@ void AC_Circle::init_start_angle(bool use_heading)
} else { } 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) // 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(); const Vector3f &curr_pos = _inav.get_position();
if (curr_pos.x == _center.x && curr_pos.y == _center.y) { if (AP_Math::is_equal(curr_pos.x,_center.x) && AP_Math::is_equal(curr_pos.y,_center.y)) {
_angle = wrap_PI(_ahrs.yaw-PI); _angle = wrap_PI(_ahrs.yaw-PI);
} else { } else {
// get bearing from circle center to vehicle in radians // get bearing from circle center to vehicle in radians

14
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -262,11 +262,11 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
float desired_speed = desired_vel.length(); float desired_speed = desired_vel.length();
if (desired_speed != 0.0f) { if (!AP_Math::is_zero(desired_speed)) {
Vector2f desired_vel_norm = desired_vel/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; float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms;
if (_pilot_accel_fwd_cms == 0.0f && _pilot_accel_rgt_cms == 0.0f) { if (_pilot_accel_fwd_cms == 0 && _pilot_accel_rgt_cms == 0) {
drag_speed_delta = min(drag_speed_delta,-_loiter_accel_min_cmss*nav_dt); drag_speed_delta = min(drag_speed_delta,-_loiter_accel_min_cmss*nav_dt);
} }
@ -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 _track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination // calculate each axis' percentage of the total distance to the destination
if (_track_length == 0.0f) { if (AP_Math::is_zero(_track_length)) {
// avoid possible divide by zero // avoid possible divide by zero
_pos_delta_unit.x = 0; _pos_delta_unit.x = 0;
_pos_delta_unit.y = 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 // calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if(pos_delta_unit_z == 0.0f && pos_delta_unit_xy == 0.0f){ if(AP_Math::is_zero(pos_delta_unit_z) &&AP_Math::is_zero(pos_delta_unit_xy)){
_track_accel = 0; _track_accel = 0;
_track_speed = 0; _track_speed = 0;
_track_leash_length = WPNAV_LEASH_LENGTH_MIN; _track_leash_length = WPNAV_LEASH_LENGTH_MIN;
}else if(_pos_delta_unit.z == 0.0f){ }else if(AP_Math::is_zero(_pos_delta_unit.z)){
_track_accel = _wp_accel_cms/pos_delta_unit_xy; _track_accel = _wp_accel_cms/pos_delta_unit_xy;
_track_speed = _wp_speed_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; _track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;
}else if(pos_delta_unit_xy == 0.0f){ }else if(AP_Math::is_zero(pos_delta_unit_xy)){
_track_accel = _wp_accel_z_cms/pos_delta_unit_z; _track_accel = _wp_accel_z_cms/pos_delta_unit_z;
_track_speed = speed_z/pos_delta_unit_z; _track_speed = speed_z/pos_delta_unit_z;
_track_leash_length = leash_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 // 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(); float target_vel_length = target_vel.length();
if (target_vel_length != 0.0f) { if (!AP_Math::is_zero(target_vel_length)) {
_spline_time_scale = _spline_vel_scaler/target_vel_length; _spline_time_scale = _spline_vel_scaler/target_vel_length;
} }