|
|
|
@ -52,29 +52,29 @@ void Copter::ModeDrift::run()
@@ -52,29 +52,29 @@ 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; |
|
|
|
|
|
|
|
|
|
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); |
|
|
|
|
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
roll_input = roll_input * .96f + (float)channel_yaw->get_control_in() * .04f; |
|
|
|
|
|
|
|
|
|
//convert user input into desired roll velocity
|
|
|
|
|
// 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); |
|
|
|
|
|
|
|
|
|
// If we let go of sticks, bring us to a stop
|
|
|
|
|
if(is_zero(target_pitch)){ |
|
|
|
|
if (is_zero(target_pitch)) { |
|
|
|
|
// .14/ (.03 * 100) = 4.6 seconds till full braking
|
|
|
|
|
braker += .03f; |
|
|
|
|
braker = MIN(braker, DRIFT_SPEEDGAIN); |
|
|
|
|
target_pitch = pitch_vel * braker; |
|
|
|
|
}else{ |
|
|
|
|
} else { |
|
|
|
|
braker = 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|