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

Copter: add acro throttle and yaw expo and smoother manual pilot throttle

This commit is contained in:
Leonard Hall 2016-06-22 13:46:28 +09:30 committed by Randy Mackay
parent 041ec327a2
commit 5c47f3f9e5
10 changed files with 83 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduCopter/Attitude.cpp
View File

@ -121,15 +121,17 @@ void Copter::set_throttle_takeoff()
pos_control.init_takeoff();
}
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// transform pilot's manual throttle input to make hover throttle mid stick
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float Copter::get_pilot_desired_throttle(int16_t throttle_control)
float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
{
float throttle_out;
if (thr_mid <= 0.0f) {
thr_mid = motors.get_throttle_hover();
}
int16_t mid_stick = channel_throttle->get_control_mid();
// protect against unlikely divide by zero
if (mid_stick <= 0) {
mid_stick = 500;
@ -138,21 +140,22 @@ float Copter::get_pilot_desired_throttle(int16_t throttle_control)
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
// ensure mid throttle is set within a reasonable range
float thr_mid = constrain_float(motors.get_throttle_hover(), 0.1f, 0.9f);
// check throttle is above, below or in the deadband
// calculate normalised throttle input
float throttle_in;
if (throttle_control < mid_stick) {
// below the deadband
throttle_out = ((float)throttle_control)*thr_mid/(float)mid_stick;
throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
}else if(throttle_control > mid_stick) {
// above the deadband
throttle_out = (thr_mid) + ((float)(throttle_control-mid_stick)) * (1.0f - thr_mid) / (float)(1000-mid_stick);
throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
}else{
// must be in the deadband
throttle_out = thr_mid;
throttle_in = 0.5f;
}
float expo = constrain_float(-(thr_mid-0.5)/0.375, -0.5f, 1.0f);
// calculate the output throttle using the given expo function
float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
return throttle_out;
}

2
ArduCopter/Copter.h
View File

@ -685,7 +685,7 @@ private:
float get_look_ahead_yaw();
void update_throttle_hover();
void set_throttle_takeoff();
float get_pilot_desired_throttle(int16_t throttle_control);
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid);
float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle();
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);

23
ArduCopter/Parameters.cpp
View File

@ -611,12 +611,13 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Advanced
GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED),
// @Param: ACRO_EXPO
// @DisplayName: Acro Expo
// @Param: ACRO_RP_EXPO
// @DisplayName: Acro Roll/Pitch Expo
// @Description: Acro roll/pitch 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
GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT),
GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_EXPO_DEFAULT),
// @Param: VEL_XY_P
// @DisplayName: Velocity (horizontal) P gain
@ -1014,6 +1015,22 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(proximity, "PRX_", 8, ParametersG2, AP_Proximity),
#endif
// @Param: ACRO_Y_EXPO
// @DisplayName: Acro Yaw Expo
// @Description: Acro 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
AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT),
// @Param: ACRO_THR_MID
// @DisplayName: Acro Thr Mid
// @Description: Acro Throttle Mid
// @Range: 0 1
// @User: Advanced
AP_GROUPINFO("ACRO_THR_MID", 10, ParametersG2, acro_thr_mid, ACRO_THR_MID_DEFAULT),
AP_GROUPEND
};

8
ArduCopter/Parameters.h
View File

@ -126,7 +126,7 @@ public:
k_param_rangefinder, // rangefinder object
k_param_fs_ekf_thresh,
k_param_terrain,
k_param_acro_expo,
k_param_acro_rp_expo,
k_param_throttle_deadzone,
k_param_optflow,
k_param_dcmcheck_thresh, // deprecated - remove
@ -484,7 +484,7 @@ public:
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_expo;
AP_Float acro_rp_expo;
// PI/D controllers
AC_PI_2D pi_vel_xy;
@ -570,6 +570,10 @@ public:
// developer options
AP_Int32 dev_options;
// acro exponent parameters
AP_Float acro_y_expo;
AP_Float acro_thr_mid;
};
extern const AP_Param::Info var_info[];

12
ArduCopter/config.h
View File

@ -443,8 +443,16 @@
#define ACRO_BALANCE_PITCH 1.0f
#endif
#ifndef ACRO_EXPO_DEFAULT
#define ACRO_EXPO_DEFAULT 0.3f
#ifndef ACRO_RP_EXPO_DEFAULT
#define ACRO_RP_EXPO_DEFAULT 0.3f
#endif
#ifndef ACRO_Y_EXPO_DEFAULT
#define ACRO_Y_EXPO_DEFAULT 0.3f
#endif
#ifndef ACRO_THR_MID_DEFAULT
#define ACRO_THR_MID_DEFAULT 0.0f
#endif
// RTL Mode

34
ArduCopter/control_acro.cpp
View File

@ -10,7 +10,8 @@
bool Copter::acro_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
(get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid) > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
@ -42,7 +43,7 @@ void Copter::acro_run()
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid);
// run attitude controller
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
@ -69,7 +70,7 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
}
// calculate roll, pitch rate requests
if (g.acro_expo <= 0) {
if (g.acro_rp_expo <= 0) {
rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p;
} else {
@ -77,25 +78,42 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
float rp_in, rp_in3, rp_out;
// range check expo
if (g.acro_expo > 1.0f) {
g.acro_expo = 1.0f;
if (g.acro_rp_expo > 1.0f) {
g.acro_rp_expo = 1.0f;
}
// 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);
rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_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);
rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_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;
if (g.acro_rp_expo <= 0) {
rate_bf_request.z = yaw_in * g.acro_yaw_p;
} else {
// expo variables
float y_in, y_in3, y_out;
// range check expo
if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
g2.acro_y_expo = 1.0f;
}
// yaw expo
y_in = float(yaw_in)/ROLL_PITCH_INPUT_MAX;
y_in3 = y_in*y_in*y_in;
y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
rate_bf_request.z = ROLL_PITCH_INPUT_MAX * y_out * g.acro_yaw_p;
}
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode

2
ArduCopter/control_drift.cpp
View File

@ -64,7 +64,7 @@ void Copter::drift_run()
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
// Grab inertial velocity
const Vector3f& vel = inertial_nav.get_velocity();

2
ArduCopter/control_flip.cpp
View File

@ -105,7 +105,7 @@ void Copter::flip_run()
}
// get pilot's desired throttle
throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in());
throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
// get corrected angle based on direction and axis of rotation
// we flip the sign of flip_angle to minimize the code repetition

5
ArduCopter/control_stabilize.cpp
View File

@ -10,7 +10,8 @@
bool Copter::stabilize_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
(get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f) > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
@ -50,7 +51,7 @@ void Copter::stabilize_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

2
ArduCopter/flight_mode.cpp
View File

@ -274,7 +274,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in()));
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f));
}
// cancel any takeoffs in progress