flight/modules/OPLink/oplinkmod.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @brief The OpenPilot Modules do the majority of the control in OpenPilot.  The
   5  * @ref OPLinkModule The OPLink Module is the equivalanet of the System
   6  * Module for the OPLink modem.  it starts all the other modules.
   7  #  This is done through the @ref PIOS "PIOS Hardware abstraction layer",
   8  # which then contains hardware specific implementations
   9  * (currently only STM32 supported)
  10  *
  11  * @{
  12  * @addtogroup OPLinkModule OPLink Module
  13  * @brief Initializes PIOS and other modules runs monitoring
  14  * After initializing all the modules runs basic monitoring and
  15  * alarms.
  16  * @{
  17  *
  18  * @file       oplinkmod.c
  19  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  20  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  21  * @brief      System module
  22  *
  23  * @see        The GNU Public License (GPL) Version 3
  24  *
  25  *****************************************************************************/
  26 /*
  27  * This program is free software; you can redistribute it and/or modify
  28  * it under the terms of the GNU General Public License as published by
  29  * the Free Software Foundation; either version 3 of the License, or
  30  * (at your option) any later version.
  31  *
  32  * This program is distributed in the hope that it will be useful, but
  33  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  34  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  35  * for more details.
  36  *
  37  * You should have received a copy of the GNU General Public License along
  38  * with this program; if not, write to the Free Software Foundation, Inc.,
  39  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  40  */
  41
  42 #include <pios.h>
  43
  44 #include <uavobjectmanager.h>
  45 #include <openpilot.h>
  46
  47 #include <oplinkstatus.h>
  48 #include <taskinfo.h>
  49
  50 #include <pios_rfm22b.h>
  51 #include <pios_board_info.h>
  52
  53 // Private constants
  54 #define SYSTEM_UPDATE_PERIOD_MS 1000
  55
  56 #if defined(PIOS_SYSTEM_STACK_SIZE)
  57 #define STACK_SIZE_BYTES        PIOS_SYSTEM_STACK_SIZE
  58 #else
  59 #define STACK_SIZE_BYTES        924
  60 #endif
  61
  62 #define TASK_PRIORITY           (tskIDLE_PRIORITY + 2)
  63
  64 // Private types
  65
  66 // Private variables
  67 static xTaskHandle systemTaskHandle;
  68 static bool stackOverflow;
  69 static bool mallocFailed;
  70 volatile int initTaskDone = 0;
  71 static uint8_t previousRFXtalCap;
  72
  73 // Private functions
  74 static void systemTask(void *parameters);
  75 static void oplinkSettingsUpdatedCb(UAVObjEvent *ev);
  76
  77 /**
  78  * Create the module task.
  79  * \returns 0 on success or -1 if initialization failed
  80  */
  81 int32_t OPLinkModStart(void)
  82 {
  83     // Initialize vars
  84     stackOverflow = false;
  85     mallocFailed  = false;
  86     // Create oplink system task
  87     xTaskCreate(systemTask, "OPLink", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
  88     // Register task
  89     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
  90
  91     return 0;
  92 }
  93
  94 /**
  95  * Initialize the module, called on startup.
  96  * \returns 0 on success or -1 if initialization failed
  97  */
  98 int32_t OPLinkModInitialize(void)
  99 {
 100     // Must registers objects here for system thread because ObjectManager started in OpenPilotInit
 101
 102     // Call the module start function.
 103     OPLinkModStart();
 104
 105     return 0;
 106 }
 107
 108 MODULE_INITCALL(OPLinkModInitialize, 0);
 109
 110 /**
 111  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 112  */
 113 static void systemTask(__attribute__((unused)) void *parameters)
 114 {
 115     portTickType lastSysTime;
 116     uint16_t prev_tx_count = 0;
 117     uint16_t prev_rx_count = 0;
 118     uint16_t prev_tx_seq   = 0;
 119     uint16_t prev_rx_seq   = 0;
 120     bool first_time = true;
 121
 122     while (!initTaskDone) {
 123         vTaskDelay(10);
 124     }
 125
 126 #ifndef PIOS_INCLUDE_WDG
 127 // if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
 128 #define PIOS_WDG_Clear()
 129 #endif
 130     /* create all modules thread */
 131     MODULE_TASKCREATE_ALL;
 132
 133     /* start the delayed callback scheduler */
 134     PIOS_CALLBACKSCHEDULER_Start();
 135
 136     if (mallocFailed) {
 137         /* We failed to malloc during task creation,
 138          * system behaviour is undefined.  Reset and let
 139          * the BootFault code recover for us.
 140          */
 141         PIOS_SYS_Reset();
 142     }
 143
 144     // Initialize previousRFXtalCap used by callback
 145     OPLinkSettingsRFXtalCapGet(&previousRFXtalCap);
 146     OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb);
 147
 148     // Initialize vars
 149     lastSysTime = xTaskGetTickCount();
 150
 151     // Main system loop
 152     while (1) {
 153         // Update the OPLinkStatus UAVO
 154         OPLinkStatusData oplinkStatus;
 155         OPLinkStatusGet(&oplinkStatus);
 156
 157         // Get the stats from the radio device
 158         struct rfm22b_stats radio_stats;
 159         PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
 160         oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
 161
 162         if (pios_rfm22b_id) {
 163             // Update the status
 164             oplinkStatus.DeviceID      = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
 165             oplinkStatus.RxGood        = radio_stats.rx_good;
 166             oplinkStatus.RxCorrected   = radio_stats.rx_corrected;
 167             oplinkStatus.RxErrors      = radio_stats.rx_error;
 168             oplinkStatus.RxMissed      = radio_stats.rx_missed;
 169             oplinkStatus.RxFailure     = radio_stats.rx_failure;
 170             oplinkStatus.TxDropped     = radio_stats.tx_dropped;
 171             oplinkStatus.TxFailure     = radio_stats.tx_failure;
 172             oplinkStatus.Resets        = radio_stats.resets;
 173             oplinkStatus.Timeouts      = radio_stats.timeouts;
 174             oplinkStatus.RSSI = radio_stats.rssi;
 175             oplinkStatus.LinkQuality   = radio_stats.link_quality;
 176             oplinkStatus.AFCCorrection = radio_stats.afc_correction;
 177             if (first_time) {
 178                 first_time = false;
 179             } else {
 180                 uint16_t tx_count   = radio_stats.tx_byte_count;
 181                 uint16_t rx_count   = radio_stats.rx_byte_count;
 182                 uint16_t tx_packets = radio_stats.tx_seq - prev_tx_seq;
 183                 uint16_t rx_packets = radio_stats.rx_seq - prev_rx_seq;
 184                 uint16_t tx_bytes   = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
 185                 uint16_t rx_bytes   = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
 186                 oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 187                 oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 188                 oplinkStatus.TXPacketRate = (uint16_t)((float)(tx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 189                 oplinkStatus.RXPacketRate = (uint16_t)((float)(rx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 190                 prev_tx_count = tx_count;
 191                 prev_rx_count = rx_count;
 192                 prev_tx_seq   = radio_stats.tx_seq;
 193                 prev_rx_seq   = radio_stats.rx_seq;
 194             }
 195             oplinkStatus.TXSeq     = radio_stats.tx_seq;
 196             oplinkStatus.RXSeq     = radio_stats.rx_seq;
 197             oplinkStatus.LinkState = radio_stats.link_state;
 198         }
 199
 200         // Turn on the link/heartbeat ID if we're connected, otherwise flash it.
 201         if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
 202             LINK_LED_ON;
 203         } else {
 204             LINK_LED_TOGGLE;
 205         }
 206
 207         // Update the object
 208         OPLinkStatusSet(&oplinkStatus);
 209
 210         // Wait until next period
 211         vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
 212     }
 213 }
 214
 215 /**
 216  * Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
 217  */
 218 void vApplicationIdleHook(void)
 219 {}
 220
 221 /**
 222  * Called by the RTOS when a stack overflow is detected.
 223  */
 224 #define DEBUG_STACK_OVERFLOW 0
 225 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
 226                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 227 {
 228     stackOverflow = true;
 229 #if DEBUG_STACK_OVERFLOW
 230     static volatile bool wait_here = true;
 231     while (wait_here) {
 232         ;
 233     }
 234     wait_here = true;
 235 #endif
 236 }
 237
 238 /**
 239  * Called by the RTOS when a malloc call fails.
 240  */
 241 #define DEBUG_MALLOC_FAILURES 0
 242 void vApplicationMallocFailedHook(void)
 243 {
 244     mallocFailed = true;
 245 #if DEBUG_MALLOC_FAILURES
 246     static volatile bool wait_here = true;
 247     while (wait_here) {
 248         ;
 249     }
 250     wait_here = true;
 251 #endif
 252 }
 253
 254 /**
 255  * Called whenever OPLink settings changed
 256  */
 257 static void oplinkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 258 {
 259     uint8_t currentRFXtalCap;
 260
 261     OPLinkSettingsRFXtalCapGet(&currentRFXtalCap);
 262
 263     // Check if RFXtalCap value changed
 264     if (currentRFXtalCap != previousRFXtalCap) {
 265         PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, currentRFXtalCap);
 266         PIOS_RFM22B_Reinit(pios_rfm22b_id);
 267         previousRFXtalCap = currentRFXtalCap;
 268     }
 269 }
 270
 271 /**
 272  * @}
 273  * @}
 274  */