void hoverRollCntrl(void)
{
int rollNavDeflection ;
union longww gyroRollFeedback ;
if ( flags._.pitch_feedback )
{
if ( AILERON_NAVIGATION && flags._.GPS_steering )
{
rollNavDeflection = (tofinish_line > HOVER_NAV_MAX_PITCH_RADIUS/2) ? determine_navigation_deflection( 'h' ) : 0 ;
}
else
{
rollNavDeflection = 0 ;
}
gyroRollFeedback.WW = __builtin_mulss( hoverrollkd , omegaAccum[1] ) ;
}
else
{
rollNavDeflection = 0 ;
gyroRollFeedback.WW = 0 ;
}
roll_control = rollNavDeflection -(long)gyroRollFeedback._.W1 ;
return ;
}
void normalRollCntrl(void)
{
union longww rollAccum = { 0 } ;
union longww gyroRollFeedback ;
union longww gyroYawFeedback ;
fractional rmat6 ;
fractional omegaAccum2 ;
if ( !canStabilizeInverted() || !desired_behavior._.inverted )
{
rmat6 = rmat[6] ;
omegaAccum2 = omegaAccum[2] ;
}
else
{
rmat6 = -rmat[6] ;
omegaAccum2 = -omegaAccum[2] ;
}
#ifdef TestGains
flags._.GPS_steering = 0 ; // turn off navigation
#endif
if ( AILERON_NAVIGATION && flags._.GPS_steering )
{
rollAccum._.W1 = determine_navigation_deflection( 'a' ) ;
}
#ifdef TestGains
flags._.pitch_feedback = 1 ;
#endif
if ( ROLL_STABILIZATION_AILERONS && flags._.pitch_feedback )
{
gyroRollFeedback.WW = __builtin_mulss( rollkd , omegaAccum[1] ) ;
rollAccum.WW += __builtin_mulss( rmat6 , rollkp ) ;
}
else
{
gyroRollFeedback.WW = 0 ;
}
if ( YAW_STABILIZATION_AILERON && flags._.pitch_feedback )
{
gyroYawFeedback.WW = __builtin_mulss( yawkdail, omegaAccum2 ) ;
}
else
{
gyroYawFeedback.WW = 0 ;
}
roll_control = (long)rollAccum._.W1 - (long)gyroRollFeedback._.W1 - (long)gyroYawFeedback._.W1 ;
// Servo reversing is handled in servoMix.c
return ;
}
void normalYawCntrl(void)
{
int16_t yawNavDeflection;
union longww rollStabilization;
union longww gyroYawFeedback;
int16_t ail_rud_mix;
#ifdef TestGains
flags._.GPS_steering = 0; // turn off navigation
flags._.pitch_feedback = 1; // turn on stabilization
#endif
if (RUDDER_NAVIGATION && flags._.GPS_steering)
{
yawNavDeflection = determine_navigation_deflection('y');
if (canStabilizeInverted() && current_orientation == F_INVERTED)
{
yawNavDeflection = -yawNavDeflection;
}
}
else
{
yawNavDeflection = 0;
}
if (YAW_STABILIZATION_RUDDER && flags._.pitch_feedback)
{
gyroYawFeedback.WW = __builtin_mulus(yawkdrud, omegaAccum[2]);
}
else
{
gyroYawFeedback.WW = 0;
}
rollStabilization.WW = 0; // default case is no roll rudder stabilization
if (ROLL_STABILIZATION_RUDDER && flags._.pitch_feedback)
{
if (!desired_behavior._.inverted && !desired_behavior._.hover) // normal
{
rollStabilization.WW = __builtin_mulsu(rmat[6], rollkprud);
}
else if (desired_behavior._.inverted) // inverted
{
rollStabilization.WW = - __builtin_mulsu(rmat[6], rollkprud);
}
rollStabilization.WW -= __builtin_mulus(rollkdrud, omegaAccum[1]);
}
if (flags._.pitch_feedback)
{
int16_t ail_offset = (udb_flags._.radio_on) ? (udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]) : 0;
ail_rud_mix = MANUAL_AILERON_RUDDER_MIX * REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, ail_offset);
if (canStabilizeInverted() && current_orientation == F_INVERTED) ail_rud_mix = -ail_rud_mix;
}
else
{
ail_rud_mix = 0;
}
yaw_control = (int32_t)yawNavDeflection
- (int32_t)gyroYawFeedback._.W1
+ (int32_t)rollStabilization._.W1
+ ail_rud_mix;
// Servo reversing is handled in servoMix.c
}
