/** * Module initialization */ int32_t StabilizationInitialize() { // Initialize variables // 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; }
/** * Module initialization */ int32_t StabilizationStart() { StabilizationSettingsConnectCallback(SettingsUpdatedCb); ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb); StabilizationBankConnectCallback(BankUpdatedCb); StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb); StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb); StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb); StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb); SettingsUpdatedCb(StabilizationSettingsHandle()); StabilizationDesiredUpdatedCb(StabilizationDesiredHandle()); FlightModeSwitchUpdatedCb(ManualControlCommandHandle()); BankUpdatedCb(StabilizationBankHandle()); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); #endif return 0; }
/** * Module initialization */ int32_t StabilizationStart() { // Initialize variables // Create object queue queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); // Listen for updates. // AttitudeActualConnectQueue(queue); GyrosConnectQueue(queue); // Connect settings callback MWRateSettingsConnectCallback(SettingsUpdatedCb); StabilizationSettingsConnectCallback(SettingsUpdatedCb); TrimAnglesSettingsConnectCallback(SettingsUpdatedCb); // Start main task taskHandle = PIOS_Thread_Create(stabilizationTask, "Stabilization", STACK_SIZE_BYTES, NULL, TASK_PRIORITY); TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle); PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION); return 0; }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (PIOS_heap_malloc_failed_p()) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); #if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) // Run this initially to make sure the configuration is checked configuration_check(); // Whenever the configuration changes, make sure it is safe to fly if (StabilizationSettingsHandle()) StabilizationSettingsConnectCallback(configurationUpdatedCb); if (SystemSettingsHandle()) SystemSettingsConnectCallback(configurationUpdatedCb); if (ManualControlSettingsHandle()) ManualControlSettingsConnectCallback(configurationUpdatedCb); if (FlightStatusHandle()) FlightStatusConnectCallback(configurationUpdatedCb); #endif #if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) if (StateEstimationHandle()) StateEstimationConnectCallback(configurationUpdatedCb); #endif // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(WDG_STATS_DIAGNOSTICS) updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set if (indicateError()) { #if defined (PIOS_LED_ALARM) PIOS_LED_On(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } else { #if defined (PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS; if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }