Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: Fix up after refactoring RC_Channel class 

Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to  get and set functionsss

old public member(int16_t)   get function -> int16_t     set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in                     c.get_radio_in()           c.set_radio_in(v)
c.control_in                   c.get_control_in()         c.set_control_in(v)
c.servo_out                    c.get_servo_out()          c.set_servo_out(v)
c.pwm_out                      c.get_pwm_out()            // use existing
c.radio_out                    c.get_radio_out()          c.set_radio_out(v)
c.radio_max                    c.get_radio_max()          c.set_radio_max(v)
c.radio_min                    c.get_radio_min()          c.set_radio_min(v)
c.radio_trim                   c.get_radio_trim()         c.set_radio_trim(v);

c.min_max_configured() // return true if min and max are configured

Because data members of RC_Channels are now private and so cannot be written directly
 some overloads are provided in the Plane classes to provide the old functionality

new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);

new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;

Rename functions

 RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
    to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)

 RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
    to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)

 Rationale:

        RC_Channel is a complicated class, which combines
        several functionalities dealing with stick inputs
        in pwm and logical units, logical and actual actuator
        outputs, unit conversion etc, etc
        The intent of this PR is to clarify existing use of
        the class. At the basic level it should now be possible
        to grep all places where private variable is set by
        searching for the set_xx function.

        (The wider purpose is to provide a more generic and
        logically simpler method of output mixing. This is a small step)
This commit is contained in:
skyscraper 2016-05-08 09:33:02 +01:00 committed by Andrew Tridgell
parent c4aa55a6d9
commit 7f29903287
14 changed files with 242 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/ArduPlane.cpp
View File

