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