Skip to content

Commit

Permalink
Plane: fixed RUDDER_ONLY to not combine direct rudder output
Browse files Browse the repository at this point in the history
this prevents us over-rolling in FBWA
  • Loading branch information
tridge committed Apr 19, 2015
1 parent 76a8106 commit 664c92f
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 7 deletions.
5 changes: 4 additions & 1 deletion ArduPlane/ArduPlane.pde
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down
13 changes: 8 additions & 5 deletions ArduPlane/Attitude.pde
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

/*
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/navigation.pde
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
10 changes: 10 additions & 0 deletions ArduPlane/radio.pde
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit 664c92f

Please sign in to comment.