Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: output rudder and steering directly removing steering_control struct

This commit is contained in:
Iampete1 2023-07-06 00:01:16 +01:00 committed by Andrew Tridgell
parent 3534417a12
commit 045cde5fcf
8 changed files with 69 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
ArduPlane/Attitude.cpp
View File

@ -307,38 +307,57 @@ void Plane::stabilize_stick_mixing_fbw()
*/
void Plane::stabilize_yaw()
{
bool ground_steering = false;
if (landing.is_flaring()) {
// in flaring then enable ground steering
steering_control.ground_steering = true;
ground_steering = true;
} else {
// otherwise use ground steering when no input control and we
// are below the GROUND_STEER_ALT
steering_control.ground_steering = (channel_roll->get_control_in() == 0 &&
ground_steering = (channel_roll->get_control_in() == 0 &&
fabsf(relative_altitude) < g.ground_steer_alt);
if (!landing.is_ground_steering_allowed()) {
// don't use ground steering on landing approach
steering_control.ground_steering = false;
ground_steering = false;
}
}
/*
first calculate steering_control.steering for a nose or tail
first calculate steering for a nose or tail
wheel. We use "course hold" mode for the rudder when either performing
a flare (when the wings are held level) or when in course hold in
FBWA mode (when we are below GROUND_STEER_ALT)
*/
float steering_output = 0.0;
if (landing.is_flaring() ||
(steer_state.hold_course_cd != -1 && steering_control.ground_steering)) {
calc_nav_yaw_course();
} else if (steering_control.ground_steering) {
calc_nav_yaw_ground();
(steer_state.hold_course_cd != -1 && ground_steering)) {
steering_output = calc_nav_yaw_course();
} else if (ground_steering) {
steering_output = calc_nav_yaw_ground();
}
/*
now calculate steering_control.rudder for the rudder
now calculate rudder for the rudder
*/
calc_nav_yaw_coordinated();
const float rudder_output = calc_nav_yaw_coordinated();
if (!ground_steering) {
// Not doing ground steering, output rudder on steering channel
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder_output);
} else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
// Ground steering active but no steering output configured, output steering on rudder channel
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output);
} else {
// Ground steering with both steering and rudder channels
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output);
}
}
/*
@ -370,15 +389,17 @@ void Plane::stabilize()
const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
float rudder = 0;
if (yawController.rate_control_enabled()) {
float rudder = nav_scripting.rudder_offset_pct * 45;
rudder = nav_scripting.rudder_offset_pct * 45;
if (nav_scripting.run_yaw_rate_controller) {
rudder += yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false);
} else {
yawController.reset_I();
}
steering_control.rudder = rudder;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder);
#endif
} else {
plane.control_mode->run();
@ -427,7 +448,7 @@ void Plane::calc_throttle()
/*
calculate yaw control for coordinated flight
*/
void Plane::calc_nav_yaw_coordinated()
int16_t Plane::calc_nav_yaw_coordinated()
{
const float speed_scaler = get_speed_scaler();
bool disable_integrator = false;
@ -461,35 +482,35 @@ void Plane::calc_nav_yaw_coordinated()
commanded_rudder += rudder_in;
}
steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500);
if (!using_rate_controller) {
/*
When not running the yaw rate controller, we need to reset the rate
*/
yawController.reset_rate_PID();
}
return constrain_int16(commanded_rudder, -4500, 4500);
}
/*
calculate yaw control for ground steering with specific course
*/
void Plane::calc_nav_yaw_course(void)
int16_t Plane::calc_nav_yaw_course(void)
{
// holding a specific navigation course on the ground. Used in
// auto-takeoff and landing
int32_t bearing_error_cd = nav_controller->bearing_error_cd();
steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd);
int16_t steering = steerController.get_steering_out_angle_error(bearing_error_cd);
if (stick_mixing_enabled()) {
steering_control.steering = channel_rudder->stick_mixing(steering_control.steering);
steering = channel_rudder->stick_mixing(steering);
}
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
return constrain_int16(steering, -4500, 4500);
}
/*
calculate yaw control for ground steering
*/
void Plane::calc_nav_yaw_ground(void)
int16_t Plane::calc_nav_yaw_ground(void)
{
if (gps.ground_speed() < 1 &&
is_zero(get_throttle_input()) &&
@ -498,8 +519,7 @@ void Plane::calc_nav_yaw_ground(void)
// manual rudder control while still
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
steering_control.steering = rudder_input();
return;
return rudder_input();
}
// if we haven't been steering for 1s then clear locked course
@ -526,15 +546,16 @@ void Plane::calc_nav_yaw_ground(void)
}
}
int16_t steering;
if (!steer_state.locked_course) {
// use a rate controller at the pilot specified rate
steering_control.steering = steerController.get_steering_out_rate(steer_rate);
steering = steerController.get_steering_out_rate(steer_rate);
} else {
// use a error controller on the summed error
int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd);
steering = steerController.get_steering_out_angle_error(yaw_error_cd);
}
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
return constrain_int16(steering, -4500, 4500);
}

