diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index d0b38c7661652..79d8ed69ebc88 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -8,6 +8,10 @@ bool ModeQAcro::_enter() quadplane.throttle_wait = false; quadplane.transition->force_transition_complete(); attitude_control->relax_attitude_controllers(); + + // disable yaw rate time contant to mantain old behaviour + quadplane.disable_yaw_rate_time_constant(); + return true; } diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 836143192eec8..0a7a1804e8a54 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -77,6 +77,9 @@ void ModeQLoiter::run() pos_control->set_externally_limited_xy(); } + // Pilot input, use yaw rate time constant + quadplane.set_pilot_yaw_rate_time_constant(); + // call attitude controller with conservative smoothing gain of 4.0f attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 0c4986066a11f..38d549ee19ad5 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -82,6 +82,7 @@ void ModeQRTL::run() pos_control->set_externally_limited_xy(); } // weathervane with no pilot input + quadplane.disable_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, quadplane.get_weathervane_yaw_rate_cds()); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 5bf0afe54f88b..55f9617cada07 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -117,7 +117,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 50 500 // @Increment: 1 // @User: Standard - AP_GROUPINFO("YAW_RATE_MAX", 25, QuadPlane, yaw_rate_max, 100), + + // YAW_RATE_MAX index 25 // @Param: LAND_SPEED // @DisplayName: Land speed @@ -449,7 +450,31 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Range: 0 5 // @User: Advanced AP_GROUPINFO("NAVALT_MIN", 32, QuadPlane, takeoff_navalt_min, 0), - + + // @Param: PLT_Y_RATE + // @DisplayName: Pilot controlled yaw rate + // @Description: Pilot controlled yaw rate max. Used in all pilot controlled modes except QAcro + // @Units: deg/s + // @Range: 1 360 + // @User: Standard + + // @Param: PLT_Y_EXPO + // @DisplayName: Pilot controlled yaw expo + // @Description: Pilot controlled yaw expo to allow faster rotation when stick at edges + // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High + // @Range: -0.5 1.0 + // @User: Advanced + + // @Param: PLT_Y_RATE_TC + // @DisplayName: Pilot yaw rate control input time constant + // @Description: Pilot yaw rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response. + // @Units: s + // @Range: 0 1 + // @Increment: 0.01 + // @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp + // @User: Standard + AP_SUBGROUPINFO(command_model_pilot, "PLT_Y_", 33, QuadPlane, AC_CommandModel), + AP_GROUPEND }; @@ -541,6 +566,9 @@ const AP_Param::ConversionInfo q_conversion_table[] = { // PARAMETER_CONVERSION - Added: Jan-2022 { Parameters::k_param_quadplane, 33, AP_PARAM_FLOAT, "Q_WVANE_GAIN" }, // Moved from quadplane to weathervane library { Parameters::k_param_quadplane, 34, AP_PARAM_FLOAT, "Q_WVANE_ANG_MIN" }, // Q_WVANE_MINROLL moved from quadplane to weathervane library + + // PARAMETER_CONVERSION - Added: July-2022 + { Parameters::k_param_quadplane, 25, AP_PARAM_FLOAT, "Q_PLT_Y_RATE" }, // Moved from quadplane to command model library }; // PARAMETER_CONVERSION - Added: Oct-2021 @@ -828,6 +856,9 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) // tailsitter in transition to VTOL flight is not really in a VTOL mode yet if (use_multicopter_control) { + // Pilot input, use yaw rate time constant + set_pilot_yaw_rate_time_constant(); + // tailsitter-only body-frame roll control options // Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw. if (tailsitter.enabled() && @@ -853,6 +884,7 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) roll_limit = tailsitter.max_roll_angle * 100.0f; } // Prevent a divide by zero + const float yaw_rate_max = command_model_pilot.get_rate(); float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f; float yaw2roll_scale = roll_limit / yaw_rate_limit; @@ -892,6 +924,9 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) // rotate into multicopter attitude refence frame ahrs_view->rotate(bf_input_cd); + // disable yaw time constant for 1:1 match of desired rates + disable_yaw_rate_time_constant(); + attitude_control->input_rate_bf_roll_pitch_yaw_2(bf_input_cd.x, bf_input_cd.y, bf_input_cd.z); } } @@ -1211,6 +1246,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const } // add in rudder input + const float yaw_rate_max = command_model_pilot.get_rate(); float max_rate = yaw_rate_max; if (!in_vtol_mode() && tailsitter.enabled()) { // scale by RUDD_DT_GAIN when not in a VTOL mode for @@ -1225,7 +1261,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const // must have a non-zero max yaw rate for scaling to work max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; } - return rudder_in * max_rate * (1/45.0); + return input_expo(rudder_in * (1/4500.0), command_model_pilot.get_expo()) * max_rate * 100.0; } /* @@ -2542,6 +2578,7 @@ void QuadPlane::vtol_position_controller(void) } // call attitude controller + disable_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); @@ -2582,6 +2619,7 @@ void QuadPlane::vtol_position_controller(void) } // call attitude controller + set_pilot_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); @@ -2621,6 +2659,7 @@ void QuadPlane::vtol_position_controller(void) plane.nav_pitch_cd = pos_control->get_pitch_cd(); // call attitude controller + set_pilot_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); @@ -2844,6 +2883,7 @@ void QuadPlane::takeoff_controller(void) run_xy_controller(); + set_pilot_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); @@ -2902,6 +2942,7 @@ void QuadPlane::waypoint_controller(void) } // call attitude controller + disable_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, wp_nav->get_yaw(), @@ -3459,7 +3500,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) pos_control->get_pitch_cd(), is_takeoff, in_vtol_land_sequence())) { - return constrain_float(wv_output * (1/45.0), -100.0, 100.0) * yaw_rate_max * 0.5; + return constrain_float(wv_output * (1/45.0), -100.0, 100.0) * command_model_pilot.get_rate() * 0.5; } return 0.0; @@ -4145,4 +4186,16 @@ void QuadPlane::mode_enter(void) guided_wait_takeoff = false; } +// Set attitude control yaw rate time constant to pilot input command model value +void QuadPlane::set_pilot_yaw_rate_time_constant() +{ + attitude_control->set_yaw_rate_tc(command_model_pilot.get_rate_tc()); +} + +// Disable attitude control yaw rate time constant +void QuadPlane::disable_yaw_rate_time_constant() +{ + attitude_control->set_yaw_rate_tc(0.0); +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 7444e773a003f..478975faaeeea 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -11,6 +11,7 @@ #include #include #include // Attitude control library +#include #include #include #include @@ -196,6 +197,13 @@ class QuadPlane // air mode state: OFF, ON, ASSISTED_FLIGHT_ONLY AirMode air_mode; + // Command model parameter class + // Default max rate, default expo, default time constant + AC_CommandModel command_model_pilot{100.0, 0.25, 0.25}; + // helper functions to set and disable time constant from command model + void set_pilot_yaw_rate_time_constant(); + void disable_yaw_rate_time_constant(); + // return true if airmode should be active bool air_mode_active() const; @@ -319,9 +327,6 @@ class QuadPlane uint32_t alt_error_start_ms; bool in_alt_assist; - // maximum yaw rate in degrees/second - AP_Float yaw_rate_max; - // landing speed in cm/s AP_Int16 land_speed_cms; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index a0b672cbebc81..87255240133e9 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -796,6 +796,7 @@ void Tailsitter_Transition::update() // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500); plane.nav_roll_cd = 0; + quadplane.disable_yaw_rate_time_constant(); quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, 0);