@ -33,16 +33,16 @@ RC_Channel_aux::output_ch(void)
@@ -33,16 +33,16 @@ RC_Channel_aux::output_ch(void)
case k_none : // disabled
return ;
case k_manual : // manual
radio_out = radio_in ;
set_radio_out ( get_radio_in ( ) ) ;
break ;
case k_rcin1 . . . k_rcin16 : // rc pass-thru
radio_out = hal . rcin - > read ( function - k_rcin1 ) ;
set_radio_out ( hal . rcin - > read ( function - k_rcin1 ) ) ;
break ;
case k_motor1 . . . k_motor8 :
// handled by AP_Motors::rc_write()
return ;
}
hal . rcout - > write ( _ch_out , radio_out ) ;
hal . rcout - > write ( _ch_out , get_ radio_out( ) ) ;
}
/*
@ -153,13 +153,13 @@ void RC_Channel_aux::enable_aux_servos()
@@ -153,13 +153,13 @@ void RC_Channel_aux::enable_aux_servos()
// trim value on startup
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] ) {
RC_Channel_aux : : Aux_servo_function_t function = ( RC_Channel_aux : : Aux_servo_function_t ) _aux_channels [ i ] - > function . get ( ) ;
// see if it is a valid function
if ( function < RC_Channel_aux : : k_nr_aux_servo_functions ) {
_aux_channels [ i ] - > enable_out ( ) ;
}
}
}
RC_Channel_aux : : Aux_servo_function_t function = ( RC_Channel_aux : : Aux_servo_function_t ) _aux_channels [ i ] - > function . get ( ) ;
// see if it is a valid function
if ( function < RC_Channel_aux : : k_nr_aux_servo_functions ) {
_aux_channels [ i ] - > enable_out ( ) ;
}
}
}
}
/*
@ -173,9 +173,9 @@ RC_Channel_aux::set_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t
@@ -173,9 +173,9 @@ RC_Channel_aux::set_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
_aux_channels [ i ] - > radio_out = constrain_int16 ( value , _aux_channels [ i ] - > radio_min , _aux_channels [ i ] - > radio_max ) ;
_aux_channels [ i ] - > set_radio_out ( constrain_int16 ( value , _aux_channels [ i ] - > get_ radio_min( ) , _aux_channels [ i ] - > get_ radio_max( ) ) ) ;
_aux_channels [ i ] - > output ( ) ;
}
}
}
}
@ -190,10 +190,10 @@ RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function,
@@ -190,10 +190,10 @@ RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function,
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
int16_t value2 = value - 1500 + _aux_channels [ i ] - > radio_trim ;
_aux_channels [ i ] - > radio_out = constrain_int16 ( value2 , _aux_channels [ i ] - > radio_min , _aux_channels [ i ] - > radio_max ) ;
int16_t value2 = value - 1500 + _aux_channels [ i ] - > get_ radio_trim( ) ;
_aux_channels [ i ] - > set_radio_out ( constrain_int16 ( value2 , _aux_channels [ i ] - > get_ radio_min( ) , _aux_channels [ i ] - > get_ radio_max( ) ) ) ;
_aux_channels [ i ] - > output ( ) ;
}
}
}
}
@ -202,18 +202,18 @@ RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function,
@@ -202,18 +202,18 @@ RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function,
the given function type
*/
void
RC_Channel_aux : : set_radio_trim ( RC_Channel_aux : : Aux_servo_function_t function )
RC_Channel_aux : : set_trim_to_radio_in_for ( RC_Channel_aux : : Aux_servo_function_t function )
{
if ( ! function_assigned ( function ) ) {
return ;
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
if ( _aux_channels [ i ] - > radio_in ! = 0 ) {
_aux_channels [ i ] - > radio_trim = _aux_channels [ i ] - > radio_in ;
_aux_channels [ i ] - > radio_trim . save ( ) ;
}
}
if ( _aux_channels [ i ] - > get_ radio_in( ) ! = 0 ) {
_aux_channels [ i ] - > set_radio_trim ( _aux_channels [ i ] - > get_ radio_in( ) ) ;
_aux_channels [ i ] - > save_ radio_trim( ) ;
}
}
}
}
@ -228,9 +228,9 @@ RC_Channel_aux::set_radio_to_min(RC_Channel_aux::Aux_servo_function_t function)
@@ -228,9 +228,9 @@ RC_Channel_aux::set_radio_to_min(RC_Channel_aux::Aux_servo_function_t function)
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
_aux_channels [ i ] - > radio_out = _aux_channels [ i ] - > radio_min ;
_aux_channels [ i ] - > set_radio_out ( _aux_channels [ i ] - > get_ radio_min( ) ) ;
_aux_channels [ i ] - > output ( ) ;
}
}
}
}
@ -245,7 +245,7 @@ RC_Channel_aux::set_radio_to_max(RC_Channel_aux::Aux_servo_function_t function)
@@ -245,7 +245,7 @@ RC_Channel_aux::set_radio_to_max(RC_Channel_aux::Aux_servo_function_t function)
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
_aux_channels [ i ] - > radio_out = _aux_channels [ i ] - > radio_max ;
_aux_channels [ i ] - > set_radio_out ( _aux_channels [ i ] - > get_ radio_max( ) ) ;
_aux_channels [ i ] - > output ( ) ;
}
}
@ -262,9 +262,9 @@ RC_Channel_aux::set_radio_to_trim(RC_Channel_aux::Aux_servo_function_t function)
@@ -262,9 +262,9 @@ RC_Channel_aux::set_radio_to_trim(RC_Channel_aux::Aux_servo_function_t function)
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
_aux_channels [ i ] - > radio_out = _aux_channels [ i ] - > radio_trim ;
_aux_channels [ i ] - > set_radio_out ( _aux_channels [ i ] - > get_ radio_trim( ) ) ;
_aux_channels [ i ] - > output ( ) ;
}
}
}
}
@ -279,14 +279,14 @@ RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function,
@@ -279,14 +279,14 @@ RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function,
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
if ( do_input_output ) {
_aux_channels [ i ] - > input ( ) ;
}
_aux_channels [ i ] - > radio_out = _aux_channels [ i ] - > radio_in ;
if ( do_input_output ) {
_aux_channels [ i ] - > output ( ) ;
}
}
if ( do_input_output ) {
_aux_channels [ i ] - > input ( ) ;
}
_aux_channels [ i ] - > set_radio_out ( _aux_channels [ i ] - > get_ radio_in( ) ) ;
if ( do_input_output ) {
_aux_channels [ i ] - > output ( ) ;
}
}
}
}
@ -294,17 +294,17 @@ RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function,
@@ -294,17 +294,17 @@ RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function,
set servo_out and call calc_pwm ( ) for a given function
*/
void
RC_Channel_aux : : set_servo_out ( RC_Channel_aux : : Aux_servo_function_t function , int16_t value )
RC_Channel_aux : : set_servo_out_for ( RC_Channel_aux : : Aux_servo_function_t function , int16_t value )
{
if ( ! function_assigned ( function ) ) {
return ;
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
_aux_channels [ i ] - > servo_out = value ;
_aux_channels [ i ] - > calc_pwm ( ) ;
_aux_channels [ i ] - > set_servo_out ( value ) ;
_aux_channels [ i ] - > calc_pwm ( ) ;
_aux_channels [ i ] - > output ( ) ;
}
}
}
}
@ -356,15 +356,15 @@ RC_Channel_aux::set_servo_limit(RC_Channel_aux::Aux_servo_function_t function, R
@@ -356,15 +356,15 @@ RC_Channel_aux::set_servo_limit(RC_Channel_aux::Aux_servo_function_t function, R
RC_Channel_aux * ch = _aux_channels [ i ] ;
if ( ch & & ch - > function . get ( ) = = function ) {
uint16_t pwm = ch - > get_limit_pwm ( limit ) ;
ch - > radio_out = pwm ;
ch - > set_radio_out ( pwm ) ;
if ( ch - > function . get ( ) = = k_manual ) {
// in order for output_ch() to work for k_manual we
// also have to override radio_in
ch - > radio_in = pwm ;
ch - > set_radio_in ( pwm ) ;
}
if ( ch - > function . get ( ) > = k_rcin1 & & ch - > function . get ( ) < = k_rcin16 ) {
// save for k_rcin*
ch - > radio_in = pwm ;
ch - > set_radio_in ( pwm ) ;
}
}
}
@ -379,7 +379,7 @@ RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t function)
@@ -379,7 +379,7 @@ RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t function)
if ( function < k_nr_aux_servo_functions ) {
return ( _function_mask & ( 1ULL < < function ) ) ! = 0 ;
}
return false ;
return false ;
}
/*
@ -395,7 +395,7 @@ RC_Channel_aux::move_servo(RC_Channel_aux::Aux_servo_function_t function,
@@ -395,7 +395,7 @@ RC_Channel_aux::move_servo(RC_Channel_aux::Aux_servo_function_t function,
}
for ( uint8_t i = 0 ; i < RC_AUX_MAX_CHANNELS ; i + + ) {
if ( _aux_channels [ i ] & & _aux_channels [ i ] - > function . get ( ) = = function ) {
_aux_channels [ i ] - > servo_out = value ;
_aux_channels [ i ] - > set_servo_out ( value ) ;
_aux_channels [ i ] - > set_range ( angle_min , angle_max ) ;
_aux_channels [ i ] - > calc_pwm ( ) ;
_aux_channels [ i ] - > output ( ) ;