Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add PILOT_THR_FILT and call set_throttle_out functions with that value

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-15 21:54:29 -07:00 committed by Randy Mackay
parent 3e0dab7b2d
commit 91a03ae0e7
18 changed files with 47 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/Parameters.h
View File

@ -128,6 +128,7 @@ public:
k_param_dcmcheck_thresh, // deprecated - remove k_param_dcmcheck_thresh, // deprecated - remove
k_param_log_bitmask, k_param_log_bitmask,
k_param_cli_enabled, // 61 k_param_cli_enabled, // 61
k_param_throttle_filt, // 62
// 65: AP_Limits Library // 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove k_param_limits = 65, // deprecated - remove
@ -266,7 +267,7 @@ public:
k_param_rc_9, k_param_rc_9,
k_param_rc_12, k_param_rc_12,
k_param_failsafe_gcs, // 198 k_param_failsafe_gcs, // 198
k_param_rcmap, k_param_rcmap, // 199
// //
// 200: flight modes // 200: flight modes
@ -339,6 +340,8 @@ public:
AP_Int8 cli_enabled; AP_Int8 cli_enabled;
#endif #endif
AP_Float throttle_filt;
AP_Int16 rtl_altitude; AP_Int16 rtl_altitude;
AP_Float sonar_gain; AP_Float sonar_gain;

9
ArduCopter/Parameters.pde
View File

@ -62,6 +62,15 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(cli_enabled, "CLI_ENABLED", 0), GSCALAR(cli_enabled, "CLI_ENABLED", 0),
#endif #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 // @Group: SERIAL
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
GOBJECT(serial_manager, "SERIAL", AP_SerialManager), GOBJECT(serial_manager, "SERIAL", AP_SerialManager),

4
ArduCopter/control_acro.pde
View File

@ -20,7 +20,7 @@ static void acro_run()
// if motors not running reset angle targets // if motors not running reset angle targets
if(!motors.armed() || g.rc_3.control_in <= 0) { 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; return;
} }
@ -34,7 +34,7 @@ static void acro_run()
attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
// output pilot's throttle without angle boost // 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);
} }

4
ArduCopter/control_althold.pde
View File

@ -27,7 +27,7 @@ static void althold_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) { 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(); pos_control.set_alt_target_to_current_alt();
return; return;
} }
@ -55,7 +55,7 @@ static void althold_run()
// reset target lean angles and heading while landed // reset target lean angles and heading while landed
if (ap.land_complete) { 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(); pos_control.set_alt_target_to_current_alt();
}else{ }else{
// call attitude controller // call attitude controller

10
ArduCopter/control_auto.pde
View File

@ -114,7 +114,7 @@ static void auto_takeoff_run()
// initialise wpnav targets // initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos(); wp_nav.shift_wp_origin_to_current_pos();
// reset attitude control targets // 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 // tell motors to do a slow start
motors.slow_start(true); motors.slow_start(true);
return; return;
@ -160,7 +160,7 @@ static void auto_wp_run()
if(!ap.auto_armed) { 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 // 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) // (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 // tell motors to do a slow start
motors.slow_start(true); motors.slow_start(true);
return; return;
@ -216,7 +216,7 @@ static void auto_spline_run()
if(!ap.auto_armed) { 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 // 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) // (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 // tell motors to do a slow start
motors.slow_start(true); motors.slow_start(true);
return; return;
@ -283,7 +283,7 @@ static void auto_land_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) { 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 // set target to current position
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();
return; return;
@ -449,7 +449,7 @@ void auto_loiter_run()
{ {
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) { 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; return;
} }

4
ArduCopter/control_autotune.pde
View File

@ -263,7 +263,7 @@ static void autotune_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
// this should not actually be possible because of the autotune_init() checks // this should not actually be possible because of the autotune_init() checks
if (!ap.auto_armed) { 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(); pos_control.set_alt_target_to_current_alt();
return; return;
} }
@ -291,7 +291,7 @@ static void autotune_run()
// reset target lean angles and heading while landed // reset target lean angles and heading while landed
if (ap.land_complete) { if (ap.land_complete) {
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground // 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(); pos_control.set_alt_target_to_current_alt();
}else{ }else{
// check if pilot is overriding the controls // check if pilot is overriding the controls

2
ArduCopter/control_circle.pde
View File

@ -35,7 +35,7 @@ static void circle_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) { if(!ap.auto_armed || ap.land_complete) {
// To-Do: add some initialisation of position controllers // 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(); pos_control.set_alt_target_to_current_alt();
return; return;
} }

4
ArduCopter/control_drift.pde
View File

@ -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 not armed or landed and throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || (ap.land_complete && ap.throttle_zero)) { 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; 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()); 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 // 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 // get_throttle_assist - return throttle output (range 0 ~ 1000) based on pilot input and z-axis velocity

4
ArduCopter/control_flip.pde
View File

@ -219,8 +219,8 @@ static void flip_run()
// output pilot's throttle without angle boost // output pilot's throttle without angle boost
if (throttle_out == 0.0f) { 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 { } else {
attitude_control.set_throttle_out(throttle_out, false); attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
} }
} }

