@ -133,101 +133,6 @@ bool Plane::suppress_throttle(void)
@@ -133,101 +133,6 @@ bool Plane::suppress_throttle(void)
return true ;
}
/*
implement a software VTail or elevon mixer . There are 4 different mixing modes
*/
void Plane : : channel_output_mixer_pwm ( uint8_t mixing_type , uint16_t & chan1_out , uint16_t & chan2_out ) const
{
int16_t c1 , c2 ;
int16_t v1 , v2 ;
// first get desired elevator and rudder as -500..500 values
c1 = chan1_out - 1500 ;
c2 = chan2_out - 1500 ;
// apply MIXING_OFFSET to input channels using long-integer version
// of formula: x = x * (g.mixing_offset/100.0 + 1.0)
// -100 => 2x on 'c1', 100 => 2x on 'c2'
if ( g . mixing_offset < 0 ) {
c1 = ( int16_t ) ( ( ( int32_t ) c1 ) * ( - g . mixing_offset + 100 ) / 100 ) ;
} else if ( g . mixing_offset > 0 ) {
c2 = ( int16_t ) ( ( ( int32_t ) c2 ) * ( g . mixing_offset + 100 ) / 100 ) ;
}
v1 = ( c1 - c2 ) * g . mixing_gain ;
v2 = ( c1 + c2 ) * g . mixing_gain ;
// now map to mixed output
switch ( mixing_type ) {
case MIXING_DISABLED :
return ;
case MIXING_UPUP :
break ;
case MIXING_UPDN :
v2 = - v2 ;
break ;
case MIXING_DNUP :
v1 = - v1 ;
break ;
case MIXING_DNDN :
v1 = - v1 ;
v2 = - v2 ;
break ;
case MIXING_UPUP_SWP :
std : : swap ( v1 , v2 ) ;
break ;
case MIXING_UPDN_SWP :
v2 = - v2 ;
std : : swap ( v1 , v2 ) ;
break ;
case MIXING_DNUP_SWP :
v1 = - v1 ;
std : : swap ( v1 , v2 ) ;
break ;
case MIXING_DNDN_SWP :
v1 = - v1 ;
v2 = - v2 ;
std : : swap ( v1 , v2 ) ;
break ;
}
// scale for a 1500 center and 900..2100 range, symmetric
v1 = constrain_int16 ( v1 , - 600 , 600 ) ;
v2 = constrain_int16 ( v2 , - 600 , 600 ) ;
chan1_out = 1500 + v1 ;
chan2_out = 1500 + v2 ;
}
/*
output mixer based on two channel output types
*/
void Plane : : channel_output_mixer ( uint8_t mixing_type , SRV_Channel : : Aux_servo_function_t func1 , SRV_Channel : : Aux_servo_function_t func2 )
{
SRV_Channel * chan1 , * chan2 ;
if ( ! ( chan1 = SRV_Channels : : get_channel_for ( func1 ) ) | |
! ( chan2 = SRV_Channels : : get_channel_for ( func2 ) ) ) {
return ;
}
uint16_t chan1_out , chan2_out ;
chan1_out = chan1 - > get_output_pwm ( ) ;
chan2_out = chan2 - > get_output_pwm ( ) ;
channel_output_mixer_pwm ( mixing_type , chan1_out , chan2_out ) ;
chan1 - > set_output_pwm ( chan1_out ) ;
chan2 - > set_output_pwm ( chan2_out ) ;
}
/*
mixer for elevon and vtail channels setup using designated servo