From cd8b1f278ce45749a749da813e76f7339255b1e6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 6 May 2014 20:40:45 +0900 Subject: [PATCH] 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 | 5 ++--- libraries/AC_AttitudeControl/AC_PosControl.h | 12 ++++-------- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 284a3bfe2d..c6dbd356b5 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/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)); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index fda1a0e8c7..351c9b3702 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/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?