diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 50745f83f6..eb41c96beb 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -532,6 +532,9 @@ public: // dual motor tailsitter rudder to differential thrust scaling: 0-100% AP_Int8 rudd_dt_gain; + // QACRO mode max yaw rate in deg/sec + AP_Int16 acro_yaw_rate; + // mask of channels to do manual pass-thru for AP_Int32 manual_rc_mask; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b22089b7c9..b38bac8c7c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -416,6 +416,33 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Increment: .1 // @User: Advanced AP_GROUPINFO("THROTTLE_EXPO", 10, QuadPlane, throttle_expo, 0.2), + + // @Param: ACRO_RLL_RATE + // @DisplayName: QACRO mode roll rate + // @Description: The maximum roll rate at full stick deflection in QACRO mode + // @Units: deg/s + // @Range: 10 500 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ACRO_RLL_RATE", 11, QuadPlane, acro_roll_rate, 360), + + // @Param: ACRO_PIT_RATE + // @DisplayName: QACRO mode pitch rate + // @Description: The maximum pitch rate at full stick deflection in QACRO mode + // @Units: deg/s + // @Range: 10 500 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ACRO_PIT_RATE", 12, QuadPlane, acro_pitch_rate, 180), + + // @Param: ACRO_YAW_RATE + // @DisplayName: QACRO mode yaw rate + // @Description: The maximum yaw rate at full stick deflection in QACRO mode + // @Units: deg/s + // @Range: 10 500 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ACRO_YAW_RATE", 13, QuadPlane, acro_yaw_rate, 90), AP_GROUPEND }; @@ -913,8 +940,11 @@ void QuadPlane::hold_hover(float target_climb_rate) float QuadPlane::get_pilot_throttle() { - // get normalized throttle [0,1] - float throttle_in = (float)plane.channel_throttle->get_control_in() / plane.channel_throttle->get_range(); + // get scaled throttle input + float throttle_in = plane.channel_throttle->get_control_in(); + + // normalize to [0,1] + throttle_in /= plane.channel_throttle->get_range(); // get hover throttle level [0,1] float thr_mid = motors->get_throttle_hover(); @@ -940,17 +970,16 @@ void QuadPlane::control_qacro(void) // convert the input to the desired body frame rate float target_roll = 0; - float target_pitch = plane.channel_pitch->norm_input() * plane.g.acro_pitch_rate * 100.0f; + float target_pitch = plane.channel_pitch->norm_input() * acro_pitch_rate * 100.0f; float target_yaw = 0; if (is_tailsitter()) { // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw // acro_roll_rate param applies to yaw in copter frame - // no parameter for acro yaw rate; just use the normal one since the default is 100 deg/sec - target_roll = plane.channel_rudder->norm_input() * yaw_rate_max * 100.0f; - target_yaw = -plane.channel_roll->norm_input() * plane.g.acro_roll_rate * 100.0f; + target_roll = plane.channel_rudder->norm_input() * acro_roll_rate * 100.0f; + target_yaw = -plane.channel_roll->norm_input() * acro_yaw_rate * 100.0f; } else { - target_roll = plane.channel_roll->norm_input() * plane.g.acro_roll_rate * 100.0f; - target_yaw = plane.channel_rudder->norm_input() * yaw_rate_max * 100.0; + target_roll = plane.channel_roll->norm_input() * acro_roll_rate * 100.0f; + target_yaw = plane.channel_rudder->norm_input() * acro_yaw_rate * 100.0; } float throttle_out = get_pilot_throttle(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 52628be8ea..a32a65c77b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -314,6 +314,11 @@ private: // manual throttle curve expo strength AP_Float throttle_expo; + // QACRO mode max roll/pitch/yaw rates + AP_Float acro_roll_rate; + AP_Float acro_pitch_rate; + AP_Float acro_yaw_rate; + // time we last got an EKF yaw reset uint32_t ekfYawReset_ms;