Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: use trig values from ahrs

Browse Source
This commit is contained in:
Randy Mackay 2014-02-09 12:34:59 +09:00 committed by Andrew Tridgell
parent 598a1b1f43
commit 8b8d6a8e01
2 changed files with 3 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -46,9 +46,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_accel_z_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default
_accel_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default
_leash(POSCONTROL_LEASH_LENGTH_MIN),
_cos_yaw(1.0),
_sin_yaw(0.0),
_cos_pitch(1.0),
_roll_target(0.0),
_pitch_target(0.0),
_vel_target_filt_z(0),
@ -639,11 +636,11 @@ void AC_PosControl::accel_to_lean_angles()
// To-Do: add 1hz filter to accel_lat, accel_lon
// rotate accelerations into body forward-right frame
accel_forward = _accel_target.x*_cos_yaw + _accel_target.y*_sin_yaw;
accel_right = -_accel_target.x*_sin_yaw + _accel_target.y*_cos_yaw;
accel_forward = _accel_target.x*_ahrs.cos_yaw() + _accel_target.y*_ahrs.sin_yaw();
accel_right = -_accel_target.x*_ahrs.sin_yaw() + _accel_target.y*_ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller
_roll_target = constrain_float(fast_atan(accel_right*_cos_pitch/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
_roll_target = constrain_float(fast_atan(accel_right*_ahrs.cos_pitch()/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
_pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
}

7
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -186,13 +186,6 @@ public:
const Vector3f get_vel_target() { return _vel_target; }
const Vector3f get_accel_target() { return _accel_target; }
/// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame
void set_cos_sin_yaw(float cos_yaw, float sin_yaw, float cos_pitch) {
_cos_yaw = cos_yaw;
_sin_yaw = sin_yaw;
_cos_pitch = cos_pitch;
}
static const struct AP_Param::GroupInfo var_info[];
private: