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     uint16_t prev_tx_seq   = 0;
 116     uint16_t prev_rx_seq   = 0;
 117     bool first_time = true;
 118
 119     while (!initTaskDone) {
 120         vTaskDelay(10);
 121     }
 122     /* create all modules thread */
 123     MODULE_TASKCREATE_ALL;
 124
 125     /* start the delayed callback scheduler */
 126     PIOS_CALLBACKSCHEDULER_Start();
 127
 128     if (mallocFailed) {
 129         /* We failed to malloc during task creation,
 130          * system behaviour is undefined.  Reset and let
 131          * the BootFault code recover for us.
 132          */
 133         PIOS_SYS_Reset();
 134     }
 135
 136     // Initialize vars
 137     lastSysTime = xTaskGetTickCount();
 138
 139     // Main system loop
 140     while (1) {
 141         // Flash the heartbeat LED
 142 #if defined(PIOS_LED_HEARTBEAT)
 143         PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 144 #endif /* PIOS_LED_HEARTBEAT */
 145
 146         // Update the OPLinkStatus UAVO
 147         OPLinkStatusData oplinkStatus;
 148         OPLinkStatusGet(&oplinkStatus);
 149
 150         // Get the other device stats.
 151         PIOS_RFM22B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM);
 152
 153         // Get the stats from the radio device
 154         struct rfm22b_stats radio_stats;
 155         PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
 156
 157         if (pios_rfm22b_id) {
 158             // Update the status
 159             oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
 160             oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
 161             oplinkStatus.RxGood = radio_stats.rx_good;
 162             oplinkStatus.RxCorrected   = radio_stats.rx_corrected;
 163             oplinkStatus.RxErrors = radio_stats.rx_error;
 164             oplinkStatus.RxMissed = radio_stats.rx_missed;
 165             oplinkStatus.RxFailure     = radio_stats.rx_failure;
 166             oplinkStatus.TxDropped     = radio_stats.tx_dropped;
 167             oplinkStatus.TxFailure     = radio_stats.tx_failure;
 168             oplinkStatus.Resets      = radio_stats.resets;
 169             oplinkStatus.Timeouts    = radio_stats.timeouts;
 170             oplinkStatus.RSSI        = radio_stats.rssi;
 171             oplinkStatus.LinkQuality = radio_stats.link_quality;
 172             if (first_time) {
 173                 first_time = false;
 174             } else {
 175                 uint16_t tx_count   = radio_stats.tx_byte_count;
 176                 uint16_t rx_count   = radio_stats.rx_byte_count;
 177                 uint16_t tx_packets = radio_stats.tx_seq - prev_tx_seq;
 178                 uint16_t rx_packets = radio_stats.rx_seq - prev_rx_seq;
 179                 uint16_t tx_bytes   = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
 180                 uint16_t rx_bytes   = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
 181                 oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 182                 oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 183                 oplinkStatus.TXPacketRate = (uint16_t)((float)(tx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 184                 oplinkStatus.RXPacketRate = (uint16_t)((float)(rx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 185                 prev_tx_count = tx_count;
 186                 prev_rx_count = rx_count;
 187                 prev_tx_seq   = radio_stats.tx_seq;
 188                 prev_rx_seq   = radio_stats.rx_seq;
 189             }
 190             oplinkStatus.TXSeq     = radio_stats.tx_seq;
 191             oplinkStatus.RXSeq     = radio_stats.rx_seq;
 192             oplinkStatus.LinkState = radio_stats.link_state;
 193         } else {
 194             oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
 195         }
 196
 197         if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
 198             LINK_LED_ON;
 199         } else {
 200             LINK_LED_OFF;
 201         }
 202
 203         // Update the object
 204         OPLinkStatusSet(&oplinkStatus);
 205
 206         // Wait until next period
 207         vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
 208     }
 209 }
 210
 211 /**
 212  * Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
 213  */
 214 void vApplicationIdleHook(void)
 215 {}
 216
 217 /**
 218  * Called by the RTOS when a stack overflow is detected.
 219  */
 220 #define DEBUG_STACK_OVERFLOW 0
 221 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
 222                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 223 {
 224     stackOverflow = true;
 225 #if DEBUG_STACK_OVERFLOW
 226     static volatile bool wait_here = true;
 227     while (wait_here) {
 228         ;
 229     }
 230     wait_here = true;
 231 #endif
 232 }
 233
 234 /**
 235  * Called by the RTOS when a malloc call fails.
 236  */
 237 #define DEBUG_MALLOC_FAILURES 0
 238 void vApplicationMallocFailedHook(void)
 239 {
 240     mallocFailed = true;
 241 #if DEBUG_MALLOC_FAILURES
 242     static volatile bool wait_here = true;
 243     while (wait_here) {
 244         ;
 245     }
 246     wait_here = true;
 247 #endif
 248 }
 249
 250 /**
 251  * @}
 252  * @}
 253  */