16
ArduPlane/Plane.h
View File

@ -211,16 +211,6 @@ private:
bool training_manual_roll; // user has manual roll control
bool training_manual_pitch; // user has manual pitch control
/*
keep steering and rudder control separated until we update servos,
to allow for a separate wheel servo from rudder servo
*/
struct {
bool ground_steering; // are we doing ground steering?
int16_t steering; // value for nose/tail wheel
int16_t rudder; // value for rudder
} steering_control;
// should throttle be pass-thru in guided?
bool guided_throttle_passthru;
@ -871,9 +861,9 @@ private:
float stabilize_pitch_get_pitch_out();
void stabilize_stick_mixing_fbw();
void stabilize_yaw();
void calc_nav_yaw_coordinated();
void calc_nav_yaw_course(void);
void calc_nav_yaw_ground(void);
int16_t calc_nav_yaw_coordinated();
int16_t calc_nav_yaw_course(void);
int16_t calc_nav_yaw_ground(void);
// Log.cpp
uint32_t last_log_fast_ms;

7
ArduPlane/mode.cpp
View File

@ -230,3 +230,10 @@ bool Mode::is_taking_off() const
{
return (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF);
}
// Helper to output to both k_rudder and k_steering servo functions
void Mode::output_rudder_and_steering(float val)
{
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val);
}

3
ArduPlane/mode.h
View File

@ -146,6 +146,9 @@ protected:
// mode specific pre-arm checks
virtual bool _pre_arm_checks(size_t buflen, char *buffer) const;
// Helper to output to both k_rudder and k_steering servo functions
void output_rudder_and_steering(float val);
#if HAL_QUADPLANE_ENABLED
// References for convenience, used by QModes
AC_PosControl*& pos_control;

14
ArduPlane/mode_acro.cpp
View File

@ -104,22 +104,24 @@ void ModeAcro::stabilize()
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler));
}
plane.steering_control.steering = plane.rudder_input();
float rudder_output;
if (plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) {
// user has asked for yaw rate control with yaw rate scaled by ACRO_YAW_RATE
const float rudd_expo = plane.rudder_in_expo(true);
const float yaw_rate = (rudd_expo/SERVO_MAX) * plane.g.acro_yaw_rate;
plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false);
rudder_output = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false);
} else if (plane.flight_option_enabled(FlightOptions::ACRO_YAW_DAMPER)) {
// use yaw controller
plane.calc_nav_yaw_coordinated();
rudder_output = plane.calc_nav_yaw_coordinated();
} else {
/*
manual rudder
*/
plane.steering_control.rudder = plane.steering_control.steering;
rudder_output = plane.rudder_input();
}
output_rudder_and_steering(rudder_output);
}
/*
@ -215,7 +217,7 @@ void ModeAcro::stabilize_quaternion()
// call to rate controllers
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(desired_rates.x, speed_scaler));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(desired_rates.y, speed_scaler));
plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false);
output_rudder_and_steering(plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false));
acro_state.roll_active_last = roll_active;
acro_state.pitch_active_last = pitch_active;

4
ArduPlane/mode_manual.cpp
View File

@ -5,9 +5,7 @@ void ModeManual::update()
{
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false));
//rudder in sets rudder, but is also assigned to steering values used later in servos.cpp for steering
plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.steering_control.steering);
output_rudder_and_steering(plane.rudder_in_expo(false));
float throttle = plane.get_throttle_input(true);

5
ArduPlane/mode_training.cpp
View File

@ -64,9 +64,6 @@ void ModeTraining::run()
}
// Always manual rudder control
const float pilot_rudder = plane.rudder_in_expo(false);
plane.steering_control.rudder = pilot_rudder;
plane.steering_control.steering = pilot_rudder;
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, pilot_rudder);
output_rudder_and_steering(plane.rudder_in_expo(false));
}

22
ArduPlane/servos.cpp
View File

@ -819,28 +819,6 @@ void Plane::set_servos(void)
return;
}
/*
see if we are doing ground steering.
*/
if (!steering_control.ground_steering) {
// we are not at an altitude for ground steering. Set the nose
// wheel to the rudder just in case the barometer has drifted
// a lot
steering_control.steering = steering_control.rudder;
} else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
// we are within the ground steering altitude but don't have a
// dedicated steering channel. Set the rudder to the ground
// steering output
steering_control.rudder = steering_control.steering;
}
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering);
if (control_mode != &mode_manual) {
set_servos_controlled();
}