/** * Module initialization */ int32_t StabilizationInitialize() { // Initialize variables StabilizationDesiredInitialize(); StabilizationSettingsInitialize(); StabilizationStatusInitialize(); StabilizationBankInitialize(); StabilizationSettingsBank1Initialize(); StabilizationSettingsBank2Initialize(); StabilizationSettingsBank3Initialize(); RateDesiredInitialize(); ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch sin_lookup_initalize(); stabilizationOuterloopInit(); stabilizationInnerloopInit(); #ifdef REVOLUTION stabilizationAltitudeloopInit(); #endif pid_zero(&stabSettings.outerPids[0]); pid_zero(&stabSettings.outerPids[1]); pid_zero(&stabSettings.outerPids[2]); pid_zero(&stabSettings.innerPids[0]); pid_zero(&stabSettings.innerPids[1]); pid_zero(&stabSettings.innerPids[2]); return 0; }
/** * @brief initialize UAVOs and structs used by this library */ void plan_initialize() { TakeOffLocationInitialize(); PositionStateInitialize(); PathDesiredInitialize(); FlightModeSettingsInitialize(); FlightStatusInitialize(); AttitudeStateInitialize(); ManualControlCommandInitialize(); VelocityStateInitialize(); VtolPathFollowerSettingsInitialize(); StabilizationBankInitialize(); StabilizationDesiredInitialize(); }
/** * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization" * @input: ManualControlCommand * @output: StabilizationDesired */ void stabilizedHandler(bool newinit) { if (newinit) { StabilizationDesiredInitialize(); StabilizationBankInitialize(); } ManualControlCommandData cmd; ManualControlCommandGet(&cmd); FlightModeSettingsData settings; FlightModeSettingsGet(&settings); StabilizationDesiredData stabilization; StabilizationDesiredGet(&stabilization); StabilizationBankData stabSettings; StabilizationBankGet(&stabSettings); cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll); cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings); break; default: // Major error, this should not occur because only enter this block when one of these is true AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); return; } stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax : 0; // this is an invalid mode stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : 0; // this is an invalid mode // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode.Roll = stab_settings[0]; stabilization.StabilizationMode.Pitch = stab_settings[1]; // Other axes (yaw) cannot be Rattitude, so use Rate // Should really do this for Attitude mode as well? if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw; } else { stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax : 0; // this is an invalid mode } stabilization.StabilizationMode.Thrust = stab_settings[3]; stabilization.Thrust = cmd.Thrust; StabilizationDesiredSet(&stabilization); }