AC_PosControl: remove unused _cos_yaw variable

Also removed _sin_yaw, _cos_pitch, _step saving a total of 17bytes of
RAM
Also made get_speed_up, get_speed_down, lean_angles_to_accel functions
const

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -37,7 +37,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
     _dt(POSCONTROL_DT_10HZ),
     _last_update_xy_ms(0),
     _last_update_z_ms(0),
-    _step(0),
     _speed_down_cms(POSCONTROL_SPEED_DOWN),
     _speed_up_cms(POSCONTROL_SPEED_UP),
     _speed_cms(POSCONTROL_SPEED),
@@ -429,7 +428,7 @@ void AC_PosControl::set_target_to_stopping_point_xy()
 ///     set_leash_length() should have been called before this method
 void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
 {
-	Vector3f curr_pos = _inav.get_position();
+	const Vector3f curr_pos = _inav.get_position();
 	Vector3f curr_vel = _inav.get_velocity();
     float linear_distance;      // the distance at which we swap from a linear to sqrt response
     float linear_velocity;      // the velocity above which we swap from a linear to sqrt response
@@ -742,7 +741,7 @@ void AC_PosControl::accel_to_lean_angles()
 }
 
 // get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
-void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss)
+void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const
 {
     // rotate our roll, pitch angles into lat/lon frame
     accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / max(_ahrs.cos_pitch(),0.5)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / max(_ahrs.cos_roll(),0.5));

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -69,10 +69,10 @@ public:
     void set_speed_z(float speed_down, float speed_up);
 
     /// get_speed_up - accessor for current up speed in cm/s
-    float get_speed_up() { return _speed_up_cms; }
+    float get_speed_up() const { return _speed_up_cms; }
 
     /// get_speed_down - accessors for current down speed in cm/s.  Will be a negative number
-    float get_speed_down() { return _speed_down_cms; }
+    float get_speed_down() const { return _speed_down_cms; }
 
     /// set_accel_z - set vertical acceleration in cm/s/s
     ///     leash length will be recalculated the next time update_z_controller() is called
@@ -211,7 +211,7 @@ public:
     const Vector3f& get_accel_target() const { return _accel_target; }
 
     // lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
-    void lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss);
+    void lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const;
 
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -305,7 +305,6 @@ private:
     float       _dt;                    // time difference (in seconds) between calls from the main program
     uint32_t    _last_update_xy_ms;     // system time of last update_xy_controller call
     uint32_t    _last_update_z_ms;      // system time of last update_z_controller call
-    uint8_t     _step;                  // used to decide which portion of position controller to run during this iteration
     float       _speed_down_cms;        // max descent rate in cm/s
     float       _speed_up_cms;          // max climb rate in cm/s
     float       _speed_cms;             // max horizontal speed in cm/s
@@ -314,9 +313,6 @@ private:
     float       _leash;                 // horizontal leash length in cm.  target will never be further than this distance from the vehicle
     float       _leash_down_z;          // vertical leash down in cm.  target will never be further than this distance below the vehicle
     float       _leash_up_z;            // vertical leash up in cm.  target will never be further than this distance above the vehicle
-    float       _cos_yaw;               // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
-    float       _sin_yaw;
-    float       _cos_pitch;
 
     // output from controller
     float       _roll_target;           // desired roll angle in centi-degrees calculated by position controller
@@ -328,7 +324,7 @@ private:
     Vector2f    _vel_desired;           // desired velocity in cm/s in lat and lon directions (provided by external callers of move_target_at_rate() method)
     Vector3f    _vel_target;            // velocity target in cm/s calculated by pos_to_rate step
     Vector3f    _vel_error;             // error between desired and actual acceleration in cm/s
-    Vector3f    _vel_last;              // previous iterations velocity in cm/s
+    Vector2f    _vel_last;              // previous iterations velocity in cm/s
     float       _vel_target_filt_z;     // filtered target vertical velocity
     Vector3f    _accel_target;          // desired acceleration in cm/s/s  // To-Do: are xy actually required?
     Vector3f    _accel_error;           // desired acceleration in cm/s/s  // To-Do: are xy actually required?