Browse Source

Plane: fixed RUDDER_ONLY to not combine direct rudder output

this prevents us over-rolling in FBWA
master
Andrew Tridgell 10 years ago
parent
commit
664c92fb81
  1. 5
      ArduPlane/ArduPlane.pde
  2. 13
      ArduPlane/Attitude.pde
  3. 2
      ArduPlane/navigation.pde
  4. 10
      ArduPlane/radio.pde

5
ArduPlane/ArduPlane.pde

@ -596,6 +596,9 @@ static int32_t nav_roll_cd; @@ -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) @@ -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;
}

13
ArduPlane/Attitude.pde

@ -339,7 +339,7 @@ static void stabilize_acro(float speed_scaler) @@ -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() @@ -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) @@ -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) @@ -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();

2
ArduPlane/navigation.pde

@ -138,7 +138,7 @@ static void update_cruise() @@ -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) {

10
ArduPlane/radio.pde

@ -9,6 +9,8 @@ @@ -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() @@ -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…
Cancel
Save