aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AR_WPNav: remove unnecessary temp variable

Browse Source 
also heading-unknown made a const
also minor comment fix
also removed unused include
This commit is contained in:
Randy Mackay 2019-05-09 19:48:48 +09:00
parent 6f97f56726
commit b0f0b16135
2 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -219,7 +219,7 @@ bool AR_WPNav::use_pivot_steering_at_next_WP(float yaw_error_cd) const
return false; return false;
} }
// if error is larger than pivot_turn_angle then use pivot steering at next WP // if error is larger than _pivot_angle then use pivot steering at next WP
if (fabsf(yaw_error_cd) * 0.01f > _pivot_angle) { if (fabsf(yaw_error_cd) * 0.01f > _pivot_angle) {
return true; return true;
} }
@ -239,7 +239,7 @@ bool AR_WPNav::use_pivot_steering(float yaw_error_cd)
// calc bearing error // calc bearing error
const float yaw_error = fabsf(yaw_error_cd) * 0.01f; const float yaw_error = fabsf(yaw_error_cd) * 0.01f;
// if error is larger than pivot_turn_angle start pivot steering // if error is larger than _pivot_angle start pivot steering
if (yaw_error > _pivot_angle) { if (yaw_error > _pivot_angle) {
_pivot_active = true; _pivot_active = true;
return true; return true;
@ -295,15 +295,14 @@ void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
_nav_controller.update_waypoint((_reached_destination ? current_loc : _origin), _destination, _radius); _nav_controller.update_waypoint((_reached_destination ? current_loc : _origin), _destination, _radius);
// retrieve lateral acceleration, heading back towards line and crosstrack error // retrieve lateral acceleration, heading back towards line and crosstrack error
float desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_turn_max_mss, _turn_max_mss); _desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_turn_max_mss, _turn_max_mss);
_desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd()); _desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd());
if (_reversed) { if (_reversed) {
desired_lat_accel *= -1.0f; _desired_lat_accel *= -1.0f;
_desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000); _desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000);
} }
_cross_track_error = _nav_controller.crosstrack_error(); _cross_track_error = _nav_controller.crosstrack_error();
_desired_lat_accel = desired_lat_accel; _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed);
_desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(desired_lat_accel, current_speed);
} }
} }

3
libraries/AR_WPNav/AR_WPNav.h
View File

@ -1,11 +1,10 @@
#pragma once #pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <APM_Control/AR_AttitudeControl.h> #include <APM_Control/AR_AttitudeControl.h>
#include <AP_Navigation/AP_Navigation.h> #include <AP_Navigation/AP_Navigation.h>
#define AR_WPNAV_HEADING_UNKNOWN 99999.0f // used to indicate to set_desired_location method that next leg's heading is unknown const float AR_WPNAV_HEADING_UNKNOWN = 99999.0f; // used to indicate to set_desired_location method that next leg's heading is unknown
class AR_WPNav { class AR_WPNav {
public: public: