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 bool isForcePowerOffRequested()
  92 {
  93   if (pwrOffPressed()) {
  94     if (timeForcePowerOffPressed == 0) {
  95       timeForcePowerOffPressed = get_tmr10ms();
  96     }
  97     else {
  98       uint16_t delay = (uint16_t)get_tmr10ms() - timeForcePowerOffPressed;
  99       if (delay > 1000/*10s*/) {
 100         return true;
 101       }
 102     }
 103   }
 104   else {
 105     resetForcePowerOffRequest();
 106   }
 107   return false;
 108 }
 109
 110 uint32_t nextMixerTime[NUM_MODULES];
 111
 112 void mixerTask(void * pdata)
 113 {
 114   static uint32_t lastRunTime;
 115   s_pulses_paused = true;
 116
 117   while(1) {
 118
 119 #if defined(SIMU)
 120     if (main_thread_running == 0)
 121       return;
 122 #endif
 123
 124 #if defined(SBUS)
 125     processSbusInput();
 126 #endif
 127
 128     CoTickDelay(1);
 129
 130     if (isForcePowerOffRequested()) {
 131       pwrOff();
 132     }
 133
 134     uint32_t now = CoGetOSTime();
 135     bool run = false;
 136 #if !defined(SIMU) && defined(STM32)
 137     if ((now - lastRunTime) >= (usbStarted() ? 5 : 10)) {     // run at least every 20ms (every 10ms if USB is active)
 138 #else
 139     if ((now - lastRunTime) >= 10) {     // run at least every 20ms
 140 #endif
 141       run = true;
 142     }
 143     else if (now == nextMixerTime[0]) {
 144       run = true;
 145     }
 146 #if NUM_MODULES >= 2
 147     else if (now == nextMixerTime[1]) {
 148       run = true;
 149     }
 150 #endif
 151     if (!run) {
 152       continue;  // go back to sleep
 153     }
 154
 155     lastRunTime = now;
 156
 157     if (!s_pulses_paused) {
 158       uint16_t t0 = getTmr2MHz();
 159
 160       DEBUG_TIMER_START(debugTimerMixer);
 161       CoEnterMutexSection(mixerMutex);
 162       doMixerCalculations();
 163       DEBUG_TIMER_START(debugTimerMixerCalcToUsage);
 164       DEBUG_TIMER_SAMPLE(debugTimerMixerIterval);
 165       CoLeaveMutexSection(mixerMutex);
 166       DEBUG_TIMER_STOP(debugTimerMixer);
 167
 168 #if defined(STM32) && !defined(SIMU)
 169       if (getSelectedUsbMode() == USB_JOYSTICK_MODE) {
 170         usbJoystickUpdate();
 171       }
 172 #endif
 173
 174 #if defined(TELEMETRY_FRSKY) || defined(TELEMETRY_MAVLINK)
 175       DEBUG_TIMER_START(debugTimerTelemetryWakeup);
 176       telemetryWakeup();
 177       DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
 178 #endif
 179
 180 #if defined(BLUETOOTH)
 181       bluetoothWakeup();
 182 #endif
 183
 184       if (heartbeat == HEART_WDT_CHECK) {
 185         wdt_reset();
 186         heartbeat = 0;
 187       }
 188
 189       t0 = getTmr2MHz() - t0;
 190       if (t0 > maxMixerDuration) maxMixerDuration = t0 ;
 191     }
 192   }
 193 }
 194
 195 void scheduleNextMixerCalculation(uint8_t module, uint16_t period_ms)
 196 {
 197   // Schedule next mixer calculation time,
 198   // for now assume mixer calculation takes 2 ms.
 199   nextMixerTime[module] = (uint32_t)CoGetOSTime() + period_ms / 2 - 1/*2ms*/;
 200   DEBUG_TIMER_STOP(debugTimerMixerCalcToUsage);
 201 }
 202
 203 #define MENU_TASK_PERIOD_TICKS      25    // 50ms
 204
 205 #if defined(COLORLCD) && defined(CLI)
 206 bool perMainEnabled = true;
 207 #endif
 208
 209 void menusTask(void * pdata)
 210 {
 211   opentxInit();
 212
 213 #if defined(PWR_BUTTON_PRESS)
 214   while (1) {
 215     uint32_t pwr_check = pwrCheck();
 216     if (pwr_check == e_power_off) {
 217       break;
 218     }
 219     else if (pwr_check == e_power_press) {
 220       CoTickDelay(MENU_TASK_PERIOD_TICKS);
 221       continue;
 222     }
 223 #else
 224   while (pwrCheck() != e_power_off) {
 225 #endif
 226     uint32_t start = (uint32_t)CoGetOSTime();
 227     DEBUG_TIMER_START(debugTimerPerMain);
 228 #if defined(COLORLCD) && defined(CLI)
 229     if (perMainEnabled) {
 230       perMain();
 231     }
 232 #else
 233     perMain();
 234 #endif
 235     DEBUG_TIMER_STOP(debugTimerPerMain);
 236     // TODO remove completely massstorage from sky9x firmware
 237     uint32_t runtime = ((uint32_t)CoGetOSTime() - start);
 238     // deduct the thread run-time from the wait, if run-time was more than
 239     // desired period, then skip the wait all together
 240     if (runtime < MENU_TASK_PERIOD_TICKS) {
 241       CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime);
 242     }
 243
 244     resetForcePowerOffRequest();
 245
 246 #if defined(SIMU)
 247     if (main_thread_running == 0)
 248       break;
 249 #endif
 250   }
 251
 252 #if defined(PCBX9E)
 253   toplcdOff();
 254 #endif
 255
 256 #if defined(PCBHORUS)
 257   ledOff();
 258 #endif
 259
 260   drawSleepBitmap();
 261   opentxClose();
 262   boardOff(); // Only turn power off if necessary
 263 }
 264
 265 void tasksStart()
 266 {
 267   CoInitOS();
 268
 269 #if defined(CLI)
 270   cliStart();
 271 #endif
 272
 273   mixerTaskId = CoCreateTask(mixerTask, NULL, 5, &mixerStack.stack[MIXER_STACK_SIZE-1], MIXER_STACK_SIZE);
 274   menusTaskId = CoCreateTask(menusTask, NULL, 10, &menusStack.stack[MENUS_STACK_SIZE-1], MENUS_STACK_SIZE);
 275
 276 #if !defined(SIMU)
 277   // TODO move the SIMU audio in this task
 278   audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack.stack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE);
 279 #endif
 280
 281   audioMutex = CoCreateMutex();
 282   mixerMutex = CoCreateMutex();
 283
 284   CoStartOS();
 285 }