Rover: yaw_error_cd becomes local variable in Loiter only

APMrover2/mode.cpp

@@ -369,7 +369,7 @@ float Mode::calc_speed_nudge(float target_speed, bool reversed)
 
 // high level call to navigate to waypoint
 // uses wp_nav to calculate turn rate and speed to drive along the path from origin to destination
-// this function updates _distance_to_destination and _yaw_error_cd
+// this function updates _distance_to_destination
 void Mode::navigate_to_waypoint()
 {
     // update navigation controller
@@ -382,8 +382,6 @@ void Mode::navigate_to_waypoint()
     calc_throttle(desired_speed, true);
 
     float desired_heading_cd = g2.wp_nav.wp_bearing_cd();
-    _yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);
-
     if (rover.sailboat_use_indirect_route(desired_heading_cd)) {
         // sailboats use heading controller when tacking upwind
         desired_heading_cd = rover.sailboat_calc_heading(desired_heading_cd);
@@ -438,9 +436,6 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse
 // rate_max is a maximum turn rate in deg/s.  set to zero to use default turn rate limits
 void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max_degs)
 {
-    // calculate yaw error so it can be used for reporting and slowing the vehicle
-    _yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);
-
     // call heading controller
     const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f),
                                                                          radians(rate_max_degs),

APMrover2/mode.h

@@ -206,7 +206,6 @@ protected:
     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 _yaw_error_cd;        // error between desired yaw and actual yaw in centi-degrees
     float _desired_speed;       // desired speed in m/s
 };
 

APMrover2/mode_follow.cpp

@@ -13,7 +13,6 @@ bool ModeFollow::_enter()
 
     // initialise heading to current heading
     _desired_yaw_cd = ahrs.yaw_sensor;
-    _yaw_error_cd = 0.0f;
 
     return true;
 }

APMrover2/mode_loiter.cpp

@@ -13,7 +13,6 @@ bool ModeLoiter::_enter()
 
     // initialise heading to current heading
     _desired_yaw_cd = ahrs.yaw_sensor;
-    _yaw_error_cd = 0.0f;
 
     return true;
 }
@@ -28,7 +27,6 @@ void ModeLoiter::update()
         // sailboats do not stop
         const float desired_speed_within_radius = g2.motors.has_sail() ? 0.1f : 0.0f;
         _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
-        _yaw_error_cd = 0.0f;
     } else {
         // P controller with hard-coded gain to convert distance to desired speed
         // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
@@ -36,17 +34,17 @@ void ModeLoiter::update()
 
         // calculate bearing to destination
         _desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
-        _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
+        float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
         // if destination is behind vehicle, reverse towards it
-        if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
+        if (fabsf(yaw_error_cd) > 9000 && g2.loit_type == 0) {
             _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
-            _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
+            yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
             _desired_speed = -_desired_speed;
         }
 
         // reduce desired speed if yaw_error is large
         // 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
-        float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
+        float yaw_error_ratio = 1.0f - constrain_float(fabsf(yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
         _desired_speed *= yaw_error_ratio;
     }