/** * Initialise the telemetry module * \return -1 if initialisation failed * \return 0 on success */ int32_t TelemetryStart(void) { // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); // Listen to objects of interest GCSTelemetryStatsConnectQueue(priorityQueue); // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #ifdef PIOS_INCLUDE_RFM22B xTaskCreate(radioRxTask, (signed char *)"RadioRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle); #endif #if defined(PIOS_TELEM_PRIORITY_QUEUE) xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); #endif return 0; }
static int32_t comUsbBridgeStart(void) { if (bridge_enabled) { // Start tasks xTaskCreate(com2UsbBridgeTask, "Com2UsbBridge", C2U_STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &com2UsbBridgeTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_COM2USBBRIDGE, com2UsbBridgeTaskHandle); xTaskCreate(usb2ComBridgeTask, "Usb2ComBridge", U2C_STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &usb2ComBridgeTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_USB2COMBRIDGE, usb2ComBridgeTaskHandle); return 0; } return -1; }
/** * Initialise the telemetry module * \return -1 if initialisation failed * \return 0 on success */ int32_t OveroSyncStart(void) { overosync = (struct overosync *)pios_malloc(sizeof(*overosync)); if (overosync == NULL) { return -1; } overosync->transaction_lock = xSemaphoreCreateMutex(); if (overosync->transaction_lock == NULL) { return -1; } overosync->buffer_lock = xSemaphoreCreateMutex(); if (overosync->buffer_lock == NULL) { return -1; } overosync->active_transaction_id = 0; overosync->loading_transaction_id = 0; overosync->write_pointer = 0; overosync->sent_bytes = 0; overosync->framesync_error = 0; // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); // Start telemetry tasks xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); return 0; }
/** * Start the task. Expects all objects to be initialized by this point. * \returns 0 on success or -1 if initialisation failed */ int32_t AttitudeStart(void) { // Create the queues for the sensors gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); // Start main task xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); #endif GyrosConnectQueue(gyroQueue); AccelsConnectQueue(accelQueue); MagnetometerConnectQueue(magQueue); AirspeedSensorConnectQueue(airspeedQueue); BaroAltitudeConnectQueue(baroQueue); GPSPositionConnectQueue(gpsQueue); GPSVelocityConnectQueue(gpsVelQueue); return 0; }
/** * Start the task. Expects all objects to be initialized by this point. * \returns 0 on success or -1 if initialisation failed */ int32_t SensorsStart(void) { // Start main task xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); REGISTER_WDG(); return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AltitudeStart() { // Start main task xTaskCreate(altitudeTask, "Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle); return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AttitudeStart(void) { // Start main task xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); #endif return 0; }
/** * Start the task. Expects all objects to be initialized by this point. * \returns 0 on success or -1 if initialisation failed */ int32_t SensorsStart(void) { // Start main task xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); #endif return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AutotuneStart(void) { // Start main task if it is enabled if (autotuneEnabled) { xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); } return 0; }
/** * Create the module task. * \returns 0 on success or -1 if initialization failed */ int32_t OPLinkModStart(void) { // Initialize vars stackOverflow = false; mallocFailed = false; // Create oplink system task xTaskCreate(systemTask, "OPLink", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle); // Register task PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle); return 0; }
/** * Start all scheduler tasks * Will instantiate all scheduler tasks registered so far. Although new * callbacks CAN be registered beyond that point, any further scheduling tasks * will be started the moment of instantiation. It is not possible to increase * the STACK requirements of a scheduler task after this function has been * run. No callbacks will be run before this function is called, although * they can be marked for later execution by executing the dispatch function. * \return Success (0), failure (-1) */ int32_t CallbackSchedulerStart() { xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // only call once PIOS_Assert(schedulerStarted == false); // start tasks struct DelayedCallbackTaskStruct *cursor = NULL; int t = 0; LL_FOREACH(schedulerTasks, cursor) { xTaskCreate( CallbackSchedulerTask, cursor->name, cursor->stackSize / 4, cursor, cursor->priorityTask, &cursor->callbackSchedulerTaskHandle ); if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) { PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle); } t++; }
/** * Initialise the overosync module * \return -1 if initialisation failed * \return 0 on success */ int32_t OveroSyncStart(void) { // Check if module is enabled or not if (overoEnabled == false) { return -1; } overosync = (struct overosync *)pios_malloc(sizeof(*overosync)); if (overosync == NULL) { return -1; } overosync->sent_bytes = 0; // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); // Start overosync tasks xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &overoSyncTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle); return 0; }