diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 818371f1f1..675e96fc28 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -236,22 +236,34 @@ static void stabilize_training(float speed_scaler) */ static void stabilize_acro(float speed_scaler) { - float roll_rate = channel_roll->norm_input() * 180; - float pitch_rate = channel_pitch->norm_input() * 180; + float roll_rate = channel_roll->norm_input() * g.acro_roll_rate; + float pitch_rate = channel_pitch->norm_input() * g.acro_pitch_rate; + /* + check for special roll handling near the pitch poles + */ if (roll_rate == 0 && acro_state.locked_roll && (ahrs.pitch_sensor > 7000 || ahrs.pitch_sensor < -7000)) { // when near the poles do rate holding, but don't unlock the - // desired roll + // desired roll. channel_roll->servo_out = g.rollController.get_rate_out(roll_rate, speed_scaler); } else if (roll_rate == 0) { + /* + we have no roll stick input, so we will enter "roll locked" + mode, and hold the roll we had when the stick was released + */ if (!acro_state.locked_roll) { acro_state.locked_roll = true; acro_state.locked_roll_cd = ahrs.roll_sensor; } - // try to cope with wrap while looping. + /* + special handling for the roll inversion while + looping. Using euler angles for this is ugly, but it fits + with the rest of the APM code, and actually works + surprisingly well + */ int32_t roll_error_cd = ahrs.roll_sensor - acro_state.locked_roll_cd; if (roll_error_cd > 13500 && roll_error_cd < 21500) { acro_state.locked_roll_cd += 18000; @@ -261,6 +273,11 @@ static void stabilize_acro(float speed_scaler) } acro_state.locked_roll_cd = wrap_180_cd(acro_state.locked_roll_cd); nav_roll_cd = acro_state.locked_roll_cd; + + /* + now we need to prevent the roll controller from seeing a + jump as roll passes through the -180 -> 180 point + */ roll_error_cd = ahrs.roll_sensor - nav_roll_cd; if (roll_error_cd > 31500) { nav_roll_cd += 36000; @@ -269,11 +286,19 @@ static void stabilize_acro(float speed_scaler) } stabilize_roll(speed_scaler); } else { + /* + aileron stick is non-zero, use pure rate control until the + user releases the stick + */ acro_state.locked_roll = false; channel_roll->servo_out = g.rollController.get_rate_out(roll_rate, speed_scaler); } if (pitch_rate == 0) { + /* + user has zero pitch stick input, so we lock pitch at the + point they release the stick + */ if (!acro_state.locked_pitch) { acro_state.locked_pitch = true; acro_state.locked_pitch_cd = ahrs.pitch_sensor; @@ -281,10 +306,18 @@ static void stabilize_acro(float speed_scaler) nav_pitch_cd = acro_state.locked_pitch_cd; stabilize_pitch(speed_scaler); } else { + /* + user has non-zero pitch input, use a pure rate controller + */ acro_state.locked_pitch = false; channel_pitch->servo_out = g.pitchController.get_rate_out(pitch_rate, speed_scaler); } + /* + call the normal yaw stabilize for now. This allows for manual + rudder input, plus automatic coordinated turn handling. For + knife-edge we'll need to do something quite different + */ stabilize_yaw(speed_scaler); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index bdd5698067..2cfd62f926 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -110,6 +110,8 @@ public: k_param_flybywire_elev_reverse, k_param_alt_control_algorithm, k_param_flybywire_climb_rate, + k_param_acro_roll_rate, + k_param_acro_pitch_rate, // // 130: Sensor parameters @@ -329,6 +331,8 @@ public: // AP_Int16 roll_limit_cd; AP_Int16 alt_offset; + AP_Int16 acro_roll_rate; + AP_Int16 acro_pitch_rate; // Misc // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 1bc613af38..daaf9a389c 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -465,6 +465,24 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard ASCALAR(pitch_limit_min_cd, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE), + // @Param: ACRO_ROLL_RATE + // @DisplayName: ACRO mode roll rate + // @Description: The maximum roll rate at full stick deflection in ACRO mode + // @Units: degrees/second + // @Range: 10 500 + // @Increment: 1 + // @User: Standard + GSCALAR(acro_roll_rate, "ACRO_ROLL_RATE", 180), + + // @Param: ACRO_PITCH_RATE + // @DisplayName: ACRO mode pitch rate + // @Description: The maximum pitch rate at full stick deflection in ACRO mode + // @Units: degrees/second + // @Range: 10 500 + // @Increment: 1 + // @User: Standard + GSCALAR(acro_pitch_rate, "ACRO_PITCH_RATE", 180), + // @Param: AUTO_TRIM // @DisplayName: Auto trim // @Description: Set RC trim PWM levels to current levels when switching away from manual mode