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

AC_AttitudeControl: Fixup some minor mistakes in AC_PosControl

This commit is contained in:
Jonathan Challinger 2014-07-08 02:02:11 -07:00 committed by Randy Mackay
parent 022425584b
commit 5f66027ba3
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -44,8 +44,8 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_accel_z_cms(POSCONTROL_ACCEL_Z),
_accel_cms(POSCONTROL_ACCEL_XY),
_leash(POSCONTROL_LEASH_LENGTH_MIN),
_roll_target(0.0),
_pitch_target(0.0),
_roll_target(0.0f),
_pitch_target(0.0f),
_alt_max(0),
_distance_to_target(0),
_xy_step(0),
@ -136,7 +136,7 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_up and _limit.pos_down?
if ((climb_rate_cms<0 && !_motors.limit.throttle_lower) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += climb_rate_cms * _dt;
_pos_target.z += climb_rate_cms * dt;
}
}
@ -701,8 +701,8 @@ void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt)
// avoid divide by zero
if (kP <= 0.0f) {
_vel_target.x = 0.0;
_vel_target.y = 0.0;
_vel_target.x = 0.0f;
_vel_target.y = 0.0f;
}else{
// calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
@ -846,8 +846,8 @@ void AC_PosControl::accel_to_lean_angles()
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));
accel_y_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.sin_yaw() * _ahrs.sin_pitch() / max(_ahrs.cos_pitch(),0.5)) + _ahrs.cos_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.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / max(_ahrs.cos_roll(),0.5f));
accel_y_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.sin_yaw() * _ahrs.sin_pitch() / max(_ahrs.cos_pitch(),0.5f)) + _ahrs.cos_yaw() * _ahrs.sin_roll() / max(_ahrs.cos_roll(),0.5f));
}
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain