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
|
// The instantaneous desired pitch angle. Hundredths of a degree
|
||||||
static int32_t nav_pitch_cd;
|
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
|
// the aerodymamic load factor. This is calculated from the demanded
|
||||||
// roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
|
// roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
|
||||||
static float aerodynamic_load_factor = 1.0f;
|
static float aerodynamic_load_factor = 1.0f;
|
||||||
@ -1362,7 +1365,7 @@ static void update_flight_mode(void)
|
|||||||
any aileron or rudder input
|
any aileron or rudder input
|
||||||
*/
|
*/
|
||||||
if ((channel_roll->control_in != 0 ||
|
if ((channel_roll->control_in != 0 ||
|
||||||
channel_rudder->control_in != 0)) {
|
rudder_input != 0)) {
|
||||||
cruise_state.locked_heading = false;
|
cruise_state.locked_heading = false;
|
||||||
cruise_state.lock_timer_ms = 0;
|
cruise_state.lock_timer_ms = 0;
|
||||||
}
|
}
|
||||||
|
@ -339,7 +339,7 @@ static void stabilize_acro(float speed_scaler)
|
|||||||
/*
|
/*
|
||||||
manual rudder for now
|
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)
|
static void calc_nav_yaw_coordinated(float speed_scaler)
|
||||||
{
|
{
|
||||||
bool disable_integrator = false;
|
bool disable_integrator = false;
|
||||||
if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
|
if (control_mode == STABILIZE && rudder_input != 0) {
|
||||||
disable_integrator = true;
|
disable_integrator = true;
|
||||||
}
|
}
|
||||||
steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
|
steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
|
||||||
|
|
||||||
// add in rudder mixing from roll
|
// add in rudder mixing from roll
|
||||||
steering_control.rudder += channel_roll->servo_out * g.kff_rudder_mix;
|
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);
|
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
|
// manual rudder control while still
|
||||||
steer_state.locked_course = false;
|
steer_state.locked_course = false;
|
||||||
steer_state.locked_course_err = 0;
|
steer_state.locked_course_err = 0;
|
||||||
steering_control.steering = channel_rudder->control_in;
|
steering_control.steering = rudder_input;
|
||||||
return;
|
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) {
|
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
|
||||||
steer_rate = 0;
|
steer_rate = 0;
|
||||||
}
|
}
|
||||||
@ -969,6 +969,9 @@ static void set_servos(void)
|
|||||||
// send values to the PWM timers for output
|
// send values to the PWM timers for output
|
||||||
// ----------------------------------------
|
// ----------------------------------------
|
||||||
if (g.rudder_only == 0) {
|
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_roll->output();
|
||||||
}
|
}
|
||||||
channel_pitch->output();
|
channel_pitch->output();
|
||||||
|
@ -138,7 +138,7 @@ static void update_cruise()
|
|||||||
{
|
{
|
||||||
if (!cruise_state.locked_heading &&
|
if (!cruise_state.locked_heading &&
|
||||||
channel_roll->control_in == 0 &&
|
channel_roll->control_in == 0 &&
|
||||||
channel_rudder->control_in == 0 &&
|
rudder_input == 0 &&
|
||||||
gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
|
gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
|
||||||
gps.ground_speed() >= 3 &&
|
gps.ground_speed() >= 3 &&
|
||||||
cruise_state.lock_timer_ms == 0) {
|
cruise_state.lock_timer_ms == 0) {
|
||||||
|
@ -9,6 +9,8 @@
|
|||||||
static void set_control_channels(void)
|
static void set_control_channels(void)
|
||||||
{
|
{
|
||||||
if (g.rudder_only) {
|
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);
|
channel_roll = RC_Channel::rc_channel(rcmap.yaw()-1);
|
||||||
} else {
|
} else {
|
||||||
channel_roll = RC_Channel::rc_channel(rcmap.roll()-1);
|
channel_roll = RC_Channel::rc_channel(rcmap.roll()-1);
|
||||||
@ -174,6 +176,14 @@ static void read_radio()
|
|||||||
}
|
}
|
||||||
|
|
||||||
rudder_arm_check();
|
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)
|
static void control_failsafe(uint16_t pwm)
|
||||||
|
Loading…
Reference in New Issue
Block a user