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 ManualControlInitialize() { /* Check the assumptions about uavobject enum's are correct */ PIOS_STATIC_ASSERT(assumptions); ManualControlCommandInitialize(); FlightStatusInitialize(); ManualControlSettingsInitialize(); FlightModeSettingsInitialize(); SystemSettingsInitialize(); StabilizationSettingsInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES VtolSelfTuningStatsInitialize(); VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); SystemSettingsConnectCallback(&SettingsUpdatedCb); #endif callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES); return 0; }