Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Quadplane: add yaw command model

This commit is contained in:
Iampete1 2022-07-03 23:39:05 +01:00 committed by Andrew Tridgell
parent 4de69823e8
commit 712611c50e
6 changed files with 74 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/mode_qacro.cpp
View File

@ -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;
}

3
ArduPlane/mode_qloiter.cpp
View File

@ -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,

1
ArduPlane/mode_qrtl.cpp
View File

@ -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());

61
ArduPlane/quadplane.cpp
View File

@ -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

11
ArduPlane/quadplane.h
View File

@ -11,6 +11,7 @@
#include <AP_Motors/AP_Motors.h>
#include <AC_PID/AC_PID.h>
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
#include <AC_AttitudeControl/AC_CommandModel.h>
#include <AP_InertialNav/AP_InertialNav.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AC_AttitudeControl/AC_WeatherVane.h>
@ -196,6 +197,13 @@ private:
// 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 @@ private:
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;

1
ArduPlane/tailsitter.cpp
View File

@ -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);