@ -32,7 +32,7 @@ extern const AP_HAL::HAL& hal;
@@ -32,7 +32,7 @@ extern const AP_HAL::HAL& hal;
/// global array with pointers to all APM RC channels, will be used by AP_Mount
/// and AP_Camera classes / It points to RC input channels.
RC_Channel * RC_Channel : : rc_ch [ RC_MAX_CHANNELS ] ;
RC_Channel * RC_Channel : : _ rc_ch[ RC_MAX_CHANNELS ] ;
const AP_Param : : GroupInfo RC_Channel : : var_info [ ] = {
// @Param: MIN
@ -42,7 +42,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
@@ -42,7 +42,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
// @Range: 800 2200
// @Increment: 1
// @User: Advanced
AP_GROUPINFO_FLAGS ( " MIN " , 0 , RC_Channel , radio_min , 1100 , AP_PARAM_NO_SHIFT ) ,
AP_GROUPINFO_FLAGS ( " MIN " , 0 , RC_Channel , _ radio_min, 1100 , AP_PARAM_NO_SHIFT ) ,
// @Param: TRIM
// @DisplayName: RC trim PWM
@ -51,7 +51,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
@@ -51,7 +51,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
// @Range: 800 2200
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " TRIM " , 1 , RC_Channel , radio_trim , 1500 ) ,
AP_GROUPINFO ( " TRIM " , 1 , RC_Channel , _ radio_trim, 1500 ) ,
// @Param: MAX
// @DisplayName: RC max PWM
@ -60,7 +60,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
@@ -60,7 +60,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
// @Range: 800 2200
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " MAX " , 2 , RC_Channel , radio_max , 1900 ) ,
AP_GROUPINFO ( " MAX " , 2 , RC_Channel , _ radio_max, 1900 ) ,
// @Param: REV
// @DisplayName: RC reverse
@ -175,20 +175,20 @@ RC_Channel::set_type_out(uint8_t t)
@@ -175,20 +175,20 @@ RC_Channel::set_type_out(uint8_t t)
void
RC_Channel : : trim ( )
{
radio_trim = radio_in ;
_ radio_trim = _ radio_in;
}
// read input from APM_RC - create a control_in value
void
RC_Channel : : set_pwm ( int16_t pwm )
{
radio_in = pwm ;
_ radio_in = pwm ;
if ( _type_in = = RC_CHANNEL_TYPE_RANGE ) {
control_in = pwm_to_range ( ) ;
_ control_in = pwm_to_range ( ) ;
} else {
//RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
control_in = pwm_to_angle ( ) ;
_ control_in = pwm_to_angle ( ) ;
}
}
@ -199,8 +199,8 @@ void
@@ -199,8 +199,8 @@ void
RC_Channel : : set_pwm_all ( void )
{
for ( uint8_t i = 0 ; i < RC_MAX_CHANNELS ; i + + ) {
if ( rc_ch [ i ] ! = NULL ) {
rc_ch [ i ] - > set_pwm ( rc_ch [ i ] - > read ( ) ) ;
if ( _ rc_ch[ i ] ! = NULL ) {
_ rc_ch[ i ] - > set_pwm ( _ rc_ch[ i ] - > read ( ) ) ;
}
}
}
@ -211,13 +211,13 @@ RC_Channel::set_pwm_all(void)
@@ -211,13 +211,13 @@ RC_Channel::set_pwm_all(void)
void
RC_Channel : : set_pwm_no_deadzone ( int16_t pwm )
{
radio_in = pwm ;
_ radio_in = pwm ;
if ( _type_in = = RC_CHANNEL_TYPE_RANGE ) {
control_in = pwm_to_range_dz ( 0 ) ;
_ control_in = pwm_to_range_dz ( 0 ) ;
} else {
//RC_CHANNEL_ANGLE, RC_CHANNEL_ANGLE_RAW
control_in = pwm_to_angle_dz ( 0 ) ;
_ control_in = pwm_to_angle_dz ( 0 ) ;
}
}
@ -226,20 +226,20 @@ void
@@ -226,20 +226,20 @@ void
RC_Channel : : calc_pwm ( void )
{
if ( _type_out = = RC_CHANNEL_TYPE_RANGE ) {
pwm_out = range_to_pwm ( ) ;
radio_out = ( _reverse > = 0 ) ? ( radio_min + pwm_out ) : ( radio_max - pwm_out ) ;
_ pwm_out = range_to_pwm ( ) ;
_ radio_out = ( _reverse > = 0 ) ? ( _ radio_min + _ pwm_out) : ( _ radio_max - _ pwm_out) ;
} else if ( _type_out = = RC_CHANNEL_TYPE_ANGLE_RAW ) {
pwm_out = ( float ) servo_out * 0.1f ;
_ pwm_out = ( float ) _ servo_out * 0.1f ;
int16_t reverse_mul = ( _reverse = = - 1 ? - 1 : 1 ) ;
radio_out = ( pwm_out * reverse_mul ) + radio_trim ;
_ radio_out = ( _ pwm_out * reverse_mul ) + _ radio_trim;
} else { // RC_CHANNEL_TYPE_ANGLE
pwm_out = angle_to_pwm ( ) ;
radio_out = pwm_out + radio_trim ;
_ pwm_out = angle_to_pwm ( ) ;
_ radio_out = _ pwm_out + _ radio_trim;
}
radio_out = constrain_int16 ( radio_out , radio_min . get ( ) , radio_max . get ( ) ) ;
_ radio_out = constrain_int16 ( _ radio_out, _ radio_min. get ( ) , _ radio_max. get ( ) ) ;
}
@ -250,15 +250,15 @@ RC_Channel::calc_pwm(void)
@@ -250,15 +250,15 @@ RC_Channel::calc_pwm(void)
int16_t
RC_Channel : : get_control_mid ( ) const {
if ( _type_in = = RC_CHANNEL_TYPE_RANGE ) {
int16_t r_in = ( radio_min . get ( ) + radio_max . get ( ) ) / 2 ;
int16_t r_in = ( _ radio_min. get ( ) + _ radio_max. get ( ) ) / 2 ;
if ( _reverse = = - 1 ) {
r_in = radio_max . get ( ) - ( r_in - radio_min . get ( ) ) ;
r_in = _ radio_max. get ( ) - ( r_in - _ radio_min. get ( ) ) ;
}
int16_t radio_trim_low = radio_min + _dead_zone ;
int16_t radio_trim_low = _ radio_min + _dead_zone ;
return ( _low_in + ( ( int32_t ) ( _high_in - _low_in ) * ( int32_t ) ( r_in - radio_trim_low ) ) / ( int32_t ) ( radio_max - radio_trim_low ) ) ;
return ( _low_in + ( ( int32_t ) ( _high_in - _low_in ) * ( int32_t ) ( r_in - radio_trim_low ) ) / ( int32_t ) ( _ radio_max - radio_trim_low ) ) ;
} else {
return 0 ;
}
@ -269,9 +269,9 @@ RC_Channel::get_control_mid() const {
@@ -269,9 +269,9 @@ RC_Channel::get_control_mid() const {
void
RC_Channel : : load_eeprom ( void )
{
radio_min . load ( ) ;
radio_trim . load ( ) ;
radio_max . load ( ) ;
_ radio_min. load ( ) ;
_ radio_trim. load ( ) ;
_ radio_max. load ( ) ;
_reverse . load ( ) ;
_dead_zone . load ( ) ;
}
@ -279,9 +279,9 @@ RC_Channel::load_eeprom(void)
@@ -279,9 +279,9 @@ RC_Channel::load_eeprom(void)
void
RC_Channel : : save_eeprom ( void )
{
radio_min . save ( ) ;
radio_trim . save ( ) ;
radio_max . save ( ) ;
_ radio_min. save ( ) ;
_ radio_trim. save ( ) ;
_ radio_max. save ( ) ;
_reverse . save ( ) ;
_dead_zone . save ( ) ;
}
@ -291,14 +291,14 @@ RC_Channel::save_eeprom(void)
@@ -291,14 +291,14 @@ RC_Channel::save_eeprom(void)
void
RC_Channel : : zero_min_max ( )
{
radio_min = radio_max = radio_in ;
_ radio_min = _ radio_max = _ radio_in;
}
void
RC_Channel : : update_min_max ( )
{
radio_min = MIN ( radio_min . get ( ) , radio_in ) ;
radio_max = MAX ( radio_max . get ( ) , radio_in ) ;
_ radio_min = MIN ( _ radio_min. get ( ) , _ radio_in) ;
_ radio_max = MAX ( _ radio_max. get ( ) , _ radio_in) ;
}
/*
@ -312,14 +312,14 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim)
@@ -312,14 +312,14 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim)
int16_t radio_trim_low = _trim - dead_zone ;
// prevent div by 0
if ( ( radio_trim_low - radio_min ) = = 0 | | ( radio_max - radio_trim_high ) = = 0 )
if ( ( radio_trim_low - _ radio_min) = = 0 | | ( _ radio_max - radio_trim_high ) = = 0 )
return 0 ;
int16_t reverse_mul = ( _reverse = = - 1 ? - 1 : 1 ) ;
if ( radio_in > radio_trim_high ) {
return reverse_mul * ( ( int32_t ) _high_in * ( int32_t ) ( radio_in - radio_trim_high ) ) / ( int32_t ) ( radio_max - radio_trim_high ) ;
} else if ( radio_in < radio_trim_low ) {
return reverse_mul * ( ( int32_t ) _high_in * ( int32_t ) ( radio_in - radio_trim_low ) ) / ( int32_t ) ( radio_trim_low - radio_min ) ;
if ( _ radio_in > radio_trim_high ) {
return reverse_mul * ( ( int32_t ) _high_in * ( int32_t ) ( _ radio_in - radio_trim_high ) ) / ( int32_t ) ( _ radio_max - radio_trim_high ) ;
} else if ( _ radio_in < radio_trim_low ) {
return reverse_mul * ( ( int32_t ) _high_in * ( int32_t ) ( _ radio_in - radio_trim_low ) ) / ( int32_t ) ( radio_trim_low - _ radio_min) ;
} else
return 0 ;
}
@ -331,7 +331,7 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim)
@@ -331,7 +331,7 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim)
int16_t
RC_Channel : : pwm_to_angle_dz ( uint16_t dead_zone )
{
return pwm_to_angle_dz_trim ( dead_zone , radio_trim ) ;
return pwm_to_angle_dz_trim ( dead_zone , _ radio_trim) ;
}
/*
@ -349,10 +349,10 @@ int16_t
@@ -349,10 +349,10 @@ int16_t
RC_Channel : : angle_to_pwm ( )
{
int16_t reverse_mul = ( _reverse = = - 1 ? - 1 : 1 ) ;
if ( ( servo_out * reverse_mul ) > 0 ) {
return reverse_mul * ( ( int32_t ) servo_out * ( int32_t ) ( radio_max - radio_trim ) ) / ( int32_t ) _high_out ;
if ( ( _ servo_out * reverse_mul ) > 0 ) {
return reverse_mul * ( ( int32_t ) _ servo_out * ( int32_t ) ( _ radio_max - _ radio_trim) ) / ( int32_t ) _high_out ;
} else {
return reverse_mul * ( ( int32_t ) servo_out * ( int32_t ) ( radio_trim - radio_min ) ) / ( int32_t ) _high_out ;
return reverse_mul * ( ( int32_t ) _ servo_out * ( int32_t ) ( _ radio_trim - _ radio_min) ) / ( int32_t ) _high_out ;
}
}
@ -363,16 +363,16 @@ RC_Channel::angle_to_pwm()
@@ -363,16 +363,16 @@ RC_Channel::angle_to_pwm()
int16_t
RC_Channel : : pwm_to_range_dz ( uint16_t dead_zone )
{
int16_t r_in = constrain_int16 ( radio_in , radio_min . get ( ) , radio_max . get ( ) ) ;
int16_t r_in = constrain_int16 ( _ radio_in, _ radio_min. get ( ) , _ radio_max. get ( ) ) ;
if ( _reverse = = - 1 ) {
r_in = radio_max . get ( ) - ( r_in - radio_min . get ( ) ) ;
r_in = _ radio_max. get ( ) - ( r_in - _ radio_min. get ( ) ) ;
}
int16_t radio_trim_low = radio_min + dead_zone ;
int16_t radio_trim_low = _ radio_min + dead_zone ;
if ( r_in > radio_trim_low )
return ( _low_in + ( ( int32_t ) ( _high_in - _low_in ) * ( int32_t ) ( r_in - radio_trim_low ) ) / ( int32_t ) ( radio_max - radio_trim_low ) ) ;
return ( _low_in + ( ( int32_t ) ( _high_in - _low_in ) * ( int32_t ) ( r_in - radio_trim_low ) ) / ( int32_t ) ( _ radio_max - radio_trim_low ) ) ;
else if ( dead_zone > 0 )
return 0 ;
else
@ -394,9 +394,9 @@ int16_t
@@ -394,9 +394,9 @@ int16_t
RC_Channel : : range_to_pwm ( )
{
if ( _high_out = = _low_out ) {
return radio_trim ;
return _ radio_trim;
}
return ( ( int32_t ) ( servo_out - _low_out ) * ( int32_t ) ( radio_max - radio_min ) ) / ( int32_t ) ( _high_out - _low_out ) ;
return ( ( int32_t ) ( _ servo_out - _low_out ) * ( int32_t ) ( _ radio_max - _ radio_min) ) / ( int32_t ) ( _high_out - _low_out ) ;
}
// ------------------------------------------
@ -406,16 +406,16 @@ RC_Channel::norm_input()
@@ -406,16 +406,16 @@ RC_Channel::norm_input()
{
float ret ;
int16_t reverse_mul = ( _reverse = = - 1 ? - 1 : 1 ) ;
if ( radio_in < radio_trim ) {
if ( radio_min > = radio_trim ) {
if ( _ radio_in < _ radio_trim) {
if ( _ radio_min > = _ radio_trim) {
return 0.0f ;
}
ret = reverse_mul * ( float ) ( radio_in - radio_trim ) / ( float ) ( radio_trim - radio_min ) ;
ret = reverse_mul * ( float ) ( _ radio_in - _ radio_trim) / ( float ) ( _ radio_trim - _ radio_min) ;
} else {
if ( radio_max < = radio_trim ) {
if ( _ radio_max < = _ radio_trim) {
return 0.0f ;
}
ret = reverse_mul * ( float ) ( radio_in - radio_trim ) / ( float ) ( radio_max - radio_trim ) ;
ret = reverse_mul * ( float ) ( _ radio_in - _ radio_trim) / ( float ) ( _ radio_max - _ radio_trim) ;
}
return constrain_float ( ret , - 1.0f , 1.0f ) ;
}
@ -423,14 +423,14 @@ RC_Channel::norm_input()
@@ -423,14 +423,14 @@ RC_Channel::norm_input()
float
RC_Channel : : norm_input_dz ( )
{
int16_t dz_min = radio_trim - _dead_zone ;
int16_t dz_max = radio_trim + _dead_zone ;
int16_t dz_min = _ radio_trim - _dead_zone ;
int16_t dz_max = _ radio_trim + _dead_zone ;
float ret ;
int16_t reverse_mul = ( _reverse = = - 1 ? - 1 : 1 ) ;
if ( radio_in < dz_min & & dz_min > radio_min ) {
ret = reverse_mul * ( float ) ( radio_in - dz_min ) / ( float ) ( dz_min - radio_min ) ;
} else if ( radio_in > dz_max & & radio_max > dz_max ) {
ret = reverse_mul * ( float ) ( radio_in - dz_max ) / ( float ) ( radio_max - dz_max ) ;
if ( _ radio_in < dz_min & & dz_min > _ radio_min) {
ret = reverse_mul * ( float ) ( _ radio_in - dz_min ) / ( float ) ( dz_min - _ radio_min) ;
} else if ( _ radio_in > dz_max & & _ radio_max > dz_max ) {
ret = reverse_mul * ( float ) ( _ radio_in - dz_max ) / ( float ) ( _ radio_max - dz_max ) ;
} else {
ret = 0 ;
}
@ -443,13 +443,13 @@ RC_Channel::norm_input_dz()
@@ -443,13 +443,13 @@ RC_Channel::norm_input_dz()
uint8_t
RC_Channel : : percent_input ( )
{
if ( radio_in < = radio_min ) {
if ( _ radio_in < = _ radio_min) {
return _reverse = = - 1 ? 100 : 0 ;
}
if ( radio_in > = radio_max ) {
if ( _ radio_in > = _ radio_max) {
return _reverse = = - 1 ? 0 : 100 ;
}
uint8_t ret = 100.0f * ( radio_in - radio_min ) / ( float ) ( radio_max - radio_min ) ;
uint8_t ret = 100.0f * ( _ radio_in - _ radio_min) / ( float ) ( _ radio_max - _ radio_min) ;
if ( _reverse = = - 1 ) {
ret = 100 - ret ;
}
@ -459,15 +459,15 @@ RC_Channel::percent_input()
@@ -459,15 +459,15 @@ RC_Channel::percent_input()
float
RC_Channel : : norm_output ( )
{
int16_t mid = ( radio_max + radio_min ) / 2 ;
int16_t mid = ( _ radio_max + _ radio_min) / 2 ;
float ret ;
if ( mid < = radio_min ) {
if ( mid < = _ radio_min) {
return 0 ;
}
if ( radio_out < mid ) {
ret = ( float ) ( radio_out - mid ) / ( float ) ( mid - radio_min ) ;
} else if ( radio_out > mid ) {
ret = ( float ) ( radio_out - mid ) / ( float ) ( radio_max - mid ) ;
if ( _ radio_out < mid ) {
ret = ( float ) ( _ radio_out - mid ) / ( float ) ( mid - _ radio_min) ;
} else if ( _ radio_out > mid ) {
ret = ( float ) ( _ radio_out - mid ) / ( float ) ( _ radio_max - mid ) ;
} else {
ret = 0 ;
}
@ -479,19 +479,19 @@ RC_Channel::norm_output()
@@ -479,19 +479,19 @@ RC_Channel::norm_output()
void RC_Channel : : output ( ) const
{
hal . rcout - > write ( _ch_out , radio_out ) ;
hal . rcout - > write ( _ch_out , _ radio_out) ;
}
void RC_Channel : : output_trim ( ) const
{
hal . rcout - > write ( _ch_out , radio_trim ) ;
hal . rcout - > write ( _ch_out , _ radio_trim) ;
}
void RC_Channel : : output_trim_all ( )
{
for ( uint8_t i = 0 ; i < RC_MAX_CHANNELS ; i + + ) {
if ( rc_ch [ i ] ! = NULL ) {
rc_ch [ i ] - > output_trim ( ) ;
if ( _ rc_ch[ i ] ! = NULL ) {
_ rc_ch[ i ] - > output_trim ( ) ;
}
}
}
@ -502,8 +502,8 @@ void RC_Channel::output_trim_all()
@@ -502,8 +502,8 @@ void RC_Channel::output_trim_all()
void RC_Channel : : setup_failsafe_trim_all ( )
{
for ( uint8_t i = 0 ; i < RC_MAX_CHANNELS ; i + + ) {
if ( rc_ch [ i ] ! = NULL ) {
hal . rcout - > set_failsafe_pwm ( 1U < < i , rc_ch [ i ] - > radio_trim ) ;
if ( _ rc_ch[ i ] ! = NULL ) {
hal . rcout - > set_failsafe_pwm ( 1U < < i , _ rc_ch[ i ] - > _ radio_trim) ;
}
}
}
@ -511,7 +511,7 @@ void RC_Channel::setup_failsafe_trim_all()
@@ -511,7 +511,7 @@ void RC_Channel::setup_failsafe_trim_all()
void
RC_Channel : : input ( )
{
radio_in = hal . rcin - > read ( _ch_out ) ;
_ radio_in = hal . rcin - > read ( _ch_out ) ;
}
uint16_t
@ -537,7 +537,7 @@ RC_Channel *RC_Channel::rc_channel(uint8_t i)
@@ -537,7 +537,7 @@ RC_Channel *RC_Channel::rc_channel(uint8_t i)
if ( i > = RC_MAX_CHANNELS ) {
return NULL ;
}
return rc_ch [ i ] ;
return _ rc_ch[ i ] ;
}
// return a limit PWM value
@ -545,14 +545,14 @@ uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const
@@ -545,14 +545,14 @@ uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const
{
switch ( limit ) {
case RC_CHANNEL_LIMIT_TRIM :
return radio_trim ;
return _ radio_trim;
case RC_CHANNEL_LIMIT_MAX :
return get_reverse ( ) ? radio_min : radio_max ;
return get_reverse ( ) ? _ radio_min : _ radio_max;
case RC_CHANNEL_LIMIT_MIN :
return get_reverse ( ) ? radio_max : radio_min ;
return get_reverse ( ) ? _ radio_max : _ radio_min;
}
// invalid limit value, return trim
return radio_trim ;
return _ radio_trim;
}
/*
@ -560,5 +560,5 @@ uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const
@@ -560,5 +560,5 @@ uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const
*/
bool RC_Channel : : in_trim_dz ( )
{
return is_bounded_int32 ( radio_in , radio_trim - _dead_zone , radio_trim + _dead_zone ) ;
return is_bounded_int32 ( _ radio_in, _ radio_trim - _dead_zone , _ radio_trim + _dead_zone ) ;
}