diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 0f76a86fad..44059ddb6a 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -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; } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6ccb0c879d..0c99290b43 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a6d76b8bc4..541ecee8e1 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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 }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 968f413829..6aa0f91cae 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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[]; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a3cf384a30..a336b8a39d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 87130f2a68..40b90913f8 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -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 diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index e70fefc533..6019af4653 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -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(); diff --git a/ArduCopter/control_flip.cpp b/ArduCopter/control_flip.cpp index 0f4dd68dd6..9c7a78ca71 100644 --- a/ArduCopter/control_flip.cpp +++ b/ArduCopter/control_flip.cpp @@ -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 diff --git a/ArduCopter/control_stabilize.cpp b/ArduCopter/control_stabilize.cpp index 7033cad67f..6e4a863ae8 100644 --- a/ArduCopter/control_stabilize.cpp +++ b/ArduCopter/control_stabilize.cpp @@ -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()); diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index ef69f45beb..7684c5d51c 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -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