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 OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  20  * @brief      System module
  21  *
  22  * @see        The GNU Public License (GPL) Version 3
  23  *
  24  *****************************************************************************/
  25 /*
  26  * This program is free software; you can redistribute it and/or modify
  27  * it under the terms of the GNU General Public License as published by
  28  * the Free Software Foundation; either version 3 of the License, or
  29  * (at your option) any later version.
  30  *
  31  * This program is distributed in the hope that it will be useful, but
  32  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  33  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  34  * for more details.
  35  *
  36  * You should have received a copy of the GNU General Public License along
  37  * with this program; if not, write to the Free Software Foundation, Inc.,
  38  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  39  */
  40
  41 #include <pios.h>
  42
  43 #include <uavobjectmanager.h>
  44 #include <openpilot.h>
  45
  46 #include <oplinkstatus.h>
  47 #include <taskinfo.h>
  48
  49 #include <pios_rfm22b.h>
  50 #include <pios_board_info.h>
  51
  52 // Private constants
  53 #define SYSTEM_UPDATE_PERIOD_MS 1000
  54
  55 #if defined(PIOS_SYSTEM_STACK_SIZE)
  56 #define STACK_SIZE_BYTES        PIOS_SYSTEM_STACK_SIZE
  57 #else
  58 #define STACK_SIZE_BYTES        924
  59 #endif
  60
  61 #define TASK_PRIORITY           (tskIDLE_PRIORITY + 2)
  62
  63 // Private types
  64
  65 // Private variables
  66 static xTaskHandle systemTaskHandle;
  67 static bool stackOverflow;
  68 static bool mallocFailed;
  69 volatile int initTaskDone = 0;
  70
  71 // Private functions
  72 static void systemTask(void *parameters);
  73
  74 /**
  75  * Create the module task.
  76  * \returns 0 on success or -1 if initialization failed
  77  */
  78 int32_t OPLinkModStart(void)
  79 {
  80     // Initialize vars
  81     stackOverflow = false;
  82     mallocFailed  = false;
  83     // Create oplink system task
  84     xTaskCreate(systemTask, "OPLink", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
  85     // Register task
  86     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
  87
  88     return 0;
  89 }
  90
  91 /**
  92  * Initialize the module, called on startup.
  93  * \returns 0 on success or -1 if initialization failed
  94  */
  95 int32_t OPLinkModInitialize(void)
  96 {
  97     // Must registers objects here for system thread because ObjectManager started in OpenPilotInit
  98
  99     // Call the module start function.
 100     OPLinkModStart();
 101
 102     return 0;
 103 }
 104
 105 MODULE_INITCALL(OPLinkModInitialize, 0);
 106
 107 /**
 108  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 109  */
 110 static void systemTask(__attribute__((unused)) void *parameters)
 111 {
 112     portTickType lastSysTime;
 113     uint16_t prev_tx_count = 0;
 114     uint16_t prev_rx_count = 0;
 115     bool first_time = true;
 116
 117     while (!initTaskDone) {
 118         vTaskDelay(10);
 119     }
 120     /* create all modules thread */
 121     MODULE_TASKCREATE_ALL;
 122
 123     /* start the delayed callback scheduler */
 124     PIOS_CALLBACKSCHEDULER_Start();
 125
 126     if (mallocFailed) {
 127         /* We failed to malloc during task creation,
 128          * system behaviour is undefined.  Reset and let
 129          * the BootFault code recover for us.
 130          */
 131         PIOS_SYS_Reset();
 132     }
 133
 134     // Initialize vars
 135     lastSysTime = xTaskGetTickCount();
 136
 137     // Main system loop
 138     while (1) {
 139         // Flash the heartbeat LED
 140 #if defined(PIOS_LED_HEARTBEAT)
 141         PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 142 #endif /* PIOS_LED_HEARTBEAT */
 143
 144         // Update the OPLinkStatus UAVO
 145         OPLinkStatusData oplinkStatus;
 146         OPLinkStatusGet(&oplinkStatus);
 147
 148         // Get the other device stats.
 149         PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM);
 150
 151         // Get the stats from the radio device
 152         struct rfm22b_stats radio_stats;
 153         PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
 154
 155         if (pios_rfm22b_id) {
 156             // Update the status
 157             oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
 158             oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
 159             oplinkStatus.RxGood = radio_stats.rx_good;
 160             oplinkStatus.RxCorrected   = radio_stats.rx_corrected;
 161             oplinkStatus.RxErrors = radio_stats.rx_error;
 162             oplinkStatus.RxMissed = radio_stats.rx_missed;
 163             oplinkStatus.RxFailure     = radio_stats.rx_failure;
 164             oplinkStatus.TxDropped     = radio_stats.tx_dropped;
 165             oplinkStatus.TxFailure     = radio_stats.tx_failure;
 166             oplinkStatus.Resets      = radio_stats.resets;
 167             oplinkStatus.Timeouts    = radio_stats.timeouts;
 168             oplinkStatus.RSSI        = radio_stats.rssi;
 169             oplinkStatus.LinkQuality = radio_stats.link_quality;
 170             if (first_time) {
 171                 first_time = false;
 172             } else {
 173                 uint16_t tx_count = radio_stats.tx_byte_count;
 174                 uint16_t rx_count = radio_stats.rx_byte_count;
 175                 uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
 176                 uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
 177                 oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 178                 oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 179                 prev_tx_count = tx_count;
 180                 prev_rx_count = rx_count;
 181             }
 182             oplinkStatus.TXSeq     = radio_stats.tx_seq;
 183             oplinkStatus.RXSeq     = radio_stats.rx_seq;
 184             oplinkStatus.LinkState = radio_stats.link_state;
 185         } else {
 186             oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
 187         }
 188
 189         if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
 190             LINK_LED_ON;
 191         } else {
 192             LINK_LED_OFF;
 193         }
 194
 195         // Update the object
 196         OPLinkStatusSet(&oplinkStatus);
 197
 198         // Wait until next period
 199         vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
 200     }
 201 }
 202
 203 /**
 204  * Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
 205  */
 206 void vApplicationIdleHook(void)
 207 {}
 208
 209 /**
 210  * Called by the RTOS when a stack overflow is detected.
 211  */
 212 #define DEBUG_STACK_OVERFLOW 0
 213 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
 214                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 215 {
 216     stackOverflow = true;
 217 #if DEBUG_STACK_OVERFLOW
 218     static volatile bool wait_here = true;
 219     while (wait_here) {
 220         ;
 221     }
 222     wait_here = true;
 223 #endif
 224 }
 225
 226 /**
 227  * Called by the RTOS when a malloc call fails.
 228  */
 229 #define DEBUG_MALLOC_FAILURES 0
 230 void vApplicationMallocFailedHook(void)
 231 {
 232     mallocFailed = true;
 233 #if DEBUG_MALLOC_FAILURES
 234     static volatile bool wait_here = true;
 235     while (wait_here) {
 236         ;
 237     }
 238     wait_here = true;
 239 #endif
 240 }
 241
 242 /**
 243  * @}
 244  * @}
 245  */