From 1f2500d268a8de99bcf8632ed02addc96d221ae9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 4 May 2019 15:40:58 +0900 Subject: [PATCH] Rover: follow uses local desired_yaw_cd reducing dependency on shared _desired_yaw_cd with the hope that it can eventually be removed also minor comment update for desired_yaw_cd --- APMrover2/mode.h | 2 +- APMrover2/mode_follow.cpp | 7 ++----- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 544ba6df1e..41601fb88f 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -205,7 +205,7 @@ protected: // private members for waypoint navigation float _distance_to_destination; // distance from vehicle to final destination in meters bool _reached_destination; // true once the vehicle has reached the destination - float _desired_yaw_cd; // desired yaw in centi-degrees + float _desired_yaw_cd; // desired yaw in centi-degrees. used in Auto, Guided and Loiter float _desired_speed; // desired speed in m/s }; diff --git a/APMrover2/mode_follow.cpp b/APMrover2/mode_follow.cpp index 386364b6c7..72e9a3e9d9 100644 --- a/APMrover2/mode_follow.cpp +++ b/APMrover2/mode_follow.cpp @@ -11,9 +11,6 @@ bool ModeFollow::_enter() // initialise waypoint speed set_desired_speed_to_default(); - // initialise heading to current heading - _desired_yaw_cd = ahrs.yaw_sensor; - return true; } @@ -64,10 +61,10 @@ void ModeFollow::update() } // calculate vehicle heading - _desired_yaw_cd = wrap_180_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x) * DEGX100); + const float desired_yaw_cd = wrap_180_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x) * DEGX100); // run steering and throttle controllers - calc_steering_to_heading(_desired_yaw_cd); + calc_steering_to_heading(desired_yaw_cd); calc_throttle(desired_speed, true); }