Plane: fixed RUDDER_ONLY to not combine direct rudder output

this prevents us over-rolling in FBWA
This commit is contained in:
Andrew Tridgell 2015-04-16 23:30:32 +10:00
parent 76a81069c2
commit 664c92fb81
4 changed files with 23 additions and 7 deletions

View File

@ -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;
}

View File

@ -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();

View File

@ -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) {

View File

@ -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)