aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: added support for a separate steering channel

Browse Source 
good for ground steering on larger aircraft
This commit is contained in:
Andrew Tridgell 2014-03-05 12:13:19 +11:00
parent dd502e98b4
commit 2cf98e476c
3 changed files with 62 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/ArduPlane.pde
View File

@ -302,6 +302,16 @@ SITL sitl;
static bool training_manual_roll; // user has manual roll control
static bool training_manual_pitch; // user has manual pitch control
/*
keep steering and rudder control separated until we update servos,
to allow for a separate wheel servo from rudder servo
*/
static struct {
bool ground_steering; // are we doing ground steering?
int16_t steering; // value for nose/tail wheel
int16_t rudder; // value for rudder
} steering_control;
// should throttle be pass-thru in guided?
static bool guided_throttle_passthru;
@ -1317,7 +1327,7 @@ static void update_flight_mode(void)
// ---------------------------------
channel_roll->servo_out = channel_roll->pwm_to_angle();
channel_pitch->servo_out = channel_pitch->pwm_to_angle();
channel_rudder->servo_out = channel_rudder->pwm_to_angle();
steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle();
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000

76
ArduPlane/Attitude.pde
View File

@ -110,7 +110,7 @@ static void stabilize_pitch(float speed_scaler)
controller as it increases the influence of the users stick input,
allowing the user full deflection if needed
*/
static void stick_mix_channel(RC_Channel *channel)
static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
{
float ch_inf;
@ -118,8 +118,8 @@ static void stick_mix_channel(RC_Channel *channel)
ch_inf = fabsf(ch_inf);
ch_inf = min(ch_inf, 400.0);
ch_inf = ((400.0 - ch_inf) / 400.0);
channel->servo_out *= ch_inf;
channel->servo_out += channel->pwm_to_angle();
servo_out *= ch_inf;
servo_out += channel->pwm_to_angle();
}
/*
@ -135,8 +135,8 @@ static void stabilize_stick_mixing_direct()
control_mode == TRAINING) {
return;
}
stick_mix_channel(channel_roll);
stick_mix_channel(channel_pitch);
stick_mix_channel(channel_roll, channel_roll->servo_out);
stick_mix_channel(channel_pitch, channel_pitch->servo_out);
}
/*
@ -194,15 +194,21 @@ static void stabilize_stick_mixing_fbw()
*/
static void stabilize_yaw(float speed_scaler)
{
bool ground_steering = (channel_roll->control_in == 0 && fabsf(relative_altitude()) < g.ground_steer_alt);
steering_control.ground_steering = (channel_roll->control_in == 0 && fabsf(relative_altitude()) < g.ground_steer_alt);
if (steer_state.hold_course_cd != -1 && ground_steering) {
/*
first calculate steering_control.steering for a nose or tail wheel
*/
if (steer_state.hold_course_cd != -1 && steering_control.ground_steering) {
calc_nav_yaw_course();
} else if (ground_steering) {
} else if (steering_control.ground_steering) {
calc_nav_yaw_ground();
} else {
calc_nav_yaw_coordinated(speed_scaler);
}
/*
now calculate steering_control.rudder for the rudder
*/
calc_nav_yaw_coordinated(speed_scaler);
}
@ -303,7 +309,7 @@ static void stabilize_acro(float speed_scaler)
/*
manual rudder for now
*/
channel_rudder->servo_out = channel_rudder->control_in;
steering_control.steering = steering_control.rudder = channel_rudder->control_in;
}
/*
@ -376,12 +382,12 @@ static void calc_nav_yaw_coordinated(float speed_scaler)
if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
disable_integrator = true;
}
channel_rudder->servo_out = 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
channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix;
channel_rudder->servo_out += channel_rudder->control_in;
channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
steering_control.rudder += channel_roll->servo_out * g.kff_rudder_mix;
steering_control.rudder += channel_rudder->control_in;
steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500);
}
/*
@ -392,11 +398,11 @@ static void calc_nav_yaw_course(void)
// holding a specific navigation course on the ground. Used in
// auto-takeoff and landing
int32_t bearing_error_cd = nav_controller->bearing_error_cd();
channel_rudder->servo_out = steerController.get_steering_out_angle_error(bearing_error_cd);
steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd);
if (stick_mixing_enabled()) {
stick_mix_channel(channel_rudder);
stick_mix_channel(channel_rudder, steering_control.steering);
}
channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}
/*
@ -409,7 +415,7 @@ static void calc_nav_yaw_ground(void)
// manual rudder control while still
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
channel_rudder->servo_out = channel_rudder->control_in;
steering_control.steering = channel_rudder->control_in;
return;
}
@ -424,14 +430,14 @@ static void calc_nav_yaw_ground(void)
}
if (!steer_state.locked_course) {
// use a rate controller at the pilot specified rate
channel_rudder->servo_out = steerController.get_steering_out_rate(steer_rate);
steering_control.steering = steerController.get_steering_out_rate(steer_rate);
} else {
// use a error controller on the summed error
steer_state.locked_course_err += ahrs.get_gyro().z * G_Dt;
int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
channel_rudder->servo_out = steerController.get_steering_out_angle_error(yaw_error_cd);
steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd);
}
channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}
@ -714,6 +720,28 @@ static void set_servos(void)
{
int16_t last_throttle = channel_throttle->radio_out;
/*
see if we are doing ground steering.
*/
if (!steering_control.ground_steering) {
// we are not at an altitude for ground steering. Set the nose
// wheel to the rudder just in case the barometer has drifted
// a lot
steering_control.steering = steering_control.rudder;
} else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) {
// we are within the ground steering altitude but don't have a
// dedicated steering channel. Set the rudder to the ground
// steering output
steering_control.rudder = steering_control.steering;
}
channel_rudder->servo_out = steering_control.rudder;
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, steering_control.rudder);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
// do a direct pass through of radio values
if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
@ -734,7 +762,6 @@ static void set_servos(void)
// aileron won't quite follow the first one
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, channel_rudder->pwm_to_angle_dz(0));
// this variant assumes you have the corresponding
// input channel setup in your transmitter for manual control
@ -757,9 +784,6 @@ static void set_servos(void)
// both types of secondary elevator are slaved to the pitch servo out
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out);
// setup secondary rudder
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, channel_rudder->servo_out);
}else{
/*Elevon mode*/
float ch1;

1
ArduPlane/failsafe.pde
View File

@ -50,6 +50,7 @@ void failsafe_check(void)
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->radio_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->radio_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, channel_rudder->radio_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_rudder->radio_out);
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out);