aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add ACRO_EXPO parameter

Browse Source
This commit is contained in:
Randy Mackay 2014-08-13 11:25:59 +09:00
parent 58257e3858
commit f3fd79597f
4 changed files with 35 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Parameters.h
View File

@ -117,8 +117,9 @@ public:
k_param_serial2_baud,
k_param_land_repositioning,
k_param_sonar, // sonar object
k_param_ekfcheck_thresh, // 54
k_param_ekfcheck_thresh,
k_param_terrain,
k_param_acro_expo, // 56
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
@ -434,6 +435,7 @@ public:
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_expo;
// PI/D controllers
#if FRAME_CONFIG == HELI_FRAME

7
ArduCopter/Parameters.pde
View File

@ -586,6 +586,13 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED),
// @Param: ACRO_EXPO
// @DisplayName: Acro Expo
// @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges
// @Values: 0:Disabled,0.2:Low,0.3:Medium,0.4:High
// @User: Advanced
GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT),
// PID controller
//---------------

4
ArduCopter/config.h
View File

@ -492,6 +492,10 @@
#define ACRO_BALANCE_PITCH 1.0f
#endif
#ifndef ACRO_EXPO_DEFAULT
#define ACRO_EXPO_DEFAULT 0.3f
#endif
// Stabilize (angle controller) gains
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5f

35
ArduCopter/control_acro.pde
View File

@ -44,10 +44,6 @@ static void acro_run()
// returns desired angle rates in centi-degrees-per-second
static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
// expo variables
static float _expo_factor = 0.3;
float rp_in, rp_in3, rp_out;
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
@ -55,17 +51,28 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
// calculate rate requests
rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in);
rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
// calculate roll, pitch rate requests
if (g.acro_expo <= 0) {
rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p;
} else {
// expo variables
float rp_in, rp_in3, rp_out;
rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in);
rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
// roll expo
rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
// pitch expo
rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
}
// calculate yaw rate request
rate_bf_request.z = yaw_in * g.acro_yaw_p;
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode