|
|
|
@ -52,7 +52,7 @@ void Copter::ModeDrift::run()
@@ -52,7 +52,7 @@ void Copter::ModeDrift::run()
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
// gain sceduling for Yaw
|
|
|
|
|
// gain scheduling for yaw
|
|
|
|
|
float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); |
|
|
|
|
float target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; |
|
|
|
|
|
|
|
|
@ -64,7 +64,7 @@ void Copter::ModeDrift::run()
@@ -64,7 +64,7 @@ void Copter::ModeDrift::run()
|
|
|
|
|
// convert user input into desired roll velocity
|
|
|
|
|
float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); |
|
|
|
|
|
|
|
|
|
// Roll velocity is feed into roll acceleration to minimize slip
|
|
|
|
|
// roll velocity is feed into roll acceleration to minimize slip
|
|
|
|
|
target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; |
|
|
|
|
target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); |
|
|
|
|
|
|
|
|
|