Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: stop passing speed_scaler all over the place

Browse Source
This commit is contained in:
Iampete1 2023-02-01 11:17:27 +00:00 committed by Andrew Tridgell
parent ccb56e573a
commit d928e8b002
2 changed files with 43 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
ArduPlane/Attitude.cpp
View File

@ -108,7 +108,7 @@ bool Plane::stick_mixing_enabled(void)
previously set nav_roll calculates roll servo_out to try to
stabilize the plane at the given roll
*/
void Plane::stabilize_roll(float speed_scaler)
void Plane::stabilize_roll()
{
if (fly_inverted()) {
// we want to fly upside down. We need to cope with wrap of
@ -120,12 +120,13 @@ void Plane::stabilize_roll(float speed_scaler)
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
}
const float roll_out = stabilize_roll_get_roll_out(speed_scaler);
const float roll_out = stabilize_roll_get_roll_out();
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
}
float Plane::stabilize_roll_get_roll_out(float speed_scaler)
float Plane::stabilize_roll_get_roll_out()
{
const float speed_scaler = get_speed_scaler();
#if HAL_QUADPLANE_ENABLED
if (!quadplane.use_fw_attitude_controllers()) {
// use the VTOL rate for control, to ensure consistency
@ -152,7 +153,7 @@ float Plane::stabilize_roll_get_roll_out(float speed_scaler)
previously set nav_pitch and calculates servo_out values to try to
stabilize the plane at the given attitude.
*/
void Plane::stabilize_pitch(float speed_scaler)
void Plane::stabilize_pitch()
{
int8_t force_elevator = takeoff_tail_hold();
if (force_elevator != 0) {
@ -162,12 +163,13 @@ void Plane::stabilize_pitch(float speed_scaler)
return;
}
const float pitch_out = stabilize_pitch_get_pitch_out(speed_scaler);
const float pitch_out = stabilize_pitch_get_pitch_out();
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
}
float Plane::stabilize_pitch_get_pitch_out(float speed_scaler)
float Plane::stabilize_pitch_get_pitch_out()
{
const float speed_scaler = get_speed_scaler();
#if HAL_QUADPLANE_ENABLED
if (!quadplane.use_fw_attitude_controllers()) {
// use the VTOL rate for control, to ensure consistency
@ -296,7 +298,7 @@ void Plane::stabilize_stick_mixing_fbw()
- rate controlled with ground steering
- yaw control for coordinated flight
*/
void Plane::stabilize_yaw(float speed_scaler)
void Plane::stabilize_yaw()
{
if (landing.is_flaring()) {
// in flaring then enable ground steering
@ -329,14 +331,14 @@ void Plane::stabilize_yaw(float speed_scaler)
/*
now calculate steering_control.rudder for the rudder
*/
calc_nav_yaw_coordinated(speed_scaler);
calc_nav_yaw_coordinated();
}
/*
a special stabilization function for training mode
*/
void Plane::stabilize_training(float speed_scaler)
void Plane::stabilize_training()
{
const float rexpo = roll_in_expo(false);
const float pexpo = pitch_in_expo(false);
@ -344,7 +346,7 @@ void Plane::stabilize_training(float speed_scaler)
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
stabilize_roll();
if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
// allow user to get out of the roll
@ -355,7 +357,7 @@ void Plane::stabilize_training(float speed_scaler)
if (training_manual_pitch) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
} else {
stabilize_pitch(speed_scaler);
stabilize_pitch();
if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
// allow user to get back to level
@ -363,7 +365,7 @@ void Plane::stabilize_training(float speed_scaler)
}
}
stabilize_yaw(speed_scaler);
stabilize_yaw();
}
@ -371,14 +373,15 @@ void Plane::stabilize_training(float speed_scaler)
this is the ACRO mode stabilization function. It does rate
stabilization on roll and pitch axes
*/
void Plane::stabilize_acro(float speed_scaler)
void Plane::stabilize_acro()
{
if (g.acro_locking == 2 && g.acro_yaw_rate > 0 &&
yawController.rate_control_enabled()) {
// we can do 3D acro locking
stabilize_acro_quaternion(speed_scaler);
stabilize_acro_quaternion();
return;
}
const float speed_scaler = get_speed_scaler();
const float rexpo = roll_in_expo(true);
const float pexpo = pitch_in_expo(true);
float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate;
@ -448,7 +451,7 @@ void Plane::stabilize_acro(float speed_scaler)
steering_control.steering = steering_control.rudder = yawController.get_rate_out(yaw_rate, speed_scaler, false);
} else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) {
// use yaw controller
calc_nav_yaw_coordinated(speed_scaler);
calc_nav_yaw_coordinated();
} else {
/*
manual rudder
@ -460,8 +463,9 @@ void Plane::stabilize_acro(float speed_scaler)
/*
quaternion based acro stabilization with continuous locking. Enabled with ACRO_LOCKING=2
*/
void Plane::stabilize_acro_quaternion(float speed_scaler)
void Plane::stabilize_acro_quaternion()
{
const float speed_scaler = get_speed_scaler();
auto &q = acro_state.q;
const float rexpo = roll_in_expo(true);
const float pexpo = pitch_in_expo(true);
@ -567,7 +571,6 @@ void Plane::stabilize()
steer_state.locked_course_err = 0;
return;
}
float speed_scaler = get_speed_scaler();
uint32_t now = AP_HAL::millis();
bool allow_stick_mixing = true;
@ -591,10 +594,11 @@ void Plane::stabilize()
last_stabilize_ms = now;
if (control_mode == &mode_training) {
stabilize_training(speed_scaler);
stabilize_training();
#if AP_SCRIPTING_ENABLED
} else if (nav_scripting_active()) {
// scripting is in control of roll and pitch rates and throttle
const float speed_scaler = get_speed_scaler();
const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler);
const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
@ -605,14 +609,14 @@ void Plane::stabilize()
}
#endif
} else if (control_mode == &mode_acro) {
stabilize_acro(speed_scaler);
stabilize_acro();
} else if (control_mode == &mode_stabilize) {
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
stabilize_roll();
stabilize_pitch();
if (allow_stick_mixing) {
stabilize_stick_mixing_direct();
}
stabilize_yaw(speed_scaler);
stabilize_yaw();
#if HAL_QUADPLANE_ENABLED
} else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) {
// run controlers specific to this mode
@ -620,10 +624,10 @@ void Plane::stabilize()
// we also stabilize using fixed wing surfaces
if (plane.control_mode->mode_number() == Mode::Number::QACRO) {
stabilize_acro(speed_scaler);
stabilize_acro();
} else {
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
stabilize_roll();
stabilize_pitch();
}
#endif
} else {
@ -632,9 +636,9 @@ void Plane::stabilize()
if (allow_stick_mixing && ((g.stick_mixing == StickMixing::FBW) || (g.stick_mixing == StickMixing::DIRECT_REMOVED))) {
stabilize_stick_mixing_fbw();
}
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
stabilize_yaw(speed_scaler);
stabilize_roll();
stabilize_pitch();
stabilize_yaw();
}
/*
@ -688,8 +692,9 @@ void Plane::calc_throttle()
/*
calculate yaw control for coordinated flight
*/
void Plane::calc_nav_yaw_coordinated(float speed_scaler)
void Plane::calc_nav_yaw_coordinated()
{
const float speed_scaler = get_speed_scaler();
bool disable_integrator = false;
int16_t rudder_in = rudder_input();

18
ArduPlane/Plane.h
View File

@ -856,17 +856,17 @@ private:
float calc_speed_scaler(void);
float get_speed_scaler(void) const { return surface_speed_scaler; }
bool stick_mixing_enabled(void);
void stabilize_roll(float speed_scaler);
float stabilize_roll_get_roll_out(float speed_scaler);
void stabilize_pitch(float speed_scaler);
float stabilize_pitch_get_pitch_out(float speed_scaler);
void stabilize_roll();
float stabilize_roll_get_roll_out();
void stabilize_pitch();
float stabilize_pitch_get_pitch_out();
void stabilize_stick_mixing_direct();
void stabilize_stick_mixing_fbw();
void stabilize_yaw(float speed_scaler);
void stabilize_training(float speed_scaler);
void stabilize_acro(float speed_scaler);
void stabilize_acro_quaternion(float speed_scaler);
void calc_nav_yaw_coordinated(float speed_scaler);
void stabilize_yaw();
void stabilize_training();
void stabilize_acro();
void stabilize_acro_quaternion();
void calc_nav_yaw_coordinated();
void calc_nav_yaw_course(void);
void calc_nav_yaw_ground(void);