@ -518,7 +518,7 @@ void Plane::handle_auto_mode(void)
if (auto_state.land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->servo_out = 0;
channel_throttle->set_servo_out(0);
}
} else {
// we are doing normal AUTO flight, the special cases
@ -638,7 +638,7 @@ void Plane::update_flight_mode(void)
// FBWA failsafe glide
nav_roll_cd = 0;
nav_pitch_cd = 0;
channel_throttle->servo_out = 0;
channel_throttle->set_servo_out(0);
}
if (g.fbwa_tdrag_chan > 0) {
// check for the user enabling FBWA taildrag takeoff mode
@ -667,7 +667,7 @@ void Plane::update_flight_mode(void)
roll when heading is locked. Heading becomes unlocked on
any aileron or rudder input
*/
if ((channel_roll->control_in != 0 ||
if ((channel_roll->get_control_in() != 0 ||
rudder_input != 0)) {
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
@ -703,8 +703,8 @@ void Plane::update_flight_mode(void)
case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
channel_roll->servo_out = channel_roll->pwm_to_angle();
channel_pitch->servo_out = channel_pitch->pwm_to_angle();
channel_roll->set_servo_out(channel_roll->pwm_to_angle());
channel_pitch->set_servo_out(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
@ -928,7 +928,7 @@ void Plane::update_flight_stage(void)
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if ((g.land_abort_throttle_enable && channel_throttle->control_in >= 90) ||
if ((g.land_abort_throttle_enable && channel_throttle->get_control_in() >= 90) ||
auto_state.commanded_go_around ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
// abort mode is sticky, it must complete while executing NAV_LAND

215
ArduPlane/Attitude.cpp
View File

@ -21,8 +21,8 @@ float Plane::get_speed_scaler(void)
}
speed_scaler = constrain_float(speed_scaler, 0.5f, 2.0f);
} else {
if (channel_throttle->servo_out > 0) {
speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->servo_out / 2.0f); // First order taylor expansion of square root
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
// Should maybe be to the 2/7 power, but we aren't going to implement that...
}else{
speed_scaler = 1.67f;
@ -79,12 +79,12 @@ void Plane::stabilize_roll(float speed_scaler)
}
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_roll->control_in != 0) {
if (control_mode == STABILIZE && channel_roll->get_control_in() != 0) {
disable_integrator = true;
}
channel_roll->servo_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
channel_roll->set_servo_out(rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler,
disable_integrator);
disable_integrator));
}
/*
@ -98,17 +98,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->servo_out = 45*force_elevator;
channel_pitch->set_servo_out(45*force_elevator);
return;
}
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch;
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->get_servo_out() * g.kff_throttle_to_pitch;
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_pitch->control_in != 0) {
if (control_mode == STABILIZE && channel_pitch->get_control_in() != 0) {
disable_integrator = true;
}
channel_pitch->servo_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
channel_pitch->set_servo_out (pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler,
disable_integrator);
disable_integrator) );
}
/*
@ -121,7 +121,7 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
{
float ch_inf;
ch_inf = (float)channel->radio_in - (float)channel->radio_trim;
ch_inf = (float)channel->get_radio_in() - (float)channel->get_radio_trim();
ch_inf = fabsf(ch_inf);
ch_inf = MIN(ch_inf, 400.0f);
ch_inf = ((400.0f - ch_inf) / 400.0f);
@ -129,6 +129,18 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
servo_out += channel->pwm_to_angle();
}
// one argument version when
void Plane::stick_mix_channel(RC_Channel *channel)
{
float ch_inf;
ch_inf = (float)channel->get_radio_in() - (float)channel->get_radio_trim();
ch_inf = fabsf(ch_inf);
ch_inf = MIN(ch_inf, 400.0f);
ch_inf = ((400.0f - ch_inf) / 400.0f);
channel->set_servo_out(channel->get_servo_out() * ch_inf + channel->pwm_to_angle());
}
/*
this gives the user control of the aircraft in stabilization modes
*/
@ -148,8 +160,8 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == TRAINING) {
return;
}
stick_mix_channel(channel_roll, channel_roll->servo_out);
stick_mix_channel(channel_pitch, channel_pitch->servo_out);
stick_mix_channel(channel_roll);
stick_mix_channel(channel_pitch);
}
/*
@ -219,7 +231,7 @@ void Plane::stabilize_yaw(float speed_scaler)
} else {
// otherwise use ground steering when no input control and we
// are below the GROUND_STEER_ALT
steering_control.ground_steering = (channel_roll->control_in == 0 &&
steering_control.ground_steering = (channel_roll->get_control_in() == 0 &&
fabsf(relative_altitude()) < g.ground_steer_alt);
if (control_mode == AUTO &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
@ -257,25 +269,25 @@ void Plane::stabilize_yaw(float speed_scaler)
void Plane::stabilize_training(float speed_scaler)
{
if (training_manual_roll) {
channel_roll->servo_out = channel_roll->control_in;
channel_roll->set_servo_out(channel_roll->get_control_in());
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && channel_roll->control_in < channel_roll->servo_out) ||
(nav_roll_cd < 0 && channel_roll->control_in > channel_roll->servo_out)) {
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())) {
// allow user to get out of the roll
channel_roll->servo_out = channel_roll->control_in;
channel_roll->set_servo_out(channel_roll->get_control_in());
}
}
if (training_manual_pitch) {
channel_pitch->servo_out = channel_pitch->control_in;
channel_pitch->set_servo_out(channel_pitch->get_control_in());
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && channel_pitch->control_in < channel_pitch->servo_out) ||
(nav_pitch_cd < 0 && channel_pitch->control_in > channel_pitch->servo_out)) {
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())) {
// allow user to get back to level
channel_pitch->servo_out = channel_pitch->control_in;
channel_pitch->set_servo_out(channel_pitch->get_control_in());
}
}
@ -289,8 +301,8 @@ void Plane::stabilize_training(float speed_scaler)
*/
void Plane::stabilize_acro(float speed_scaler)
{
float roll_rate = (channel_roll->control_in/4500.0f) * g.acro_roll_rate;
float pitch_rate = (channel_pitch->control_in/4500.0f) * g.acro_pitch_rate;
float roll_rate = (channel_roll->get_control_in()/4500.0f) * g.acro_roll_rate;
float pitch_rate = (channel_pitch->get_control_in()/4500.0f) * g.acro_pitch_rate;
/*
check for special roll handling near the pitch poles
@ -310,16 +322,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->servo_out = rollController.get_servo_out(roll_error_cd,
channel_roll->set_servo_out(rollController.get_servo_out(roll_error_cd,
speed_scaler,
true);
true));
} else {
/*
aileron stick is non-zero, use pure rate control until the
user releases the stick
*/
acro_state.locked_roll = false;
channel_roll->servo_out = rollController.get_rate_out(roll_rate, speed_scaler);
channel_roll->set_servo_out(rollController.get_rate_out(roll_rate, speed_scaler));
}
if (g.acro_locking && is_zero(pitch_rate)) {
@ -334,15 +346,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->servo_out = pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
channel_pitch->set_servo_out(pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false);
false));
} else {
/*
user has non-zero pitch input, use a pure rate controller
*/
acro_state.locked_pitch = false;
channel_pitch->servo_out = pitchController.get_rate_out(pitch_rate, speed_scaler);
channel_pitch->set_servo_out( pitchController.get_rate_out(pitch_rate, speed_scaler));
}
/*
@ -387,7 +399,7 @@ void Plane::stabilize()
/*
see if we should zero the attitude controller integrators.
*/
if (channel_throttle->control_in == 0 &&
if (channel_throttle->get_control_in() == 0 &&
relative_altitude_abs_cm() < 500 &&
fabsf(barometer.get_climb_rate()) < 0.5f &&
gps.ground_speed() < 3) {
@ -412,11 +424,11 @@ 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->servo_out = 0;
channel_throttle->set_servo_out(0);
return;
}
channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
channel_throttle->set_servo_out(SpdHgt_Controller->get_throttle_demand());
}
/*****************************************
@ -435,7 +447,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
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_roll->get_servo_out() * g.kff_rudder_mix;
steering_control.rudder += rudder_input;
steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500);
}
@ -461,7 +473,7 @@ void Plane::calc_nav_yaw_course(void)
void Plane::calc_nav_yaw_ground(void)
{
if (gps.ground_speed() < 1 &&
channel_throttle->control_in == 0 &&
channel_throttle->get_control_in() == 0 &&
flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
// manual rudder control while still
@ -538,12 +550,12 @@ void Plane::throttle_slew_limit(int16_t last_throttle)
// if slew limit rate is set to zero then do not slew limit
if (slewrate) {
// limit throttle change by the given percentage per second
float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
// allow a minimum change of 1 PWM per cycle
if (temp < 1) {
temp = 1;
}
channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp);
channel_throttle->set_radio_out(constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp));
}
}
@ -662,7 +674,7 @@ bool Plane::suppress_throttle(void)
/*
implement a software VTail or elevon mixer. There are 4 different mixing modes
*/
void Plane::channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out)
void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16_t & chan2_out)const
{
int16_t c1, c2;
int16_t v1, v2;
@ -704,6 +716,17 @@ void Plane::channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_
chan2_out = 1500 + v2;
}
void Plane::channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const
{
int16_t ch1 = chan1->get_radio_out();
int16_t ch2 = chan2->get_radio_out();
channel_output_mixer(mixing_type,ch1,ch2);
chan1->set_radio_out(ch1);
chan2->set_radio_out(ch2);
}
/*
setup flaperon output channels
*/
@ -724,7 +747,7 @@ void Plane::flaperon_update(int8_t flap_percent)
by mixing gain). flapin_channel's trim is not used.
*/
ch1 = channel_roll->radio_out;
ch1 = channel_roll->get_radio_out();
// The *5 is to take a percentage to a value from -500 to 500 for the mixer
ch2 = 1500 - flap_percent * 5;
channel_output_mixer(g.flaperon_output, ch1, ch2);
@ -758,9 +781,9 @@ void Plane::set_servos_idle(void)
} else {
auto_state.idle_wiggle_stage = 0;
}
channel_roll->servo_out = servo_value;
channel_pitch->servo_out = servo_value;
channel_rudder->servo_out = servo_value;
channel_roll->set_servo_out(servo_value);
channel_pitch->set_servo_out(servo_value);
channel_rudder->set_servo_out(servo_value);
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
channel_rudder->calc_pwm();
@ -777,9 +800,9 @@ void Plane::set_servos_idle(void)
uint16_t Plane::throttle_min(void) const
{
if (aparm.throttle_min < 0) {
return channel_throttle->radio_trim;
return channel_throttle->get_radio_trim();
}
return channel_throttle->get_reverse() ? channel_throttle->radio_max : channel_throttle->radio_min;
return channel_throttle->get_reverse() ? channel_throttle->get_radio_max() : channel_throttle->get_radio_min();
};
@ -788,7 +811,7 @@ uint16_t Plane::throttle_min(void) const
*****************************************/
void Plane::set_servos(void)
{
int16_t last_throttle = channel_throttle->radio_out;
int16_t last_throttle = channel_throttle->get_radio_out();
// do any transition updates for quadplane
quadplane.update();
@ -813,25 +836,25 @@ void Plane::set_servos(void)
// steering output
steering_control.rudder = steering_control.steering;
}
channel_rudder->servo_out = steering_control.rudder;
channel_rudder->set_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);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder);
RC_Channel_aux::set_servo_out_for(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) {
channel_roll->radio_out = channel_roll->radio_in;
channel_pitch->radio_out = channel_pitch->radio_in;
channel_roll->set_radio_out(channel_roll->get_radio_in());
channel_pitch->set_radio_out(channel_pitch->get_radio_in());
} else {
channel_roll->radio_out = channel_roll->read();
channel_pitch->radio_out = channel_pitch->read();
channel_roll->set_radio_out(channel_roll->read());
channel_pitch->set_radio_out(channel_pitch->read());
}
channel_throttle->radio_out = channel_throttle->radio_in;
channel_rudder->radio_out = channel_rudder->radio_in;
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
channel_rudder->set_radio_out(channel_rudder->get_radio_in());
// setup extra channels. We want this to come from the
// main input channel, but using the 2nd channels dead
@ -839,8 +862,8 @@ void Plane::set_servos(void)
// pwm_to_angle_dz() to ensure we don't trim the value for the
// deadzone of the main aileron channel, otherwise the 2nd
// 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_for(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0));
// this variant assumes you have the corresponding
// input channel setup in your transmitter for manual control
@ -851,24 +874,24 @@ void Plane::set_servos(void)
if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) {
// set any differential spoilers to follow the elevons in
// manual mode.
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->radio_out);
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->radio_out);
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->get_radio_out());
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->get_radio_out());
}
} else {
if (g.mix_mode == 0) {
// both types of secondary aileron are slaved to the roll servo out
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());
// 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);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());
}else{
/*Elevon mode*/
float ch1;
float ch2;
ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
ch1 = channel_pitch->get_servo_out() - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());
ch2 = channel_pitch->get_servo_out() + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());
/* Differential Spoilers
If differential spoilers are setup, then we translate
@ -879,20 +902,20 @@ void Plane::set_servos(void)
if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) {
float ch3 = ch1;
float ch4 = ch2;
if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) {
ch1 += abs(channel_rudder->servo_out);
ch3 -= abs(channel_rudder->servo_out);
if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->get_servo_out() < 0) {
ch1 += abs(channel_rudder->get_servo_out());
ch3 -= abs(channel_rudder->get_servo_out());
} else {
ch2 += abs(channel_rudder->servo_out);
ch4 -= abs(channel_rudder->servo_out);
ch2 += abs(channel_rudder->get_servo_out());
ch4 -= abs(channel_rudder->get_servo_out());
}
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, ch3);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, ch4);
}
// directly set the radio_out values for elevon mode
channel_roll->radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX));
channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX));
channel_roll->set_radio_out(elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)));
channel_pitch->set_radio_out(elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)));
}
// push out the PWM values
@ -903,7 +926,7 @@ void Plane::set_servos(void)
channel_rudder->calc_pwm();
#if THROTTLE_OUT == 0
channel_throttle->servo_out = 0;
channel_throttle->set_servo_out(0);
#else
// convert 0 to 100% (or -100 to +100) into PWM
int8_t min_throttle = aparm.throttle_min.get();
@ -933,14 +956,14 @@ void Plane::set_servos(void)
// overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value
// throttle limit will attack by 10% per second
if (channel_throttle->servo_out > 0 && // demanding too much positive thrust
if (channel_throttle->get_servo_out() > 0 && // demanding too much positive thrust
throttle_watt_limit_max < max_throttle - 25 &&
now - throttle_watt_limit_timer_ms >= 1) {
// always allow for 25% throttle available regardless of battery status
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_max++;
} else if (channel_throttle->servo_out < 0 &&
} else if (channel_throttle->get_servo_out() < 0 &&
min_throttle < 0 && // reverse thrust is available
throttle_watt_limit_min < -(min_throttle) - 25 &&
now - throttle_watt_limit_timer_ms >= 1) {
@ -953,12 +976,12 @@ void Plane::set_servos(void)
// it has been 1 second since last over-current, check if we can resume higher throttle.
// this throttle release is needed to allow raising the max_throttle as the battery voltage drains down
// throttle limit will release by 1% per second
if (channel_throttle->servo_out > throttle_watt_limit_max && // demanding max forward thrust
if (channel_throttle->get_servo_out() > throttle_watt_limit_max && // demanding max forward thrust
throttle_watt_limit_max > 0) { // and we're currently limiting it
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_max--;
} else if (channel_throttle->servo_out < throttle_watt_limit_min && // demanding max negative thrust
} else if (channel_throttle->get_servo_out() < throttle_watt_limit_min && // demanding max negative thrust
throttle_watt_limit_min > 0) { // and we're limiting it
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_min--;
@ -970,19 +993,19 @@ void Plane::set_servos(void)
min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0);
}
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
min_throttle,
max_throttle);
max_throttle));
if (!hal.util->get_soft_armed()) {
channel_throttle->servo_out = 0;
channel_throttle->set_servo_out(0);
channel_throttle->calc_pwm();
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
channel_throttle->servo_out = 0;
channel_throttle->set_servo_out(0);
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
channel_throttle->radio_out = channel_throttle->radio_in;
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else {
channel_throttle->calc_pwm();
}
@ -994,14 +1017,14 @@ void Plane::set_servos(void)
control_mode == AUTOTUNE)) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
channel_throttle->radio_out = channel_throttle->radio_in;
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else if (control_mode == GUIDED &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->radio_out = channel_throttle->radio_in;
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else if (quadplane.in_vtol_mode()) {
// ask quadplane code for forward throttle
channel_throttle->servo_out = quadplane.forward_throttle_pct();
channel_throttle->set_servo_out(quadplane.forward_throttle_pct());
channel_throttle->calc_pwm();
} else {
// normal throttle calculation based on servo_out
@ -1070,8 +1093,8 @@ void Plane::set_servos(void)
flap_slew_limit(last_auto_flap, auto_flap_percent);
flap_slew_limit(last_manual_flap, manual_flap_percent);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent);
if (control_mode >= FLY_BY_WIRE_B ||
quadplane.in_assisted_flight() ||
@ -1083,16 +1106,16 @@ void Plane::set_servos(void)
if (control_mode == TRAINING) {
// copy rudder in training mode
channel_rudder->radio_out = channel_rudder->radio_in;
channel_rudder->set_radio_out(channel_rudder->get_radio_in());
}
if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) {
flaperon_update(auto_flap_percent);
}
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out);
channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder);
} else if (g.elevon_output != MIXING_DISABLED) {
channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out);
channel_output_mixer(g.elevon_output, channel_pitch, channel_roll);
}
if (!arming.is_armed()) {
@ -1105,12 +1128,12 @@ void Plane::set_servos(void)
break;
case AP_Arming::YES_ZERO_PWM:
channel_throttle->radio_out = 0;
channel_throttle->set_radio_out(0);
break;
case AP_Arming::YES_MIN_PWM:
default:
channel_throttle->radio_out = throttle_min();
channel_throttle->set_radio_out(throttle_min());
break;
}
}
@ -1143,9 +1166,9 @@ void Plane::set_servos(void)
// after an auto land and auto disarm, set the servos to be neutral just
// in case we're upside down or some crazy angle and straining the servos.
if (g.land_then_servos_neutral == 1) {
channel_roll->radio_out = channel_roll->radio_trim;
channel_pitch->radio_out = channel_pitch->radio_trim;
channel_rudder->radio_out = channel_rudder->radio_trim;
channel_roll->set_radio_out(channel_roll->get_radio_trim());
channel_pitch->set_radio_out(channel_pitch->get_radio_trim());
channel_rudder->set_radio_out(channel_rudder->get_radio_trim());
} else if (g.land_then_servos_neutral == 2) {
channel_roll->disable_out();
channel_pitch->disable_out();

18
ArduPlane/GCS_Mavlink.cpp
View File

@ -309,7 +309,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
}
#endif
if (aparm.throttle_min < 0 && channel_throttle->servo_out < 0) {
if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
@ -424,14 +424,14 @@ void Plane::send_radio_out(mavlink_channel_t chan)
chan,
micros(),
0, // port
RC_Channel::rc_channel(0)->radio_out,
RC_Channel::rc_channel(1)->radio_out,
RC_Channel::rc_channel(2)->radio_out,
RC_Channel::rc_channel(3)->radio_out,
RC_Channel::rc_channel(4)->radio_out,
RC_Channel::rc_channel(5)->radio_out,
RC_Channel::rc_channel(6)->radio_out,
RC_Channel::rc_channel(7)->radio_out);
RC_Channel::rc_channel(0)->get_radio_out(),
RC_Channel::rc_channel(1)->get_radio_out(),
RC_Channel::rc_channel(2)->get_radio_out(),
RC_Channel::rc_channel(3)->get_radio_out(),
RC_Channel::rc_channel(4)->get_radio_out(),
RC_Channel::rc_channel(5)->get_radio_out(),
RC_Channel::rc_channel(6)->get_radio_out(),
RC_Channel::rc_channel(7)->get_radio_out());
return;
}
#endif

6
ArduPlane/Log.cpp
View File

@ -260,8 +260,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->servo_out,
rudder_out : (int16_t)channel_rudder->servo_out,
throttle_out : (int16_t)channel_throttle->get_servo_out(),
rudder_out : (int16_t)channel_rudder->get_servo_out(),
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -417,7 +417,7 @@ struct PACKED log_Arm_Disarm {
void Plane::Log_Write_Current()
{
DataFlash.Log_Write_Current(battery, channel_throttle->control_in);
DataFlash.Log_Write_Current(battery, channel_throttle->get_control_in());
// also write power status
DataFlash.Log_Write_Power();

6
ArduPlane/Plane.h
View File

@ -994,7 +994,8 @@ private:
bool stick_mixing_enabled(void);
void stabilize_roll(float speed_scaler);
void stabilize_pitch(float speed_scaler);
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
static void stick_mix_channel(RC_Channel *channel);
static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
void stabilize_stick_mixing_direct();
void stabilize_stick_mixing_fbw();
void stabilize_yaw(float speed_scaler);
@ -1006,7 +1007,8 @@ private:
void throttle_slew_limit(int16_t last_throttle);
void flap_slew_limit(int8_t &last_value, int8_t &new_value);
bool suppress_throttle(void);
void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out);
void channel_output_mixer(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
void channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const;
void flaperon_update(int8_t flap_percent);
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);

2
ArduPlane/arming_checks.cpp
View File

@ -54,7 +54,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool report)
if (plane.channel_throttle->get_reverse() &&
plane.g.throttle_fs_enabled &&
plane.g.throttle_fs_value <
plane.channel_throttle->radio_max) {
plane.channel_throttle->get_radio_max()) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Invalid THR_FS_VALUE for rev throttle");
}

24
ArduPlane/failsafe.cpp
View File

@ -57,12 +57,12 @@ void Plane::failsafe_check(void)
// pass RC inputs to outputs every 20ms
hal.rcin->clear_overrides();
channel_roll->radio_out = channel_roll->read();
channel_pitch->radio_out = channel_pitch->read();
channel_roll->set_radio_out(channel_roll->read());
channel_pitch->set_radio_out(channel_pitch->read());
if (hal.util->get_soft_armed()) {
channel_throttle->radio_out = channel_throttle->read();
channel_throttle->set_radio_out(channel_throttle->read());
}
channel_rudder->radio_out = channel_rudder->read();
channel_rudder->set_radio_out(channel_rudder->read());
int16_t roll = channel_roll->pwm_to_angle_dz(0);
int16_t pitch = channel_pitch->pwm_to_angle_dz(0);
@ -70,15 +70,15 @@ void Plane::failsafe_check(void)
// setup secondary output channels that don't have
// corresponding input channels
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, roll);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, pitch);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, rudder);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, rudder);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, roll);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, pitch);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, rudder);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, rudder);
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out);
channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder);
} else if (g.elevon_output != MIXING_DISABLED) {
channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out);
channel_output_mixer(g.elevon_output, channel_pitch, channel_roll);
}
#if OBC_FAILSAFE == ENABLED
@ -102,8 +102,8 @@ void Plane::failsafe_check(void)
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input, true);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, 0);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, 0);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, 0);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, 0);
// setup flaperons
flaperon_update(0);

4
ArduPlane/motor_test.cpp
View File

@ -48,7 +48,7 @@ void QuadPlane::motor_test_output()
case MOTOR_TEST_THROTTLE_PERCENT:
// sanity check motor_test.throttle value
if (motor_test.throttle_value <= 100) {
pwm = plane.channel_throttle->radio_min + (plane.channel_throttle->radio_max - plane.channel_throttle->radio_min) * (float)motor_test.throttle_value/100.0f;
pwm = plane.channel_throttle->get_radio_min() + (plane.channel_throttle->get_radio_max() - plane.channel_throttle->get_radio_min()) * (float)motor_test.throttle_value/100.0f;
}
break;
@ -57,7 +57,7 @@ void QuadPlane::motor_test_output()
break;
case MOTOR_TEST_THROTTLE_PILOT:
pwm = plane.channel_throttle->radio_in;
pwm = plane.channel_throttle->get_radio_in();
break;
default:

6
ArduPlane/navigation.cpp
View File

@ -94,7 +94,7 @@ void Plane::calc_airspeed_errors()
control_mode == CRUISE) {
target_airspeed_cm = ((int32_t)(aparm.airspeed_max -
aparm.airspeed_min) *
channel_throttle->control_in) +
channel_throttle->get_control_in()) +
((int32_t)aparm.airspeed_min * 100);
}
@ -203,7 +203,7 @@ void Plane::update_loiter(uint16_t radius)
void Plane::update_cruise()
{
if (!cruise_state.locked_heading &&
channel_roll->control_in == 0 &&
channel_roll->get_control_in() == 0 &&
rudder_input == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
gps.ground_speed() >= 3 &&
@ -240,7 +240,7 @@ void Plane::update_fbwb_speed_height(void)
{
static float last_elevator_input;
float elevator_input;
elevator_input = channel_pitch->control_in / 4500.0f;
elevator_input = channel_pitch->get_control_in() / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;

22
ArduPlane/px4_mixer.cpp
View File

@ -136,24 +136,24 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
// channels and adjust for trims
const RC_Channel *chan1 = RC_Channel::rc_channel(i);
const RC_Channel *chan2 = RC_Channel::rc_channel(c1);
int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->radio_trim);
int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->radio_trim);
int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim());
int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim());
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
// if the input and output channels are the same then we
// apply clipping. This allows for direct pass-thru
int32_t limit = (c1==i?scale_max2:scale_max1);
int32_t in_scale_low;
if (chan2_trim <= chan2->radio_min) {
if (chan2_trim <= chan2->get_radio_min()) {
in_scale_low = scale_max1;
} else {
in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->radio_min);
in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->get_radio_min());
}
int32_t in_scale_high;
if (chan2->radio_max <= chan2_trim) {
if (chan2->get_radio_max() <= chan2_trim) {
in_scale_high = scale_max1;
} else {
in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->radio_max - chan2_trim);
in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->get_radio_max() - chan2_trim);
}
if (chan1->get_reverse() != chan2->get_reverse()) {
in_scale_low = -in_scale_low;
@ -161,8 +161,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
}
if (!print_buffer(buf, buf_size, "M: 1\n") ||
!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
(int)(pwm_scale*(chan1_trim - chan1->radio_min)),
(int)(pwm_scale*(chan1->radio_max - chan1_trim)),
(int)(pwm_scale*(chan1_trim - chan1->get_radio_min())),
(int)(pwm_scale*(chan1->get_radio_max() - chan1_trim)),
(int)(pwm_scale*(chan1_trim - 1500)),
(int)-scale_max2, (int)scale_max2) ||
!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1,
@ -175,8 +175,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
} else {
const RC_Channel *chan1 = RC_Channel::rc_channel(c1);
const RC_Channel *chan2 = RC_Channel::rc_channel(c2);
int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->radio_trim);
int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->radio_trim);
int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim());
int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim());
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
// mix of two input channels to give an output channel. To
@ -315,7 +315,7 @@ bool Plane::setup_failsafe_mixing(void)
// by small numbers near RC3_MIN
config.rc_trim = 1500;
} else {
config.rc_trim = constrain_int16(ch->radio_trim, config.rc_min+1, config.rc_max-1);
config.rc_trim = constrain_int16(ch->get_radio_trim(), config.rc_min+1, config.rc_max-1);
}
config.rc_dz = 0; // zero for the purposes of manual takeover

22
ArduPlane/quadplane.cpp
View File

@ -460,7 +460,7 @@ void QuadPlane::hold_stabilize(float throttle_in)
// quadplane stabilize mode
void QuadPlane::control_stabilize(void)
{
float pilot_throttle_scaled = plane.channel_throttle->control_in / 100.0f;
float pilot_throttle_scaled = plane.channel_throttle->get_control_in() / 100.0f;
hold_stabilize(pilot_throttle_scaled);
}
@ -622,8 +622,8 @@ void QuadPlane::control_loiter()
pos_control->set_accel_z(pilot_accel_z);
// process pilot's roll and pitch input
wp_nav->set_pilot_desired_acceleration(plane.channel_roll->control_in,
plane.channel_pitch->control_in);
wp_nav->set_pilot_desired_acceleration(plane.channel_roll->get_control_in(),
plane.channel_pitch->get_control_in());
// Update EKF speed limit - used to limit speed when we are using optical flow
float ekfGndSpdLimit, ekfNavVelGainScaler;
@ -666,7 +666,7 @@ void QuadPlane::control_loiter()
*/
float QuadPlane::get_pilot_input_yaw_rate_cds(void)
{
if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) {
if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) {
// the user may be trying to disarm
return 0;
}
@ -685,7 +685,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void)
// use bank angle to get desired yaw rate
yaw_cds += desired_auto_yaw_rate_cds();
}
if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) {
if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) {
// the user may be trying to disarm
return 0;
}
@ -706,7 +706,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void)
return -50;
}
uint16_t dead_zone = plane.channel_throttle->get_dead_zone();
uint16_t trim = (plane.channel_throttle->radio_max + plane.channel_throttle->radio_min)/2;
uint16_t trim = (plane.channel_throttle->get_radio_max() + plane.channel_throttle->get_radio_min())/2;
return pilot_velocity_z_max * plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) / 100.0f;
}
@ -716,7 +716,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void)
*/
void QuadPlane::init_throttle_wait(void)
{
if (plane.channel_throttle->control_in >= 10 ||
if (plane.channel_throttle->get_control_in() >= 10 ||
plane.is_flying()) {
throttle_wait = false;
} else {
@ -749,7 +749,7 @@ float QuadPlane::assist_climb_rate_cms(void)
} else {
// otherwise estimate from pilot input
climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd);
climb_rate *= plane.channel_throttle->control_in;
climb_rate *= plane.channel_throttle->get_control_in();
}
climb_rate = constrain_float(climb_rate, -wp_nav->get_speed_down(), wp_nav->get_speed_up());
return climb_rate;
@ -794,7 +794,7 @@ void QuadPlane::update_transition(void)
*/
if (have_airspeed && aspeed < assist_speed &&
(plane.auto_throttle_mode ||
plane.channel_throttle->control_in>0 ||
plane.channel_throttle->get_control_in()>0 ||
plane.is_flying())) {
// the quad should provide some assistance to the plane
transition_state = TRANSITION_AIRSPEED_WAIT;
@ -894,7 +894,7 @@ void QuadPlane::update(void)
// disable throttle_wait when throttle rises above 10%
if (throttle_wait &&
(plane.channel_throttle->control_in > 10 ||
(plane.channel_throttle->get_control_in() > 10 ||
plane.failsafe.ch3_failsafe ||
plane.failsafe.ch3_counter>0)) {
throttle_wait = false;
@ -1620,7 +1620,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
weathervane.last_output = 0;
return 0;
}
if (plane.channel_rudder->control_in != 0) {
if (plane.channel_rudder->get_control_in() != 0) {
weathervane.last_pilot_input_ms = AP_HAL::millis();
weathervane.last_output = 0;
return 0;

79
ArduPlane/radio.cpp
View File

@ -39,7 +39,7 @@ void Plane::set_control_channels(void)
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
// take a proportion of speed
hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max);
hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
}
/*
@ -67,7 +67,7 @@ void Plane::init_rc_out()
configuration error where the user sets CH3_TRIM incorrectly and
the motor may start on power up
*/
channel_throttle->radio_trim = throttle_min();
channel_throttle->set_radio_trim (throttle_min());
if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) {
channel_throttle->enable_out();
@ -102,7 +102,7 @@ void Plane::rudder_arm_disarm_check()
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (channel_throttle->control_in != 0){
if (channel_throttle->get_control_in() != 0){
rudder_arm_timer = 0;
return;
}
@ -115,7 +115,7 @@ void Plane::rudder_arm_disarm_check()
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->control_in > 4000) {
if (channel_rudder->get_control_in() > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
@ -135,7 +135,7 @@ void Plane::rudder_arm_disarm_check()
}
} else if (arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->control_in < -4000) {
if (channel_rudder->get_control_in() < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
@ -158,7 +158,7 @@ void Plane::rudder_arm_disarm_check()
void Plane::read_radio()
{
if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->radio_in);
control_failsafe(channel_throttle->get_radio_in());
return;
}
@ -177,8 +177,10 @@ void Plane::read_radio()
pwm_roll = elevon.ch1_temp;
pwm_pitch = elevon.ch2_temp;
}else{
pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2)
- BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2)
+ BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
}
RC_Channel::set_pwm_all();
@ -195,12 +197,12 @@ void Plane::read_radio()
channel_pitch->set_pwm(pwm_pitch);
}
control_failsafe(channel_throttle->radio_in);
control_failsafe(channel_throttle->get_radio_in());
channel_throttle->servo_out = channel_throttle->control_in;
channel_throttle->set_servo_out(channel_throttle->get_control_in());
if (g.throttle_nudge && channel_throttle->servo_out > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->servo_out - 50) * 0.02f;
if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {
@ -218,7 +220,7 @@ void Plane::read_radio()
// attitude from the roll channel.
rudder_input = 0;
} else {
rudder_input = channel_rudder->control_in;
rudder_input = channel_rudder->get_control_in();
}
// check for transmitter tuning changes
@ -230,17 +232,16 @@ void Plane::control_failsafe(uint16_t pwm)
if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
// we do not have valid RC input. Set all primary channel
// control inputs to the trim value and throttle to min
channel_roll->radio_in = channel_roll->radio_trim;
channel_pitch->radio_in = channel_pitch->radio_trim;
channel_rudder->radio_in = channel_rudder->radio_trim;
channel_roll->set_radio_in(channel_roll->get_radio_trim());
channel_pitch->set_radio_in(channel_pitch->get_radio_trim());
channel_rudder->set_radio_in(channel_rudder->get_radio_trim());
// note that we don't set channel_throttle->radio_in to radio_trim,
// as that would cause throttle failsafe to not activate
channel_roll->control_in = 0;
channel_pitch->control_in = 0;
channel_rudder->control_in = 0;
channel_throttle->control_in = 0;
channel_roll->set_control_in(0);
channel_pitch->set_control_in(0);
channel_rudder->set_control_in(0);
channel_throttle->set_control_in(0);
}
if(g.throttle_fs_enabled == 0)
@ -280,12 +281,12 @@ void Plane::control_failsafe(uint16_t pwm)
void Plane::trim_control_surfaces()
{
read_radio();
int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5;
int16_t trim_pitch_range = (channel_pitch->radio_max - channel_pitch->radio_min)/5;
if (channel_roll->radio_in < channel_roll->radio_min+trim_roll_range ||
channel_roll->radio_in > channel_roll->radio_max-trim_roll_range ||
channel_pitch->radio_in < channel_pitch->radio_min+trim_pitch_range ||
channel_pitch->radio_in > channel_pitch->radio_max-trim_pitch_range) {
int16_t trim_roll_range = (channel_roll->get_radio_max() - channel_roll->get_radio_min())/5;
int16_t trim_pitch_range = (channel_pitch->get_radio_max() - channel_pitch->get_radio_min())/5;
if (channel_roll->get_radio_in() < channel_roll->get_radio_min()+trim_roll_range ||
channel_roll->get_radio_in() > channel_roll->get_radio_max()-trim_roll_range ||
channel_pitch->get_radio_in() < channel_pitch->get_radio_min()+trim_pitch_range ||
channel_pitch->get_radio_in() > channel_pitch->get_radio_max()-trim_pitch_range) {
// don't trim for extreme values - if we attempt to trim so
// there is less than 20 percent range left then assume the
// sticks are not properly centered. This also prevents
@ -296,18 +297,18 @@ void Plane::trim_control_surfaces()
// Store control surface trim values
// ---------------------------------
if(g.mix_mode == 0) {
if (channel_roll->radio_in != 0) {
channel_roll->radio_trim = channel_roll->radio_in;
if (channel_roll->get_radio_in() != 0) {
channel_roll->set_radio_trim(channel_roll->get_radio_in());
}
if (channel_pitch->radio_in != 0) {
channel_pitch->radio_trim = channel_pitch->radio_in;
if (channel_pitch->get_radio_in() != 0) {
channel_pitch->set_radio_trim(channel_pitch->get_radio_in());
}
// the secondary aileron/elevator is trimmed only if it has a
// corresponding transmitter input channel, which k_aileron
// doesn't have
RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron_with_input);
RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_elevator_with_input);
RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_aileron_with_input);
RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_elevator_with_input);
} else{
if (elevon.ch1_temp != 0) {
elevon.trim1 = elevon.ch1_temp;
@ -318,11 +319,11 @@ void Plane::trim_control_surfaces()
//Recompute values here using new values for elevon1_trim and elevon2_trim
//We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed
uint16_t center = 1500;
channel_roll->radio_trim = center;
channel_pitch->radio_trim = center;
channel_roll->set_radio_trim(center);
channel_pitch->set_radio_trim(center);
}
if (channel_rudder->radio_in != 0) {
channel_rudder->radio_trim = channel_rudder->radio_in;
if (channel_rudder->get_radio_in() != 0) {
channel_rudder->set_radio_trim(channel_rudder->get_radio_in());
}
// save to eeprom
@ -354,7 +355,7 @@ bool Plane::rc_failsafe_active(void)
return true;
}
if (channel_throttle->get_reverse()) {
return channel_throttle->radio_in >= g.throttle_fs_value;
return channel_throttle->get_radio_in() >= g.throttle_fs_value;
}
return channel_throttle->radio_in <= g.throttle_fs_value;
return channel_throttle->get_radio_in() <= g.throttle_fs_value;
}

2
ArduPlane/system.cpp
View File

@ -738,7 +738,7 @@ void Plane::servo_write(uint8_t ch, uint16_t pwm)
#if HIL_SUPPORT
if (g.hil_mode==1 && !g.hil_servos) {
if (ch < 8) {
RC_Channel::rc_channel(ch)->radio_out = pwm;
RC_Channel::rc_channel(ch)->set_radio_out(pwm);
}
return;
}

40
ArduPlane/test.cpp
View File

@ -60,14 +60,14 @@ int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
read_radio();
cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
(int)channel_roll->radio_in,
(int)channel_pitch->radio_in,
(int)channel_throttle->radio_in,
(int)channel_rudder->radio_in,
(int)g.rc_5.radio_in,
(int)g.rc_6.radio_in,
(int)g.rc_7.radio_in,
(int)g.rc_8.radio_in);
(int)channel_roll->get_radio_in(),
(int)channel_pitch->get_radio_in(),
(int)channel_throttle->get_radio_in(),
(int)channel_rudder->get_radio_in(),
(int)g.rc_5.get_radio_in(),
(int)g.rc_6.get_radio_in(),
(int)g.rc_7.get_radio_in(),
(int)g.rc_8.get_radio_in());
if(cliSerial->available() > 0) {
return (0);
@ -124,14 +124,14 @@ int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
set_servos();
cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
(int)channel_roll->control_in,
(int)channel_pitch->control_in,
(int)channel_throttle->control_in,
(int)channel_rudder->control_in,
(int)g.rc_5.control_in,
(int)g.rc_6.control_in,
(int)g.rc_7.control_in,
(int)g.rc_8.control_in);
(int)channel_roll->get_control_in(),
(int)channel_pitch->get_control_in(),
(int)channel_throttle->get_control_in(),
(int)channel_rudder->get_control_in(),
(int)g.rc_5.get_control_in(),
(int)g.rc_6.get_control_in(),
(int)g.rc_7.get_control_in(),
(int)g.rc_8.get_control_in() );
if(cliSerial->available() > 0) {
return (0);
@ -155,7 +155,7 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch();
cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n");
while(channel_throttle->control_in > 0) {
while(channel_throttle->get_control_in() > 0) {
hal.scheduler->delay(20);
read_radio();
}
@ -164,8 +164,8 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
hal.scheduler->delay(20);
read_radio();
if(channel_throttle->control_in > 0) {
cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->control_in);
if(channel_throttle->get_control_in() > 0) {
cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->get_control_in());
fail_test++;
}
@ -177,7 +177,7 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
}
if(rc_failsafe_active()) {
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->radio_in);
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->get_radio_in());
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;