/** * Initialise the module. Called before the start function * \returns 0 on success or -1 if initialisation failed */ int32_t AttitudeInitialize(void) { AttitudeActualInitialize(); AirspeedActualInitialize(); AirspeedSensorInitialize(); AttitudeSettingsInitialize(); PositionActualInitialize(); VelocityActualInitialize(); RevoSettingsInitialize(); RevoCalibrationInitialize(); EKFConfigurationInitialize(); EKFStateVarianceInitialize(); FlightStatusInitialize(); // Initialize this here while we aren't setting the homelocation in GPS HomeLocationInitialize(); // Initialize quaternion AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = 1.0f; attitude.q2 = 0.0f; attitude.q3 = 0.0f; attitude.q4 = 0.0f; AttitudeActualSet(&attitude); // Cannot trust the values to init right above if BL runs GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosBias.x = 0.0f; gyrosBias.y = 0.0f; gyrosBias.z = 0.0f; GyrosBiasSet(&gyrosBias); AttitudeSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb); HomeLocationConnectCallback(&settingsUpdatedCb); EKFConfigurationConnectCallback(&settingsUpdatedCb); FlightStatusConnectCallback(&settingsUpdatedCb); return 0; }
int32_t NotifyInitialize(void) { uint8_t ws281xOutStatus; HwSettingsWS2811LED_OutGet(&ws281xOutStatus); // Todo: Until further applications exists for WS2811 notify enabled status is tied to ws281x output configuration bool enabled = ws281xOutStatus != HWSETTINGS_WS2811LED_OUT_DISABLED; if (enabled) { alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize); for (uint8_t i = 0; i < alarmsMapSize; i++) { alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK; alarmStatus[i].lastAlarmTime = 0; } FlightStatusConnectCallback(&updatedCb); static UAVObjEvent ev; memset(&ev, 0, sizeof(UAVObjEvent)); EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS); updatedCb(0); } return 0; }
int32_t LoggingStart(void) { DebugLogSettingsConnectCallback(SettingsUpdatedCb); DebugLogControlConnectCallback(ControlUpdatedCb); FlightStatusConnectCallback(FlightStatusUpdatedCb); SettingsUpdatedCb(DebugLogSettingsHandle()); UAVObjEvent ev = { .obj = DebugLogSettingsHandle(), .instId = 0, .event = EV_UPDATED_PERIODIC, .lowPriority = true, }; EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000); // invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything! StatusUpdatedCb(&ev); return 0; } MODULE_INITCALL(LoggingInitialize, LoggingStart); static void StatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { PIOS_DEBUGLOG_Info(&status.Flight, &status.Entry, &status.FreeSlots, &status.UsedSlots); DebugLogStatusSet(&status); } static void FlightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FlightStatusGet(&flightstatus); if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ONLYWHENARMED) { if (flightstatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { PIOS_DEBUGLOG_Printf("FlightStatus Disarmed: On board Logging disabled."); PIOS_DEBUGLOG_Enable(0); } else { PIOS_DEBUGLOG_Enable(1); PIOS_DEBUGLOG_Printf("FlightStatus Armed: On board logging enabled."); } } } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { DebugLogSettingsGet(&settings); if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ALWAYS) { PIOS_DEBUGLOG_Enable(1); PIOS_DEBUGLOG_Printf("On board logging enabled."); } else if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_DISABLED) { PIOS_DEBUGLOG_Printf("On board logging disabled."); PIOS_DEBUGLOG_Enable(0); } else { FlightStatusUpdatedCb(NULL); } } static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { DebugLogControlGet(&control); if (control.Operation == DEBUGLOGCONTROL_OPERATION_RETRIEVE) { memset(entry, 0, sizeof(DebugLogEntryData)); if (PIOS_DEBUGLOG_Read(entry, control.Flight, control.Entry) != 0) { // reading from log failed, mark as non existent in output entry->Flight = control.Flight; entry->Entry = control.Entry; entry->Type = DEBUGLOGENTRY_TYPE_EMPTY; } DebugLogEntrySet(entry); } else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) { uint8_t armed; FlightStatusArmedGet(&armed); if (armed == FLIGHTSTATUS_ARMED_DISARMED) { PIOS_DEBUGLOG_Format(); } } StatusUpdatedCb(ev); }
/** * 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); } } }