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 }