mirror of https://github.com/ArduPilot/ardupilot
Sub: Add low pass filter (from Copter)
This commit is contained in:
parent
46a9cea0be
commit
7e7b7c9afb
|
@ -126,9 +126,6 @@ void Sub::set_throttle_takeoff()
|
|||
{
|
||||
// tell position controller to reset alt target and reset I terms
|
||||
pos_control.init_takeoff();
|
||||
|
||||
// tell motors to do a slow start
|
||||
motors.slow_start(true);
|
||||
}
|
||||
|
||||
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
|
||||
|
|
|
@ -62,6 +62,7 @@ Sub::Sub(void) :
|
|||
baro_alt(0),
|
||||
baro_climbrate(0.0f),
|
||||
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
|
||||
rc_throttle_control_in_filter(1.0f),
|
||||
auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
|
||||
yaw_look_at_WP_bearing(0.0f),
|
||||
yaw_look_at_heading(0),
|
||||
|
|
|
@ -400,6 +400,9 @@ private:
|
|||
float baro_climbrate; // barometer climbrate in cm/s
|
||||
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
|
||||
|
||||
// filtered pilot's throttle input used to cancel landing if throttle held high
|
||||
LowPassFilterFloat rc_throttle_control_in_filter;
|
||||
|
||||
// 3D Location vectors
|
||||
// Current location of the Sub (altitude is relative to home)
|
||||
struct Location current_loc;
|
||||
|
|
|
@ -321,6 +321,14 @@ void Sub::auto_land_run()
|
|||
|
||||
// process pilot's input
|
||||
if (!failsafe.radio) {
|
||||
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
|
||||
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
|
||||
// exit land if throttle is high
|
||||
if (!set_mode(LOITER)) {
|
||||
set_mode(ALT_HOLD);
|
||||
}
|
||||
}
|
||||
|
||||
if (g.land_repositioning) {
|
||||
// apply SIMPLE mode transform to pilot inputs
|
||||
update_simple_mode();
|
||||
|
|
|
@ -84,6 +84,14 @@ void Sub::land_gps_run()
|
|||
|
||||
// process pilot inputs
|
||||
if (!failsafe.radio) {
|
||||
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
|
||||
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
|
||||
// exit land if throttle is high
|
||||
if (!set_mode(LOITER)) {
|
||||
set_mode(ALT_HOLD);
|
||||
}
|
||||
}
|
||||
|
||||
if (g.land_repositioning) {
|
||||
// apply SIMPLE mode transform to pilot inputs
|
||||
update_simple_mode();
|
||||
|
|
|
@ -290,6 +290,14 @@ void Sub::rtl_descent_run()
|
|||
|
||||
// process pilot's input
|
||||
if (!failsafe.radio) {
|
||||
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
|
||||
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
|
||||
// exit land if throttle is high
|
||||
if (!set_mode(LOITER)) {
|
||||
set_mode(ALT_HOLD);
|
||||
}
|
||||
}
|
||||
|
||||
if (g.land_repositioning) {
|
||||
// apply SIMPLE mode transform to pilot inputs
|
||||
update_simple_mode();
|
||||
|
|
Loading…
Reference in New Issue