get_stabilize_pitch(nav_pitch); break;
case ROLL_PITCH_SPORT:
// apply SIMPLE mode transform update_simple_mode();
// copy user input for reporting purposes control_roll = g.rc_1.control_in; control_pitch = g.rc_2.control_in;
get_roll_rate_stabilized_ef(g.rc_1.control_in); get_pitch_rate_stabilized_ef(g.rc_2.control_in); break;
#if AUTOTUNE == ENABLED
case ROLL_PITCH_AUTOTUNE:
// apply SIMPLE mode transform
if(ap.simple_mode && ap.new_radio_frame) { update_simple_mode(); }
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// pass desired roll, pitch to stabilize attitude controllers get_stabilize_roll(control_roll); get_stabilize_pitch(control_pitch);
// copy user input for reporting purposes
get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in); break; #endif } #if FRAME_CONFIG != HELI_FRAME
if(g.rc_3.control_in == 0 && control_mode <= ACRO) { reset_rate_I(); } #endif //HELI_FRAME
if(ap.new_radio_frame) {
// clear new radio frame info ap.new_radio_frame = false; } }
static void
init_simple_bearing() {
// capture current cos_yaw and sin_yaw values simple_cos_yaw = cos_yaw; simple_sin_yaw = sin_yaw;
// initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading super_simple_last_bearing = wrap_360_cd(ahrs.yaw_se