4
ArduCopter/control_guided.pde
View File

@ -161,7 +161,7 @@ static void guided_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) { if(!ap.auto_armed) {
// To-Do: reset waypoint controller? // 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 // To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
return; return;
} }
@ -200,7 +200,7 @@ static void guided_takeoff_run()
// initialise wpnav targets // initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos(); wp_nav.shift_wp_origin_to_current_pos();
// reset attitude control targets // 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 // tell motors to do a slow start
motors.slow_start(true); motors.slow_start(true);
return; return;

4
ArduCopter/control_land.pde
View File

@ -52,7 +52,7 @@ static void land_gps_run()
// if not auto armed or landed set throttle to zero and exit immediately // if not auto armed or landed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) { 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(); wp_nav.init_loiter_target();
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED #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 not auto armed or landed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) { 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 #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum // disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {

4
ArduCopter/control_loiter.pde
View File

@ -35,7 +35,7 @@ static void loiter_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) { if(!ap.auto_armed) {
wp_nav.init_loiter_target(); 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(); pos_control.set_alt_target_to_current_alt();
return; return;
} }
@ -75,7 +75,7 @@ static void loiter_run()
if (ap.land_complete) { if (ap.land_complete) {
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground // 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(); pos_control.set_alt_target_to_current_alt();
}else{ }else{
// run loiter controller // run loiter controller

4
ArduCopter/control_poshold.pde
View File

@ -156,7 +156,7 @@ static void poshold_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) { if(!ap.auto_armed) {
wp_nav.init_loiter_target(); 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(); pos_control.set_alt_target_to_current_alt();
return; return;
} }
@ -190,7 +190,7 @@ static void poshold_run()
if (ap.land_complete) { if (ap.land_complete) {
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground // 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(); pos_control.set_alt_target_to_current_alt();
return; return;
}else{ }else{

8
ArduCopter/control_rtl.pde
View File

@ -134,7 +134,7 @@ static void rtl_climb_return_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) { if(!ap.auto_armed) {
// reset attitude control targets // 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 // To-Do: re-initialise wpnav targets
return; return;
} }
@ -190,7 +190,7 @@ static void rtl_loiterathome_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) { if(!ap.auto_armed) {
// reset attitude control targets // 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 // To-Do: re-initialise wpnav targets
return; return;
} }
@ -259,7 +259,7 @@ static void rtl_descent_run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) { 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 // set target to current position
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();
return; return;
@ -321,7 +321,7 @@ static void rtl_land_run()
float target_yaw_rate = 0; float target_yaw_rate = 0;
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) { 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 // set target to current position
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();

4
ArduCopter/control_sport.pde
View File

@ -26,7 +26,7 @@ static void sport_run()
// if not armed or throttle at zero, set throttle to zero and exit immediately // if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || g.rc_3.control_in <= 0) { 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(); pos_control.set_alt_target_to_current_alt();
return; return;
} }
@ -76,7 +76,7 @@ static void sport_run()
// reset target lean angles and heading while landed // reset target lean angles and heading while landed
if (ap.land_complete) { if (ap.land_complete) {
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground // 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(); pos_control.set_alt_target_to_current_alt();
}else{ }else{

4
ArduCopter/control_stabilize.pde
View File

@ -25,7 +25,7 @@ static void stabilize_run()
// if not armed or throttle at zero, set throttle to zero and exit immediately // if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || g.rc_3.control_in <= 0) { 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; return;
} }
@ -48,5 +48,5 @@ static void stabilize_run()
// body-frame rate controller is run directly from 100hz loop // body-frame rate controller is run directly from 100hz loop
// output pilot's throttle // 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);
} }

2
ArduCopter/heli_control_acro.pde
View File

@ -49,7 +49,7 @@ static void heli_acro_run()
} }
// output pilot's throttle without angle boost // 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 // get_pilot_desired_yaw_rate - transform pilot's yaw input into a desired yaw angle rate

2
ArduCopter/heli_control_stabilize.pde
View File

@ -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()); 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 // 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 #endif //HELI_FRAME