Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: control_acro, drift use trig from ahrs

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

2
ArduCopter/control_acro.pde
View File

@ -94,7 +94,7 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
}else{
acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*cos_pitch_x;
acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix;

4
ArduCopter/control_drift.pde
View File

@ -40,8 +40,8 @@ static void drift_run()
Vector3f vel = inertial_nav.get_velocity();
// rotate roll, pitch input from north facing to vehicle's perspective
float roll_vel = vel.y * cos_yaw - vel.x * sin_yaw; // body roll vel
float pitch_vel = vel.y * sin_yaw + vel.x * cos_yaw; // body pitch vel
float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
float pitch_vel2 = min(fabs(pitch_vel), 800);