Ejemplo n.º 1
0
/**
 * 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;
}
Ejemplo n.º 2
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);
}