Skip to content

Commit

Permalink
Plane: use new SRV_Channels API
Browse files Browse the repository at this point in the history
  • Loading branch information
@tridge
tridge committed Jan 12, 2017
1 parent 4cb9f77 commit b83f50b
Show file tree
Hide file tree
Showing 18 changed files with 309 additions and 802 deletions.
12 changes: 5 additions & 7 deletions ArduPlane/ArduPlane.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -311,7 +311,7 @@ void Plane::afs_fs_check(void)
*/
void Plane::update_aux(void)
{
RC_Channel_aux::enable_aux_servos();
SRV_Channels::enable_aux_servos();
}

void Plane::one_second_loop()
Expand Down Expand Up @@ -558,7 +558,7 @@ void Plane::handle_auto_mode(void)

// we are in the final stage of a landing - force
// zero throttle
channel_throttle->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else {
calc_throttle();
}
Expand Down Expand Up @@ -685,7 +685,7 @@ void Plane::update_flight_mode(void)
// FBWA failsafe glide
nav_roll_cd = 0;
nav_pitch_cd = 0;
channel_throttle->set_servo_out(0);
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
if (g.fbwa_tdrag_chan > 0) {
// check for the user enabling FBWA taildrag takeoff mode
Expand Down Expand Up @@ -748,10 +748,8 @@ void Plane::update_flight_mode(void)
break;

case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
channel_roll->set_servo_out(channel_roll->pwm_to_angle());
channel_pitch->set_servo_out(channel_pitch->pwm_to_angle());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->pwm_to_angle());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->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
Expand Down
78 changes: 36 additions & 42 deletions ArduPlane/Attitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ float Plane::get_speed_scaler(void)
}
speed_scaler = constrain_float(speed_scaler, 0.5f, 2.0f);
} else {
if (channel_throttle->get_servo_out() > 0) {
speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->get_servo_out() / 2.0f); // First order taylor expansion of square root
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) {
speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 2.0f); // First order taylor expansion of square root
// Should maybe be to the 2/7 power, but we aren't going to implement that...
}else{
speed_scaler = 1.67f;
Expand Down Expand Up @@ -80,9 +80,9 @@ void Plane::stabilize_roll(float speed_scaler)
if (control_mode == STABILIZE && channel_roll->get_control_in() != 0) {
disable_integrator = true;
}
channel_roll->set_servo_out(rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler,
disable_integrator));
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler,
disable_integrator));
}

/*
Expand All @@ -96,17 +96,17 @@ void Plane::stabilize_pitch(float speed_scaler)
if (force_elevator != 0) {
// we are holding the tail down during takeoff. Just convert
// from a percentage to a -4500..4500 centidegree angle
channel_pitch->set_servo_out(45*force_elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator);
return;
}
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->get_servo_out() * g.kff_throttle_to_pitch;
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch;
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_pitch->get_control_in() != 0) {
disable_integrator = true;
}
channel_pitch->set_servo_out(pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler,
disable_integrator));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler,
disable_integrator));
}

/*
Expand All @@ -127,17 +127,6 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
servo_out += channel->pwm_to_angle();
}

/*
One argument version for when the servo out in the rc channel
is the target
*/
void Plane::stick_mix_channel(RC_Channel * channel)
{
int16_t servo_out = channel->get_servo_out();
stick_mix_channel(channel,servo_out);
channel->set_servo_out(servo_out);
}

/*
this gives the user control of the aircraft in stabilization modes
*/
Expand All @@ -157,8 +146,13 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == TRAINING) {
return;
}
stick_mix_channel(channel_roll);
stick_mix_channel(channel_pitch);
int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
stick_mix_channel(channel_roll, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);

int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
stick_mix_channel(channel_pitch, elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
}

/*
Expand Down Expand Up @@ -268,25 +262,25 @@ void Plane::stabilize_yaw(float speed_scaler)
void Plane::stabilize_training(float speed_scaler)
{
if (training_manual_roll) {
channel_roll->set_servo_out(channel_roll->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && channel_roll->get_control_in() < channel_roll->get_servo_out()) ||
(nav_roll_cd < 0 && channel_roll->get_control_in() > channel_roll->get_servo_out())) {
if ((nav_roll_cd > 0 && channel_roll->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && channel_roll->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
// allow user to get out of the roll
channel_roll->set_servo_out(channel_roll->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
}
}

if (training_manual_pitch) {
channel_pitch->set_servo_out(channel_pitch->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < channel_pitch->get_servo_out()) ||
(nav_pitch_cd < 0 && channel_pitch->get_control_in() > channel_pitch->get_servo_out())) {
if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && channel_pitch->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
// allow user to get back to level
channel_pitch->set_servo_out(channel_pitch->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
}
}

Expand Down Expand Up @@ -321,16 +315,16 @@ void Plane::stabilize_acro(float speed_scaler)
nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
// try to reduce the integrated angular error to zero. We set
// 'stabilze' to true, which disables the roll integrator
channel_roll->set_servo_out(rollController.get_servo_out(roll_error_cd,
speed_scaler,
true));
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd,
speed_scaler,
true));
} else {
/*
aileron stick is non-zero, use pure rate control until the
user releases the stick
*/
acro_state.locked_roll = false;
channel_roll->set_servo_out(rollController.get_rate_out(roll_rate, speed_scaler));
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_rate_out(roll_rate, speed_scaler));
}

