/**
* 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;
}
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
StabilizationSettingsInitialize();
MWRateSettingsInitialize();
ActuatorDesiredInitialize();
TrimAnglesInitialize();
TrimAnglesSettingsInitialize();
#if defined(RATEDESIRED_DIAGNOSTICS)
RateDesiredInitialize();
#endif
return 0;
}
void stabilizationInnerloopInit()
{
RateDesiredInitialize();
ActuatorDesiredInitialize();
GyroStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
#ifdef REVOLUTION
AirspeedStateInitialize();
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
#endif
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
GyroStateConnectCallback(GyroStateUpdatedCb);
// schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
StabilizationSettingsInitialize();
ActuatorDesiredInitialize();
#if defined(DIAGNOSTICS)
RateDesiredInitialize();
#endif
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
return 0;
}
