src/main/drivers/rx/expresslrs_driver.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  * Based on https://github.com/ExpressLRS/ExpressLRS
  23  * Thanks to AlessandroAU, original creator of the ExpressLRS project.
  24  *
  25  * Authors:
  26  * Dominic Clifton/Hydra - Timer-based timeout implementation.
  27  * AlessandroAU - stdperiph Timer-based timeout implementation.
  28  */
  29
  30 #include <string.h>
  31 #include "platform.h"
  32
  33 #ifdef USE_RX_EXPRESSLRS
  34
  35 #include "build/debug.h"
  36 #include "build/debug_pin.h"
  37
  38 #include "drivers/timer.h"
  39 #include "drivers/nvic.h"
  40 #include "drivers/rx/expresslrs_driver.h"
  41
  42 #include "common/maths.h"
  43
  44 #define TIMER_INTERVAL_US_DEFAULT 20000
  45 #define TICK_TOCK_COUNT 2
  46
  47 TIM_TypeDef *timer;
  48
  49 typedef enum {
  50     TICK,
  51     TOCK
  52 } tickTock_e;
  53
  54 typedef struct elrsTimerState_s {
  55     bool running;
  56     volatile tickTock_e tickTock;
  57     uint32_t intervalUs;
  58     int32_t frequencyOffsetTicks;
  59     int32_t phaseShiftUs;
  60 } elrsTimerState_t;
  61
  62 // Use a little ram to keep the amount of CPU cycles used in the ISR lower.
  63 typedef struct elrsPhaseShiftLimits_s {
  64     int32_t min;
  65     int32_t max;
  66 } elrsPhaseShiftLimits_t;
  67
  68 elrsPhaseShiftLimits_t phaseShiftLimits;
  69
  70 static elrsTimerState_t timerState = {
  71     false,
  72     TOCK, // Start on TOCK (in ELRS isTick is initialised to false)
  73     TIMER_INTERVAL_US_DEFAULT,
  74     0,
  75     0
  76 };
  77
  78 void expressLrsTimerDebug(void)
  79 {
  80     DEBUG_SET(DEBUG_RX_EXPRESSLRS_PHASELOCK, 2, timerState.frequencyOffsetTicks);
  81     DEBUG_SET(DEBUG_RX_EXPRESSLRS_PHASELOCK, 3, timerState.phaseShiftUs);
  82 }
  83
  84 static void expressLrsRecalculatePhaseShiftLimits(void)
  85 {
  86     phaseShiftLimits.max = (timerState.intervalUs / TICK_TOCK_COUNT);
  87     phaseShiftLimits.min = -phaseShiftLimits.max;
  88 }
  89
  90 static uint16_t expressLrsCalculateMaximumExpectedPeriod(uint16_t intervalUs)
  91 {
  92     // The timer reload register must not overflow when frequencyOffsetTicks is added to it.
  93     // frequencyOffsetTicks is not expected to be higher than 1/4 of the interval.
  94     // also, timer resolution must be as high as possible.
  95     const uint16_t maximumExpectedPeriod = (intervalUs / TICK_TOCK_COUNT) + (timerState.intervalUs / 4);
  96     return maximumExpectedPeriod;
  97 }
  98
  99 void expressLrsUpdateTimerInterval(uint16_t intervalUs)
 100 {
 101     timerState.intervalUs = intervalUs;
 102     expressLrsRecalculatePhaseShiftLimits();
 103
 104 #ifdef USE_HAL_DRIVER
 105     timerReconfigureTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1));
 106     LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1);
 107 #else
 108     configTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1));
 109     TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1);
 110 #endif
 111 }
 112
 113 void expressLrsUpdatePhaseShift(int32_t newPhaseShift)
 114 {
 115     timerState.phaseShiftUs = constrain(newPhaseShift, phaseShiftLimits.min, phaseShiftLimits.max);
 116 }
 117
 118 void expressLrsTimerIncreaseFrequencyOffset(void)
 119 {
 120     timerState.frequencyOffsetTicks++;
 121 }
 122
 123 void expressLrsTimerDecreaseFrequencyOffset(void)
 124 {
 125     timerState.frequencyOffsetTicks--;
 126 }
 127
 128 void expressLrsTimerResetFrequencyOffset(void)
 129 {
 130     timerState.frequencyOffsetTicks = 0;
 131 }
 132
 133 static void expressLrsOnTimerUpdate(timerOvrHandlerRec_t *cbRec, captureCompare_t capture)
 134 {
 135     UNUSED(cbRec);
 136     UNUSED(capture);
 137
 138     if (timerState.tickTock == TICK) {
 139         dbgPinHi(0);
 140
 141         uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.frequencyOffsetTicks;
 142
 143 #ifdef USE_HAL_DRIVER
 144         LL_TIM_SetAutoReload(timer, adjustedPeriod - 1);
 145 #else
 146         TIM_SetAutoreload(timer, adjustedPeriod - 1);
 147 #endif
 148
 149         expressLrsOnTimerTickISR();
 150
 151         timerState.tickTock = TOCK;
 152     } else {
 153         dbgPinLo(0);
 154
 155         uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.phaseShiftUs + timerState.frequencyOffsetTicks;
 156
 157 #ifdef USE_HAL_DRIVER
 158         LL_TIM_SetAutoReload(timer, adjustedPeriod - 1);
 159 #else
 160         TIM_SetAutoreload(timer, adjustedPeriod - 1);
 161 #endif
 162
 163         timerState.phaseShiftUs = 0;
 164
 165         expressLrsOnTimerTockISR();
 166
 167         timerState.tickTock = TICK;
 168     }
 169 }
 170
 171 bool expressLrsTimerIsRunning(void)
 172 {
 173     return timerState.running;
 174 }
 175
 176 void expressLrsTimerStop(void)
 177 {
 178 #ifdef USE_HAL_DRIVER
 179     LL_TIM_DisableIT_UPDATE(timer);
 180     LL_TIM_DisableCounter(timer);
 181     LL_TIM_SetCounter(timer, 0);
 182 #else
 183     TIM_ITConfig(timer, TIM_IT_Update, DISABLE);
 184     TIM_Cmd(timer, DISABLE);
 185     TIM_SetCounter(timer, 0);
 186 #endif
 187     timerState.running = false;
 188 }
 189
 190 void expressLrsTimerResume(void)
 191 {
 192     timerState.tickTock = TOCK;
 193
 194 #ifdef USE_HAL_DRIVER
 195     LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT));
 196     LL_TIM_SetCounter(timer, 0);
 197
 198     LL_TIM_ClearFlag_UPDATE(timer);
 199     LL_TIM_EnableIT_UPDATE(timer);
 200 #else
 201     TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT));
 202     TIM_SetCounter(timer, 0);
 203
 204     TIM_ClearFlag(timer, TIM_FLAG_Update);
 205     TIM_ITConfig(timer, TIM_IT_Update, ENABLE);
 206 #endif
 207
 208     timerState.running = true;
 209
 210 #ifdef USE_HAL_DRIVER
 211     LL_TIM_EnableCounter(timer);
 212     LL_TIM_GenerateEvent_UPDATE(timer);
 213 #else
 214     TIM_Cmd(timer, ENABLE);
 215     TIM_GenerateEvent(timer, TIM_EventSource_Update);
 216 #endif
 217 }
 218
 219 void expressLrsInitialiseTimer(TIM_TypeDef *t, timerOvrHandlerRec_t *timerUpdateCb)
 220 {
 221     timer = t;
 222
 223     configTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1));
 224
 225     expressLrsUpdateTimerInterval(timerState.intervalUs);
 226
 227     timerChOvrHandlerInit(timerUpdateCb, expressLrsOnTimerUpdate);
 228
 229     timerConfigUpdateCallback(timer, timerUpdateCb);
 230 }
 231
 232 void expressLrsTimerEnableIRQs(void)
 233 {
 234     uint8_t irq = timerInputIrq(timer);
 235
 236     // Use the NVIC TIMER priority for now
 237 #ifdef USE_HAL_DRIVER
 238     HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
 239     HAL_NVIC_EnableIRQ(irq);
 240 #else
 241     NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER));
 242     NVIC_EnableIRQ(irq);
 243 #endif
 244 }
 245
 246 #endif