@ -21,8 +21,8 @@ float Plane::get_speed_scaler(void)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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()
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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()
@@ -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()
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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_
@@ -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)
@@ -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)
@@ -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)
@@ -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
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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 ( ) ;