if (g.acro_locking && is_zero(pitch_rate)) {
Expand All @@ -345,15 +339,15 @@ void Plane::stabilize_acro(float speed_scaler)
// try to hold the locked pitch. Note that we have the pitch
// integrator enabled, which helps with inverted flight
nav_pitch_cd = acro_state.locked_pitch_cd;
channel_pitch->set_servo_out(pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false));
} else {
/*
user has non-zero pitch input, use a pure rate controller
*/
acro_state.locked_pitch = false;
channel_pitch->set_servo_out( pitchController.get_rate_out(pitch_rate, speed_scaler));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_rate_out(pitch_rate, speed_scaler));
}

/*
Expand Down Expand Up @@ -423,7 +417,7 @@ void Plane::calc_throttle()
// user has asked for zero throttle - this may be done by a
// mission which wants to turn off the engine for a parachute
// landing
channel_throttle->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
return;
}

Expand All @@ -436,7 +430,7 @@ void Plane::calc_throttle()
commanded_throttle = plane.guided_state.forced_throttle;
}

channel_throttle->set_servo_out(commanded_throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
}

/*****************************************
Expand Down Expand Up @@ -464,7 +458,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator);

// add in rudder mixing from roll
commanded_rudder += channel_roll->get_servo_out() * g.kff_rudder_mix;
commanded_rudder += SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * g.kff_rudder_mix;
commanded_rudder += rudder_input;
}

Expand Down
8 changes: 4 additions & 4 deletions ArduPlane/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,10 +216,10 @@ void Plane::send_servo_out(mavlink_channel_t chan)
chan,
millis(),
0, // port 0
10000 * channel_roll->norm_output() * (channel_roll->get_reverse()?-1:1),
10000 * channel_pitch->norm_output() * (channel_pitch->get_reverse()?-1:1),
10000 * channel_throttle->norm_output() * (channel_throttle->get_reverse()?-1:1),
10000 * channel_rudder->norm_output() * (channel_rudder->get_reverse()?-1:1),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) / 4500.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / 4500.0f),
0,
0,
0,
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/Log.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -263,8 +263,8 @@ void Plane::Log_Write_Control_Tuning()
roll : (int16_t)ahrs.roll_sensor,
nav_pitch_cd : (int16_t)nav_pitch_cd,
pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)channel_throttle->get_servo_out(),
rudder_out : (int16_t)channel_rudder->get_servo_out(),
throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
rudder_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_rudder),
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
Expand Down
86 changes: 16 additions & 70 deletions ArduPlane/Parameters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1047,72 +1047,6 @@ const AP_Param::Info Plane::var_info[] = {
(const void *)&plane.quadplane.attitude_control,
{group_info : AC_AttitudeControl_Multi::var_info}, AP_PARAM_FLAG_POINTER },

// RC channel
//-----------
// @Group: RC1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_1, "RC1_", RC_Channel),

// @Group: RC2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_2, "RC2_", RC_Channel),

// @Group: RC3_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_3, "RC3_", RC_Channel),

// @Group: RC4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_4, "RC4_", RC_Channel),

// @Group: RC5_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_5, "RC5_", RC_Channel_aux),

// @Group: RC6_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_6, "RC6_", RC_Channel_aux),

// @Group: RC7_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_7, "RC7_", RC_Channel_aux),

// @Group: RC8_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux),

// @Group: RC9_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_9, "RC9_", RC_Channel_aux),

// @Group: RC10_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_10, "RC10_", RC_Channel_aux),

// @Group: RC11_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_11, "RC11_", RC_Channel_aux),

// @Group: RC12_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_12, "RC12_", RC_Channel_aux),

// @Group: RC13_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_13, "RC13_", RC_Channel_aux),

// @Group: RC14_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_14, "RC14_", RC_Channel_aux),

// @Group: RC15_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_15, "RC15_", RC_Channel_aux),

// @Group: RC16_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_16, "RC16_", RC_Channel_aux),

// @Group: RLL2SRV_
// @Path: ../libraries/APM_Control/AP_RollController.cpp
GOBJECT(rollController, "RLL2SRV_", AP_RollController),
Expand Down Expand Up @@ -1267,10 +1201,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp
AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine),

// @Group: SERVO_
// @Path: ../libraries/RC_Channel/SRV_Channel.cpp
AP_SUBGROUPINFO(servo_channels, "SERVO", 3, ParametersG2, SRV_Channels),

// 3 was used by prototype for servo_channels

// @Group: SYSID_ENFORCE
// @DisplayName: GCS sysid enforcement
// @Description: This controls whether packets from other than the expected GCS system ID will be accepted
Expand All @@ -1282,6 +1214,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_Stats/AP_Stats.cpp
AP_SUBGROUPINFO(stats, "STAT", 5, ParametersG2, AP_Stats),

// @Group: SERVO
// @Path: ../libraries/SRV_Channel/SRV_Channel.cpp
AP_SUBGROUPINFO(servo_channels, "SERVO", 6, ParametersG2, SRV_Channels),

// @Group: RC
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(rc_channels, "RC", 7, ParametersG2, RC_Channels),

AP_GROUPEND
};

Expand Down Expand Up @@ -1387,6 +1327,12 @@ void Plane::load_parameters(void)
// quadplanes needs a higher loop rate
AP_Param::set_default_by_name("SCHED_LOOP_RATE", 300);
}

// setup defaults in SRV_Channels
g2.servo_channels.set_default_function(CH_1, SRV_Channel::k_aileron);
g2.servo_channels.set_default_function(CH_2, SRV_Channel::k_elevator);
g2.servo_channels.set_default_function(CH_3, SRV_Channel::k_throttle);
g2.servo_channels.set_default_function(CH_4, SRV_Channel::k_rudder);

cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
}
Loading

0 comments on commit b83f50b

Please sign in to comment.