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.

Sign up for GitHub

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

Jump to bottom

Plane: rework set_servos_controlled function #25371

Merged
merged 1 commit into from
Oct 31, 2023
Merged

Plane: rework set_servos_controlled function #25371

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1096,6 +1096,7 @@ class Plane : public AP_Vehicle {
void set_servos_idle(void);
void set_servos();
void set_servos_controlled(void);
void set_takeoff_expected(void);
void set_servos_old_elevons(void);
void set_servos_flaps(void);
void set_landing_gear(void);
Expand Down
40 changes: 22 additions & 18 deletions ArduPlane/servos.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -485,28 +485,22 @@ void Plane::set_servos_controlled(void)
min_throttle = 0;
}

bool flight_stage_determines_max_throttle = false;
if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF ||
flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING
) {
flight_stage_determines_max_throttle = true;
}
const bool use_takeoff_throttle_max =
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_transition()) {
flight_stage_determines_max_throttle = true;
}
quadplane.in_transition() ||
#endif
if (flight_stage_determines_max_throttle) {
(flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
(flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);

if (use_takeoff_throttle_max) {
if (aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
max_throttle = aparm.throttle_max;
max_throttle = aparm.takeoff_throttle_max.get();
}
} else if (landing.is_flaring()) {
min_throttle = 0;
}

// conpensate for battery voltage drop
// compensate for battery voltage drop
throttle_voltage_comp(min_throttle, max_throttle);

// apply watt limiter
Expand All @@ -516,14 +510,16 @@ void Plane::set_servos_controlled(void)
constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));

if (!arming.is_armed_and_safety_off()) {
// Always set 0 scaled even if overriding to zero pwm.
// This ensures slew limits and other functions using the scaled value pick up in the correct place
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);

if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
}
} else if (suppress_throttle()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default
Expand Down Expand Up @@ -573,6 +569,13 @@ void Plane::set_servos_controlled(void)
#endif // HAL_QUADPLANE_ENABLED
}

}

/*
Warn AHRS that we might take off soon
*/
void Plane::set_takeoff_expected(void)
{
// let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (!is_flying() && arming.is_armed()) {
Expand Down Expand Up @@ -821,6 +824,7 @@ void Plane::set_servos(void)

if (control_mode != &mode_manual) {
set_servos_controlled();
set_takeoff_expected();
}

// setup flap outputs
Expand Down