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

Plane: add QACRO roll/pitch/yaw rate params

This commit is contained in:
Mark Whitehorn 2019-03-23 21:35:13 -06:00 committed by Andrew Tridgell
parent 8bcf2439ad
commit f054301ec2
3 changed files with 45 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/Parameters.h
View File

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

45
ArduPlane/quadplane.cpp
View File

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

5
ArduPlane/quadplane.h
View File

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