/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @brief The OpenPilot Modules do the majority of the control in OpenPilot. The * @ref OPLinkModule The OPLink Module is the equivalanet of the System * Module for the OPLink modem. it starts all the other modules. # This is done through the @ref PIOS "PIOS Hardware abstraction layer", # which then contains hardware specific implementations * (currently only STM32 supported) * * @{ * @addtogroup OPLinkModule OPLink Module * @brief Initializes PIOS and other modules runs monitoring * After initializing all the modules runs basic monitoring and * alarms. * @{ * * @file oplinkmod.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief System module * * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include #include #include #include #include #include #include // Private constants #define SYSTEM_UPDATE_PERIOD_MS 1000 #if defined(PIOS_SYSTEM_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE #else #define STACK_SIZE_BYTES 924 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) // Private types // Private variables static xTaskHandle systemTaskHandle; static bool stackOverflow; static bool mallocFailed; volatile int initTaskDone = 0; static uint8_t previousRFXtalCap; // Private functions static void systemTask(void *parameters); static void oplinkSettingsUpdatedCb(UAVObjEvent *ev); /** * 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; } /** * Initialize the module, called on startup. * \returns 0 on success or -1 if initialization failed */ int32_t OPLinkModInitialize(void) { // Must registers objects here for system thread because ObjectManager started in OpenPilotInit // Call the module start function. OPLinkModStart(); return 0; } MODULE_INITCALL(OPLinkModInitialize, 0); /** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(__attribute__((unused)) void *parameters) { portTickType lastSysTime; uint16_t prev_tx_count = 0; uint16_t prev_rx_count = 0; uint16_t prev_tx_seq = 0; uint16_t prev_rx_seq = 0; bool first_time = true; while (!initTaskDone) { vTaskDelay(10); } #ifndef PIOS_INCLUDE_WDG // if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop #define PIOS_WDG_Clear() #endif /* create all modules thread */ MODULE_TASKCREATE_ALL; /* start the delayed callback scheduler */ PIOS_CALLBACKSCHEDULER_Start(); if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } // Initialize previousRFXtalCap used by callback OPLinkSettingsRFXtalCapGet(&previousRFXtalCap); OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb); // Initialize vars lastSysTime = xTaskGetTickCount(); // Main system loop while (1) { // Update the OPLinkStatus UAVO OPLinkStatusData oplinkStatus; OPLinkStatusGet(&oplinkStatus); // Get the stats from the radio device struct rfm22b_stats radio_stats; PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); if (pios_rfm22b_id) { // Update the status oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); oplinkStatus.RxGood = radio_stats.rx_good; oplinkStatus.RxCorrected = radio_stats.rx_corrected; oplinkStatus.RxErrors = radio_stats.rx_error; oplinkStatus.RxMissed = radio_stats.rx_missed; oplinkStatus.RxFailure = radio_stats.rx_failure; oplinkStatus.TxDropped = radio_stats.tx_dropped; oplinkStatus.TxFailure = radio_stats.tx_failure; oplinkStatus.Resets = radio_stats.resets; oplinkStatus.Timeouts = radio_stats.timeouts; oplinkStatus.RSSI = radio_stats.rssi; oplinkStatus.LinkQuality = radio_stats.link_quality; oplinkStatus.AFCCorrection = radio_stats.afc_correction; if (first_time) { first_time = false; } else { uint16_t tx_count = radio_stats.tx_byte_count; uint16_t rx_count = radio_stats.rx_byte_count; uint16_t tx_packets = radio_stats.tx_seq - prev_tx_seq; uint16_t rx_packets = radio_stats.rx_seq - prev_rx_seq; uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); oplinkStatus.TXPacketRate = (uint16_t)((float)(tx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS); oplinkStatus.RXPacketRate = (uint16_t)((float)(rx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS); prev_tx_count = tx_count; prev_rx_count = rx_count; prev_tx_seq = radio_stats.tx_seq; prev_rx_seq = radio_stats.rx_seq; } oplinkStatus.TXSeq = radio_stats.tx_seq; oplinkStatus.RXSeq = radio_stats.rx_seq; oplinkStatus.LinkState = radio_stats.link_state; } // Turn on the link/heartbeat ID if we're connected, otherwise flash it. if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { LINK_LED_ON; } else { LINK_LED_TOGGLE; } // Update the object OPLinkStatusSet(&oplinkStatus); // Wait until next period vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } /** * Called by the RTOS when the CPU is idle, used to measure the CPU idle time. */ void vApplicationIdleHook(void) {} /** * Called by the RTOS when a stack overflow is detected. */ #define DEBUG_STACK_OVERFLOW 0 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, __attribute__((unused)) signed portCHAR *pcTaskName) { stackOverflow = true; #if DEBUG_STACK_OVERFLOW static volatile bool wait_here = true; while (wait_here) { ; } wait_here = true; #endif } /** * Called by the RTOS when a malloc call fails. */ #define DEBUG_MALLOC_FAILURES 0 void vApplicationMallocFailedHook(void) { mallocFailed = true; #if DEBUG_MALLOC_FAILURES static volatile bool wait_here = true; while (wait_here) { ; } wait_here = true; #endif } /** * Called whenever OPLink settings changed */ static void oplinkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { uint8_t currentRFXtalCap; OPLinkSettingsRFXtalCapGet(¤tRFXtalCap); // Check if RFXtalCap value changed if (currentRFXtalCap != previousRFXtalCap) { PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, currentRFXtalCap); PIOS_RFM22B_Reinit(pios_rfm22b_id); previousRFXtalCap = currentRFXtalCap; } } /** * @} * @} */