/** * Initialise the telemetry module * \return -1 if initialisation failed * \return 0 on success */ int32_t TelemetryInitialize(void) { UAVObjEvent ev; // Initialize vars timeOfLastObjectUpdate = 0; // Create object queues queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #endif // Get telemetry settings object updateSettings(); // Initialise UAVTalk UAVTalkInitialize(&transmitData); // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); // Create periodic event that will be used to update the telemetry stats txErrors = 0; txRetries = 0; memset(&ev, 0, sizeof(UAVObjEvent)); EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); // Listen to objects of interest GCSTelemetryStatsConnectQueue(priorityQueue); TelemetrySettingsConnectQueue(priorityQueue); return 0; }
/** * 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; }
/** * 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); // Register to receive data from the radio packet handler. // This must be after the radio module is initialized. #ifdef PIOS_PACKET_HANDLER if (PIOS_PACKET_HANDLER) PHRegisterDataHandler(PIOS_PACKET_HANDLER, receivePacketData); #endif // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #if defined(PIOS_TELEM_PRIORITY_QUEUE) xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); #endif return 0; }
/** * Initialise the telemetry module * \return -1 if initialisation failed * \return 0 on success */ int32_t TelemetryInitialize(void) { UAVObjEvent ev; // Initialize vars timeOfLastObjectUpdate = 0; // Create object queues queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #endif // Get telemetry settings object updateSettings(); // Initialise UAVTalk UAVTalkInitialize(&transmitData); // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); // Create periodic event that will be used to update the telemetry stats txErrors = 0; txRetries = 0; memset(&ev, 0, sizeof(UAVObjEvent)); EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); // Listen to objects of interest GCSTelemetryStatsConnectQueue(priorityQueue); TelemetrySettingsConnectQueue(priorityQueue); // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #if defined(PIOS_TELEM_PRIORITY_QUEUE) xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); #endif return 0; }
/** * 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 telemetryTxTaskHandle = PIOS_Thread_Create(telemetryTxTask, "TelTx", STACK_SIZE_BYTES, NULL, TASK_PRIORITY_TX); telemetryRxTaskHandle = PIOS_Thread_Create(telemetryRxTask, "TelRx", STACK_SIZE_BYTES, NULL, TASK_PRIORITY_RX); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #if defined(PIOS_TELEM_PRIORITY_QUEUE) telemetryTxPriTaskHandle = PIOS_Thread_Create(telemetryTxPriTask, "TelPriTx", STACK_SIZE_BYTES, NULL, TASK_PRIORITY_TXPRI); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); #endif return 0; }
/** * Initialise the telemetry module * \return -1 if initialisation failed * \return 0 on success */ int32_t MAVLinkStart(void) { // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); // Listen to objects of interest GCSTelemetryStatsConnectQueue(priorityQueue); TelemetrySettingsConnectQueue(priorityQueue); // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char *)"MAVLinkTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); xTaskCreate(telemetryRxTask, (signed char *)"MAVLinkRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); xTaskCreate(mavlinkStateMachineTask, (signed char*) "MAVLinkSM", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_STATE_MACHINE, &mavlinkStateMachineTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #if defined(PIOS_TELEM_PRIORITY_QUEUE) xTaskCreate(telemetryTxPriTask, (signed char *)"MAVLPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); #endif return 0; }