radio/src/tasks_arm.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 OS_TID menusTaskId;
  24 // menus stack must be aligned to 8 bytes otherwise printf for %f does not work!
  25 TaskStack<MENUS_STACK_SIZE> _ALIGNED(8) menusStack;
  26
  27 OS_TID mixerTaskId;
  28 TaskStack<MIXER_STACK_SIZE> mixerStack;
  29
  30 OS_TID audioTaskId;
  31 TaskStack<AUDIO_STACK_SIZE> audioStack;
  32
  33 OS_MutexID audioMutex;
  34 OS_MutexID mixerMutex;
  35
  36 enum TaskIndex {
  37   MENU_TASK_INDEX,
  38   MIXER_TASK_INDEX,
  39   AUDIO_TASK_INDEX,
  40   CLI_TASK_INDEX,
  41   BLUETOOTH_TASK_INDEX,
  42   TASK_INDEX_COUNT,
  43   MAIN_TASK_INDEX = 255
  44 };
  45
  46 template<int SIZE>
  47 void TaskStack<SIZE>::paint()
  48 {
  49   for (uint32_t i=0; i<SIZE; i++) {
  50     stack[i] = 0x55555555;
  51   }
  52 }
  53
  54 uint16_t getStackAvailable(void * address, uint16_t size)
  55 {
  56   uint32_t * array = (uint32_t *)address;
  57   uint16_t i = 0;
  58   while (i < size && array[i] == 0x55555555) {
  59     i++;
  60   }
  61   return i*4;
  62 #if defined(CLI)
  63   cliStackPaint();
  64 #endif
  65 }
  66
  67 void stackPaint()
  68 {
  69   menusStack.paint();
  70   mixerStack.paint();
  71   audioStack.paint();
  72 #if defined(CLI)
  73   cliStack.paint();
  74 #endif
  75 }
  76
  77 #if defined(STM32) && !defined(SIMU)
  78 uint16_t stackSize()
  79 {
  80   return ((unsigned char *)&_estack - (unsigned char *)&_main_stack_start) / 4;
  81 }
  82
  83 uint16_t stackAvailable()
  84 {
  85   return getStackAvailable(&_main_stack_start, stackSize());
  86 }
  87 #endif
  88
  89 volatile uint16_t timeForcePowerOffPressed = 0;
  90
  91 void resetForcePowerOffRequest()
  92 {
  93   timeForcePowerOffPressed = 0;
  94 }
  95
  96 bool isForcePowerOffRequested()
  97 {
  98   if (pwrOffPressed()) {
  99     if (timeForcePowerOffPressed == 0) {
 100       timeForcePowerOffPressed = get_tmr10ms();
 101     }
 102     else {
 103       uint16_t delay = (uint16_t)get_tmr10ms() - timeForcePowerOffPressed;
 104       if (delay > 1000/*10s*/) {
 105         return true;
 106       }
 107     }
 108   }
 109   else {
 110     resetForcePowerOffRequest();
 111   }
 112   return false;
 113 }
 114
 115 uint32_t nextMixerTime[NUM_MODULES];
 116
 117 void mixerTask(void * pdata)
 118 {
 119   static uint32_t lastRunTime;
 120   s_pulses_paused = true;
 121
 122   while(1) {
 123
 124 #if defined(SIMU)
 125     if (main_thread_running == 0)
 126       return;
 127 #endif
 128
 129 #if defined(PCBTARANIS)
 130     processSbusInput();
 131 #endif
 132
 133     CoTickDelay(1);
 134
 135     if (isForcePowerOffRequested()) {
 136       pwrOff();
 137     }
 138
 139     uint32_t now = CoGetOSTime();
 140     bool run = false;
 141 #if !defined(SIMU) && defined(STM32)
 142     if ((now - lastRunTime) >= (usbStarted() ? 5 : 10)) {     // run at least every 20ms (every 10ms if USB is active)
 143 #else
 144     if ((now - lastRunTime) >= 10) {     // run at least every 20ms
 145 #endif
 146       run = true;
 147     }
 148     else if (now == nextMixerTime[0]) {
 149       run = true;
 150     }
 151 #if NUM_MODULES >= 2
 152     else if (now == nextMixerTime[1]) {
 153       run = true;
 154     }
 155 #endif
 156     if (!run) {
 157       continue;  // go back to sleep
 158     }
 159
 160     lastRunTime = now;
 161
 162     if (!s_pulses_paused) {
 163       uint16_t t0 = getTmr2MHz();
 164
 165       DEBUG_TIMER_START(debugTimerMixer);
 166       CoEnterMutexSection(mixerMutex);
 167       doMixerCalculations();
 168       DEBUG_TIMER_START(debugTimerMixerCalcToUsage);
 169       DEBUG_TIMER_SAMPLE(debugTimerMixerIterval);
 170       CoLeaveMutexSection(mixerMutex);
 171       DEBUG_TIMER_STOP(debugTimerMixer);
 172
 173 #if defined(USB_JOYSTICK) && !defined(SIMU)
 174       usbJoystickUpdate();
 175 #endif
 176
 177 #if defined(TELEMETRY_FRSKY) || defined(TELEMETRY_MAVLINK)
 178       DEBUG_TIMER_START(debugTimerTelemetryWakeup);
 179       telemetryWakeup();
 180       DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
 181 #endif
 182
 183 #if defined(BLUETOOTH)
 184       bluetoothWakeup();
 185 #endif
 186
 187       if (heartbeat == HEART_WDT_CHECK) {
 188         wdt_reset();
 189         heartbeat = 0;
 190       }
 191
 192       t0 = getTmr2MHz() - t0;
 193       if (t0 > maxMixerDuration) maxMixerDuration = t0 ;
 194     }
 195   }
 196 }
 197
 198 void scheduleNextMixerCalculation(uint8_t module, uint16_t delay)
 199 {
 200   // Schedule next mixer calculation time,
 201   // for now assume mixer calculation takes 2 ms.
 202   nextMixerTime[module] = (uint32_t)CoGetOSTime() + (delay)/2 - 1/*2ms*/;
 203   DEBUG_TIMER_STOP(debugTimerMixerCalcToUsage);
 204 }
 205
 206 #define MENU_TASK_PERIOD_TICKS      25    // 50ms
 207
 208 #if defined(COLORLCD) && defined(CLI)
 209 bool perMainEnabled = true;
 210 #endif
 211
 212 void menusTask(void * pdata)
 213 {
 214   opentxInit();
 215
 216 #if defined(PWR_BUTTON_PRESS)
 217   while (1) {
 218     uint32_t pwr_check = pwrCheck();
 219     if (pwr_check == e_power_off) {
 220       break;
 221     }
 222     else if (pwr_check == e_power_press) {
 223       CoTickDelay(MENU_TASK_PERIOD_TICKS);
 224       continue;
 225     }
 226 #else
 227   while (pwrCheck() != e_power_off) {
 228 #endif
 229     uint32_t start = (uint32_t)CoGetOSTime();
 230     DEBUG_TIMER_START(debugTimerPerMain);
 231 #if defined(COLORLCD) && defined(CLI)
 232     if (perMainEnabled) {
 233       perMain();
 234     }
 235 #else
 236     perMain();
 237 #endif
 238     DEBUG_TIMER_STOP(debugTimerPerMain);
 239     // TODO remove completely massstorage from sky9x firmware
 240     uint32_t runtime = ((uint32_t)CoGetOSTime() - start);
 241     // deduct the thread run-time from the wait, if run-time was more than
 242     // desired period, then skip the wait all together
 243     if (runtime < MENU_TASK_PERIOD_TICKS) {
 244       CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime);
 245     }
 246
 247     resetForcePowerOffRequest();
 248
 249 #if defined(SIMU)
 250     if (main_thread_running == 0)
 251       break;
 252 #endif
 253   }
 254
 255 #if defined(PCBX9E)
 256   toplcdOff();
 257 #endif
 258
 259 #if defined(PCBHORUS)
 260   ledOff();
 261 #endif
 262
 263   drawSleepBitmap();
 264   opentxClose();
 265   boardOff(); // Only turn power off if necessary
 266 }
 267
 268 void tasksStart()
 269 {
 270   CoInitOS();
 271
 272 #if defined(CLI)
 273   cliStart();
 274 #endif
 275
 276   mixerTaskId = CoCreateTask(mixerTask, NULL, 5, &mixerStack.stack[MIXER_STACK_SIZE-1], MIXER_STACK_SIZE);
 277   menusTaskId = CoCreateTask(menusTask, NULL, 10, &menusStack.stack[MENUS_STACK_SIZE-1], MENUS_STACK_SIZE);
 278
 279 #if !defined(SIMU)
 280   // TODO move the SIMU audio in this task
 281   audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack.stack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE);
 282 #endif
 283
 284   audioMutex = CoCreateMutex();
 285   mixerMutex = CoCreateMutex();
 286
 287   CoStartOS();
 288 }