Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttControl: use trig values from ahrs

Browse Source
This commit is contained in:
Randy Mackay 2014-02-09 12:35:08 +09:00 committed by Andrew Tridgell
parent 8b8d6a8e01
commit d76180d605
2 changed files with 5 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -352,9 +352,9 @@ void AC_AttitudeControl::rate_stab_bf_to_rate_bf_yaw()
void AC_AttitudeControl::rate_ef_targets_to_bf(const Vector3f& rate_ef_target, Vector3f& rate_bf_target)
{
// convert earth frame rates to body frame rates
rate_bf_target.x = rate_ef_target.x - _sin_pitch * rate_ef_target.z;
rate_bf_target.y = _cos_roll * rate_ef_target.y + _sin_roll * _cos_pitch * rate_ef_target.z;
rate_bf_target.z = _cos_pitch * _cos_roll * rate_ef_target.z - _sin_roll * rate_ef_target.y;
rate_bf_target.x = rate_ef_target.x - _ahrs.sin_pitch() * rate_ef_target.z;
rate_bf_target.y = _ahrs.cos_roll() * rate_ef_target.y + _ahrs.sin_roll() * _ahrs.cos_pitch() * rate_ef_target.z;
rate_bf_target.z = _ahrs.cos_pitch() * _ahrs.cos_roll() * rate_ef_target.z - _ahrs.sin_roll() * rate_ef_target.y;
}
//
@ -505,7 +505,7 @@ void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle
// throttle value should be 0 ~ 1000
int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm)
{
float temp = _cos_pitch * _cos_roll;
float temp = _ahrs.cos_pitch() * _ahrs.cos_roll();
int16_t throttle_out;
temp = constrain_float(temp, 0.5f, 1.0f);

22
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -59,11 +59,7 @@ public:
_motor_yaw(motor_yaw),
_motor_throttle(motor_throttle),
_dt(AC_ATTITUDE_100HZ_DT),
_angle_boost(0),
_cos_roll(1.0),
_cos_pitch(1.0),
_sin_roll(0.0),
_sin_pitch(0.0)
_angle_boost(0)
{
AP_Param::setup_object_defaults(this, var_info);
@ -191,15 +187,6 @@ public:
// helper functions
//
/// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame
/// To-Do: make these references or calculate in ahrs
void set_cos_sin_yaw(float cos_roll, float cos_pitch, float sin_roll, float sin_pitch) {
_cos_roll = cos_roll;
_cos_pitch = cos_pitch;
_sin_roll = sin_roll;
_sin_pitch = sin_pitch;
}
// lean_angle_max - maximum lean angle of the copter in centi-degrees
int16_t lean_angle_max() { return _aparm.angle_max; }
@ -278,13 +265,6 @@ protected:
Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors
Vector3f _rate_bf_target; // rate controller body-frame targets
int16_t _angle_boost; // used only for logging
// precalculated values for efficiency saves
// To-Do: could these be changed to references and passed into the constructor?
float _cos_roll;
float _cos_pitch;
float _sin_roll;
float _sin_pitch;
};
#define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \