3 #if MODE_DRIFT_ENABLED == ENABLED 9 #ifndef DRIFT_SPEEDGAIN 10 # define DRIFT_SPEEDGAIN 8.0f 12 #ifndef DRIFT_SPEEDLIMIT 13 # define DRIFT_SPEEDLIMIT 560.0f 16 #ifndef DRIFT_THR_ASSIST_GAIN 17 # define DRIFT_THR_ASSIST_GAIN 0.0018f // gain controlling amount of throttle assistance 20 #ifndef DRIFT_THR_ASSIST_MAX 21 # define DRIFT_THR_ASSIST_MAX 0.3f // maximum assistance throttle assist will provide 25 # define DRIFT_THR_MIN 0.213f // throttle assist will be active when pilot's throttle is above this value 28 # define DRIFT_THR_MAX 0.787f // throttle assist will be active when pilot's throttle is below this value 34 if (
copter.position_ok() || ignore_checks) {
45 static float braker = 0.0f;
46 static float roll_input = 0.0f;
47 float target_roll, target_pitch;
48 float target_yaw_rate;
49 float pilot_throttle_scaled;
52 if (!
motors->armed() || !
motors->get_interlock() || (
ap.land_complete &&
ap.throttle_zero)) {
58 if (!
ap.throttle_zero) {
76 float pitch_vel2 =
MIN(fabsf(pitch_vel), 2000);
77 target_yaw_rate = ((float)target_roll/1.0
f) * (1.0f - (pitch_vel2 / 5000.0f)) *
g.
acro_yaw_p;
95 braker =
MIN(braker, DRIFT_SPEEDGAIN);
96 target_pitch = pitch_vel * braker;
105 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
117 float thr_assist = 0.0f;
120 thr_assist = 1.2f - ((float)fabsf(pilot_throttle_scaled - 0.5
f) / 0.24f);
void zero_throttle_and_relax_ac()
float get_throttle_assist(float velz, float pilot_throttle_scaled)
AC_AttitudeControl_t *& attitude_control
int16_t get_control_in() const
bool is_zero(const T fVal1)
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid=0.0f)
DESIRED_THROTTLE_UNLIMITED
#define DRIFT_THR_ASSIST_GAIN
float constrain_float(const float amt, const float low, const float high)
#define DRIFT_THR_ASSIST_MAX
AP_HAL_MAIN_CALLBACKS & copter
AP_InertialNav & inertial_nav
virtual const Vector3f & get_velocity() const=0
bool init(bool ignore_checks) override
RC_Channel *& channel_yaw
void set_land_complete(bool b)
RC_Channel *& channel_throttle