|
|
|
@ -40,8 +40,8 @@ static bool drift_init(bool ignore_checks)
@@ -40,8 +40,8 @@ static bool drift_init(bool ignore_checks)
|
|
|
|
|
// should be called at 100hz or more |
|
|
|
|
static void drift_run() |
|
|
|
|
{ |
|
|
|
|
static float breaker = 0.0; |
|
|
|
|
static float roll_input = 0.0; |
|
|
|
|
static float breaker = 0.0f; |
|
|
|
|
static float roll_input = 0.0f; |
|
|
|
|
float target_roll, target_pitch; |
|
|
|
|
float target_yaw_rate; |
|
|
|
|
int16_t pilot_throttle_scaled; |
|
|
|
@ -72,7 +72,7 @@ static void drift_run()
@@ -72,7 +72,7 @@ static void drift_run()
|
|
|
|
|
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); |
|
|
|
|
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); |
|
|
|
|
|
|
|
|
|
roll_input = roll_input * .96 + (float)g.rc_4.control_in * .04; |
|
|
|
|
roll_input = roll_input * .96f + (float)g.rc_4.control_in * .04f; |
|
|
|
|
|
|
|
|
|
//convert user input into desired roll velocity |
|
|
|
|
float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); |
|
|
|
@ -84,11 +84,11 @@ static void drift_run()
@@ -84,11 +84,11 @@ static void drift_run()
|
|
|
|
|
// If we let go of sticks, bring us to a stop |
|
|
|
|
if(target_pitch == 0){ |
|
|
|
|
// .14/ (.03 * 100) = 4.6 seconds till full breaking |
|
|
|
|
breaker += .03; |
|
|
|
|
breaker += .03f; |
|
|
|
|
breaker = min(breaker, DRIFT_SPEEDGAIN); |
|
|
|
|
target_pitch = pitch_vel * breaker; |
|
|
|
|
}else{ |
|
|
|
|
breaker = 0.0; |
|
|
|
|
breaker = 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// call attitude controller |
|
|
|
@ -108,7 +108,7 @@ int16_t get_throttle_assist(float velz, int16_t pilot_throttle_scaled)
@@ -108,7 +108,7 @@ int16_t get_throttle_assist(float velz, int16_t pilot_throttle_scaled)
|
|
|
|
|
if (pilot_throttle_scaled > g.throttle_min && pilot_throttle_scaled < THR_MAX && |
|
|
|
|
pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) { |
|
|
|
|
// calculate throttle assist gain |
|
|
|
|
thr_assist = 1.2 - ((float)abs(pilot_throttle_scaled - 500) / 240.0f); |
|
|
|
|
thr_assist = 1.2f - ((float)abs(pilot_throttle_scaled - 500) / 240.0f); |
|
|
|
|
thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * -DRIFT_THR_ASSIST_GAIN * velz; |
|
|
|
|
|
|
|
|
|
// ensure throttle assist never adjusts the throttle by more than 300 pwm |
|
|
|
|