mirror of https://github.com/ArduPilot/ardupilot
Plane: fixed RUDDER_ONLY to not combine direct rudder output
this prevents us over-rolling in FBWA
This commit is contained in:
parent
76a81069c2
commit
664c92fb81
|
@ -596,6 +596,9 @@ static int32_t nav_roll_cd;
|
|||
// The instantaneous desired pitch angle. Hundredths of a degree
|
||||
static int32_t nav_pitch_cd;
|
||||
|
||||
// we separate out rudder input to allow for RUDDER_ONLY=1
|
||||
static int16_t rudder_input;
|
||||
|
||||
// the aerodymamic load factor. This is calculated from the demanded
|
||||
// roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
|
||||
static float aerodynamic_load_factor = 1.0f;
|
||||
|
@ -1362,7 +1365,7 @@ static void update_flight_mode(void)
|
|||
any aileron or rudder input
|
||||
*/
|
||||
if ((channel_roll->control_in != 0 ||
|
||||
channel_rudder->control_in != 0)) {
|
||||
rudder_input != 0)) {
|
||||
cruise_state.locked_heading = false;
|
||||
cruise_state.lock_timer_ms = 0;
|
||||
}
|
||||
|
|
|
@ -339,7 +339,7 @@ static void stabilize_acro(float speed_scaler)
|
|||
/*
|
||||
manual rudder for now
|
||||
*/
|
||||
steering_control.steering = steering_control.rudder = channel_rudder->control_in;
|
||||
steering_control.steering = steering_control.rudder = rudder_input;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -414,14 +414,14 @@ static void calc_throttle()
|
|||
static void calc_nav_yaw_coordinated(float speed_scaler)
|
||||
{
|
||||
bool disable_integrator = false;
|
||||
if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
|
||||
if (control_mode == STABILIZE && rudder_input != 0) {
|
||||
disable_integrator = true;
|
||||
}
|
||||
steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
|
||||
|
||||
// add in rudder mixing from roll
|
||||
steering_control.rudder += channel_roll->servo_out * g.kff_rudder_mix;
|
||||
steering_control.rudder += channel_rudder->control_in;
|
||||
steering_control.rudder += rudder_input;
|
||||
steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500);
|
||||
}
|
||||
|
||||
|
@ -451,11 +451,11 @@ static void calc_nav_yaw_ground(void)
|
|||
// manual rudder control while still
|
||||
steer_state.locked_course = false;
|
||||
steer_state.locked_course_err = 0;
|
||||
steering_control.steering = channel_rudder->control_in;
|
||||
steering_control.steering = rudder_input;
|
||||
return;
|
||||
}
|
||||
|
||||
float steer_rate = (channel_rudder->control_in/4500.0f) * g.ground_steer_dps;
|
||||
float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps;
|
||||
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
|
||||
steer_rate = 0;
|
||||
}
|
||||
|
@ -969,6 +969,9 @@ static void set_servos(void)
|
|||
// send values to the PWM timers for output
|
||||
// ----------------------------------------
|
||||
if (g.rudder_only == 0) {
|
||||
// when we RUDDER_ONLY mode we don't send the channel_roll
|
||||
// output and instead rely on KFF_RDDRMIX. That allows the yaw
|
||||
// damper to operate.
|
||||
channel_roll->output();
|
||||
}
|
||||
channel_pitch->output();
|
||||
|
|
|
@ -138,7 +138,7 @@ static void update_cruise()
|
|||
{
|
||||
if (!cruise_state.locked_heading &&
|
||||
channel_roll->control_in == 0 &&
|
||||
channel_rudder->control_in == 0 &&
|
||||
rudder_input == 0 &&
|
||||
gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
|
||||
gps.ground_speed() >= 3 &&
|
||||
cruise_state.lock_timer_ms == 0) {
|
||||
|
|
|
@ -9,6 +9,8 @@
|
|||
static void set_control_channels(void)
|
||||
{
|
||||
if (g.rudder_only) {
|
||||
// in rudder only mode the roll and rudder channels are the
|
||||
// same.
|
||||
channel_roll = RC_Channel::rc_channel(rcmap.yaw()-1);
|
||||
} else {
|
||||
channel_roll = RC_Channel::rc_channel(rcmap.roll()-1);
|
||||
|
@ -174,6 +176,14 @@ static void read_radio()
|
|||
}
|
||||
|
||||
rudder_arm_check();
|
||||
|
||||
if (g.rudder_only != 0) {
|
||||
// in rudder only mode we discard rudder input and get target
|
||||
// attitude from the roll channel.
|
||||
rudder_input = 0;
|
||||
} else {
|
||||
rudder_input = channel_rudder->control_in;
|
||||
}
|
||||
}
|
||||
|
||||
static void control_failsafe(uint16_t pwm)
|
||||
|
|
Loading…
Reference in New Issue