diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0fe42bf06a..3a53c8558c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -128,6 +128,7 @@ public: k_param_dcmcheck_thresh, // deprecated - remove k_param_log_bitmask, k_param_cli_enabled, // 61 + k_param_throttle_filt, // 62 // 65: AP_Limits Library k_param_limits = 65, // deprecated - remove @@ -266,7 +267,7 @@ public: k_param_rc_9, k_param_rc_12, k_param_failsafe_gcs, // 198 - k_param_rcmap, + k_param_rcmap, // 199 // // 200: flight modes @@ -339,6 +340,8 @@ public: AP_Int8 cli_enabled; #endif + AP_Float throttle_filt; + AP_Int16 rtl_altitude; AP_Float sonar_gain; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 14faa8a424..5dae748c1f 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -62,6 +62,15 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(cli_enabled, "CLI_ENABLED", 0), #endif + // @Param: PILOT_THR_FILT + // @DisplayName: Throttle filter cutoff + // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable + // @User: Advanced + // @Units: Hz + // @Range: 0 10 + // @Increment: .5 + GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), + // @Group: SERIAL // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp GOBJECT(serial_manager, "SERIAL", AP_SerialManager), diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index 5ffe4e568e..3b08a0cc59 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -20,7 +20,7 @@ static void acro_run() // if motors not running reset angle targets if(!motors.armed() || g.rc_3.control_in <= 0) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } @@ -34,7 +34,7 @@ static void acro_run() attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost - attitude_control.set_throttle_out(pilot_throttle_scaled, false); + attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); } diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index 5a5cd8a44e..ba16c30948 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -27,7 +27,7 @@ static void althold_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; } @@ -55,7 +55,7 @@ static void althold_run() // reset target lean angles and heading while landed if (ap.land_complete) { - attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true); + attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); }else{ // call attitude controller diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index b63404a971..182ddd47ea 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -114,7 +114,7 @@ static void auto_takeoff_run() // initialise wpnav targets wp_nav.shift_wp_origin_to_current_pos(); // reset attitude control targets - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // tell motors to do a slow start motors.slow_start(true); return; @@ -160,7 +160,7 @@ static void auto_wp_run() if(!ap.auto_armed) { // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // tell motors to do a slow start motors.slow_start(true); return; @@ -216,7 +216,7 @@ static void auto_spline_run() if(!ap.auto_armed) { // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // tell motors to do a slow start motors.slow_start(true); return; @@ -283,7 +283,7 @@ static void auto_land_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // set target to current position wp_nav.init_loiter_target(); return; @@ -449,7 +449,7 @@ void auto_loiter_run() { // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index a7ec3231a2..43c245aa0d 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -263,7 +263,7 @@ static void autotune_run() // if not auto armed set throttle to zero and exit immediately // this should not actually be possible because of the autotune_init() checks if (!ap.auto_armed) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; } @@ -291,7 +291,7 @@ static void autotune_run() // reset target lean angles and heading while landed if (ap.land_complete) { // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true); + attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); }else{ // check if pilot is overriding the controls diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index 478c547ee4..7fb31e4d6b 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -35,7 +35,7 @@ static void circle_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { // To-Do: add some initialisation of position controllers - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; } diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index 1f8e67b54e..f855b9210e 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -48,7 +48,7 @@ static void drift_run() // if not armed or landed and throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || (ap.land_complete && ap.throttle_zero)) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } @@ -95,7 +95,7 @@ static void drift_run() attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle with angle boost - attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true); + attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); } // get_throttle_assist - return throttle output (range 0 ~ 1000) based on pilot input and z-axis velocity diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde index 6e3e4628fb..c82dd87bf0 100644 --- a/ArduCopter/control_flip.pde +++ b/ArduCopter/control_flip.pde @@ -219,8 +219,8 @@ static void flip_run() // output pilot's throttle without angle boost if (throttle_out == 0.0f) { - attitude_control.set_throttle_out_unstabilized(0,false); + attitude_control.set_throttle_out_unstabilized(0,false,g.throttle_filt); } else { - attitude_control.set_throttle_out(throttle_out, false); + attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt); } } diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 1d04a0862e..7fccbc2fb2 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -161,7 +161,7 @@ static void guided_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { // To-Do: reset waypoint controller? - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true return; } @@ -200,7 +200,7 @@ static void guided_takeoff_run() // initialise wpnav targets wp_nav.shift_wp_origin_to_current_pos(); // reset attitude control targets - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // tell motors to do a slow start motors.slow_start(true); return; diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index d72a9ca87b..72e821a120 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -52,7 +52,7 @@ static void land_gps_run() // if not auto armed or landed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); wp_nav.init_loiter_target(); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED @@ -125,7 +125,7 @@ static void land_nogps_run() // if not auto armed or landed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 5b31aadad5..8bf71f7904 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -35,7 +35,7 @@ static void loiter_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { wp_nav.init_loiter_target(); - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; } @@ -75,7 +75,7 @@ static void loiter_run() if (ap.land_complete) { wp_nav.init_loiter_target(); // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true); + attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); }else{ // run loiter controller diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index e595235f86..76610fbaf9 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -156,7 +156,7 @@ static void poshold_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { wp_nav.init_loiter_target(); - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; } @@ -190,7 +190,7 @@ static void poshold_run() if (ap.land_complete) { wp_nav.init_loiter_target(); // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true); + attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; }else{ diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 25a0760429..3a908e59f0 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -134,7 +134,7 @@ static void rtl_climb_return_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { // reset attitude control targets - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // To-Do: re-initialise wpnav targets return; } @@ -190,7 +190,7 @@ static void rtl_loiterathome_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { // reset attitude control targets - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // To-Do: re-initialise wpnav targets return; } @@ -259,7 +259,7 @@ static void rtl_descent_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // set target to current position wp_nav.init_loiter_target(); return; @@ -321,7 +321,7 @@ static void rtl_land_run() float target_yaw_rate = 0; // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // set target to current position wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index 7da2f9a576..0589ef5a1d 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -26,7 +26,7 @@ static void sport_run() // if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || g.rc_3.control_in <= 0) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; } @@ -76,7 +76,7 @@ static void sport_run() // reset target lean angles and heading while landed if (ap.land_complete) { // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true); + attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); }else{ diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index 73d82a572d..4640ce15a0 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -25,7 +25,7 @@ static void stabilize_run() // if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || g.rc_3.control_in <= 0) { - attitude_control.set_throttle_out_unstabilized(0,true); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } @@ -48,5 +48,5 @@ static void stabilize_run() // body-frame rate controller is run directly from 100hz loop // output pilot's throttle - attitude_control.set_throttle_out(pilot_throttle_scaled, true); + attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); } diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde index 4f3feb8d1b..0fbd3b3635 100644 --- a/ArduCopter/heli_control_acro.pde +++ b/ArduCopter/heli_control_acro.pde @@ -49,7 +49,7 @@ static void heli_acro_run() } // output pilot's throttle without angle boost - attitude_control.set_throttle_out(g.rc_3.control_in, false); + attitude_control.set_throttle_out(g.rc_3.control_in, false, g.throttle_filt); } // get_pilot_desired_yaw_rate - transform pilot's yaw input into a desired yaw angle rate diff --git a/ArduCopter/heli_control_stabilize.pde b/ArduCopter/heli_control_stabilize.pde index f4de9d3277..4ae37a8343 100644 --- a/ArduCopter/heli_control_stabilize.pde +++ b/ArduCopter/heli_control_stabilize.pde @@ -54,7 +54,7 @@ static void heli_stabilize_run() attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle - note that TradHeli does not used angle-boost - attitude_control.set_throttle_out(pilot_throttle_scaled, false); + attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); } #endif //HELI_FRAME