Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Randy Mackay 2014-05-06 20:40:45 +09:00
parent 48ec0caf75
commit cd8b1f278c
2 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -37,7 +37,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_dt(POSCONTROL_DT_10HZ), _dt(POSCONTROL_DT_10HZ),
_last_update_xy_ms(0), _last_update_xy_ms(0),
_last_update_z_ms(0), _last_update_z_ms(0),
_step(0),
_speed_down_cms(POSCONTROL_SPEED_DOWN), _speed_down_cms(POSCONTROL_SPEED_DOWN),
_speed_up_cms(POSCONTROL_SPEED_UP), _speed_up_cms(POSCONTROL_SPEED_UP),
_speed_cms(POSCONTROL_SPEED), _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 /// set_leash_length() should have been called before this method
void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const 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(); Vector3f curr_vel = _inav.get_velocity();
float linear_distance; // the distance at which we swap from a linear to sqrt response 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 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 // 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 // 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)); 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));

12
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -69,10 +69,10 @@ public:
void set_speed_z(float speed_down, float speed_up); void set_speed_z(float speed_down, float speed_up);
/// get_speed_up - accessor for current up speed in cm/s /// 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 /// 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 /// set_accel_z - set vertical acceleration in cm/s/s
/// leash length will be recalculated the next time update_z_controller() is called /// 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; } 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 // 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[]; 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 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_xy_ms; // system time of last update_xy_controller call
uint32_t _last_update_z_ms; // system time of last update_z_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_down_cms; // max descent rate in cm/s
float _speed_up_cms; // max climb rate in cm/s float _speed_up_cms; // max climb rate in cm/s
float _speed_cms; // max horizontal speed 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; // 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_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 _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 // output from controller
float _roll_target; // desired roll angle in centi-degrees calculated by position 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) 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_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_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 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_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? Vector3f _accel_error; // desired acceleration in cm/s/s // To-Do: are xy actually required?