5 #if MODE_ACRO_ENABLED == ENABLED 14 if (
motors->armed() &&
ap.land_complete && !
copter.flightmode->has_manual_throttle() &&
26 float target_roll, target_pitch, target_yaw;
27 float pilot_throttle_scaled;
30 if (!
motors->armed() ||
ap.throttle_zero || !
motors->get_interlock()) {
47 attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
59 Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
64 float total_in =
norm(pitch_in, roll_in);
78 float rp_in, rp_in3, rp_out;
87 rp_in3 = rp_in*rp_in*rp_in;
93 rp_in3 = rp_in*rp_in*rp_in;
123 }
else if (roll_angle < -aparm.
angle_max) {
129 }
else if (pitch_angle < -aparm.
angle_max) {
139 rate_bf_request.
x += rate_bf_level.
x;
140 rate_bf_request.
y += rate_bf_level.
y;
141 rate_bf_request.
z += rate_bf_level.
z;
146 rate_bf_level = rate_bf_level*acro_level_mix;
149 rate_limit = fabsf(fabsf(rate_bf_request.
x)-fabsf(rate_bf_level.
x));
150 rate_bf_request.
x += rate_bf_level.
x;
154 rate_limit = fabsf(fabsf(rate_bf_request.
y)-fabsf(rate_bf_level.
y));
155 rate_bf_request.
y += rate_bf_level.
y;
159 rate_limit = fabsf(fabsf(rate_bf_request.
z)-fabsf(rate_bf_level.
z));
160 rate_bf_request.
z += rate_bf_level.
z;
166 roll_out = rate_bf_request.
x;
167 pitch_out = rate_bf_request.
y;
168 yaw_out = rate_bf_request.
z;
float norm(const T first, const U second, const Params... parameters)
void zero_throttle_and_relax_ac()
virtual bool init(bool ignore_checks) override
AP_Float acro_balance_roll
AC_AttitudeControl_t *& attitude_control
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
float get_pilot_desired_yaw_rate(int16_t stick_angle)
#define ACRO_TRAINER_DISABLED
void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
int16_t get_control_in() const
int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid=0.0f)
DESIRED_THROTTLE_UNLIMITED
virtual void run() override
RC_Channel *& channel_roll
AC_PosControl *& pos_control
float constrain_float(const float amt, const float low, const float high)
void set_alt_target(float alt_cm)
#define ROLL_PITCH_YAW_INPUT_MAX
AP_HAL_MAIN_CALLBACKS & copter
#define ACRO_TRAINER_LIMITED
AP_Float acro_balance_pitch
#define ACRO_LEVEL_MAX_ANGLE
RC_Channel *& channel_yaw
void set_land_complete(bool b)
AP_Vehicle::MultiCopter aparm
RC_Channel *& channel_throttle
RC_Channel *& channel_pitch