Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Landing: option to keep landing throttle at thr_min during flare #14387

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
7 changes: 5 additions & 2 deletions ArduPlane/servos.cpp
Expand Up @@ -469,8 +469,11 @@ void Plane::set_servos_controlled(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0);
}
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0 ); // default
// throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines:
if (landing.is_flaring() && landing.use_thr_min_during_flare() ) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get());
}
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
Expand Down
15 changes: 14 additions & 1 deletion libraries/AP_Landing/AP_Landing.cpp
Expand Up @@ -142,7 +142,14 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
// @Group: DS_
// @Path: AP_Landing_Deepstall.cpp
AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall),


// @Param: OPTIONS
// @DisplayName: Landing options bitmask
// @Description: Bitmask of options to use with landing.
// @Bitmask: 0: honor min throttle during landing flare
// @User: Advanced
AP_GROUPINFO("OPTIONS", 16, AP_Landing, _options, 0),

AP_GROUPEND
};

Expand Down Expand Up @@ -606,6 +613,12 @@ bool AP_Landing::is_throttle_suppressed(void) const
}
}

//defaults to false, but _options bit zero enables it.
bool AP_Landing::use_thr_min_during_flare(void) const
{
return (OptionsMask::ON_LANDING_FLARE_USE_THR_MIN & _options) != 0;
}

/*
* returns false when the vehicle might not be flying forward while landing
*/
Expand Down
8 changes: 8 additions & 0 deletions libraries/AP_Landing/AP_Landing.h
Expand Up @@ -57,6 +57,11 @@ class AP_Landing {
// TODO: TYPE_HELICAL,
};

// we support upto 32 boolean bits for users wanting to change landing behaviour.
enum OptionsMask {
ON_LANDING_FLARE_USE_THR_MIN = (1<<0), // If set then set trottle to thr_min instead of zero on final flare
};

void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed);
Expand All @@ -72,6 +77,7 @@ class AP_Landing {
bool is_ground_steering_allowed(void) const;
bool is_throttle_suppressed(void) const;
bool is_flying_forward(void) const;
bool use_thr_min_during_flare(void) const; //defaults to false, but _options bit zero enables it.
void handle_flight_stage_change(const bool _in_landing_stage);
int32_t constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
bool get_target_altitude_location(Location &location);
Expand Down Expand Up @@ -115,6 +121,8 @@ class AP_Landing {
bool in_progress:1;
} flags;

AP_Int16 _options; // user-configurable bitmask options, via a parameter, for landing

// same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope
float initial_slope;

Expand Down