optimise mavlink SS packet size (#3029)
[ExpressLRS.git] / src / lib / HWTIMER / ESP8266_hwTimer.cpp
blob5c0f9acd57b6cacc46088af8a74c1fcf00cab53f
1 #if defined(PLATFORM_ESP8266)
2 #include "hwTimer.h"
4 void (*hwTimer::callbackTick)() = nullptr;
5 void (*hwTimer::callbackTock)() = nullptr;
7 volatile bool hwTimer::running = false;
8 volatile bool hwTimer::isTick = false;
10 volatile uint32_t hwTimer::HWtimerInterval = TimerIntervalUSDefault;
11 volatile int32_t hwTimer::PhaseShift = 0;
12 volatile int32_t hwTimer::FreqOffset = 0;
14 // Internal implementation specific variables
15 static uint32_t NextTimeout;
17 #define HWTIMER_TICKS_PER_US 5
18 #define HWTIMER_PRESCALER (clockCyclesPerMicrosecond() / HWTIMER_TICKS_PER_US)
20 void hwTimer::init(void (*callbackTick)(), void (*callbackTock)())
22 hwTimer::callbackTick = callbackTick;
23 hwTimer::callbackTock = callbackTock;
24 timer0_isr_init();
25 running = false;
28 void hwTimer::stop()
30 if (running)
32 timer0_detachInterrupt();
33 running = false;
37 void ICACHE_RAM_ATTR hwTimer::resume()
39 if (!running)
41 noInterrupts();
42 timer0_attachInterrupt(hwTimer::callback);
43 // We want the timer to fire tock() ASAP after enabling
44 // tock() should always be the first event to maintain consistency
45 isTick = false;
46 #if defined(TARGET_TX)
47 NextTimeout = ESP.getCycleCount() + HWtimerInterval;
48 #else
49 // Fire the timer in 20us to get it started close to the correct starting time and phase.
50 // The 20us delay is requied to allow the transceiver IRQ enough time to return and finish IsrCallback().
51 // If it does not there is a chance that the IRQ will not be cleared and the radio will hang.
52 NextTimeout = ESP.getCycleCount() + (20 * HWTIMER_TICKS_PER_US * HWTIMER_PRESCALER);
53 #endif
54 timer0_write(NextTimeout);
55 interrupts();
57 running = true;
61 void hwTimer::updateInterval(uint32_t newTimerInterval)
63 // timer should not be running when updateInterval() is called
64 HWtimerInterval = newTimerInterval * (HWTIMER_TICKS_PER_US * HWTIMER_PRESCALER);
67 void ICACHE_RAM_ATTR hwTimer::phaseShift(int32_t newPhaseShift)
69 int32_t minVal = -(HWtimerInterval >> 2);
70 int32_t maxVal = (HWtimerInterval >> 2);
72 // phase shift is in microseconds
73 PhaseShift = constrain(newPhaseShift, minVal, maxVal) * (HWTIMER_TICKS_PER_US * HWTIMER_PRESCALER);
76 void ICACHE_RAM_ATTR hwTimer::callback()
78 if (running)
80 #if defined(TARGET_TX)
81 NextTimeout += HWtimerInterval;
82 timer0_write(NextTimeout);
83 callbackTock();
84 #else
85 NextTimeout += (HWtimerInterval >> 1) + (FreqOffset * HWTIMER_PRESCALER);
86 if (hwTimer::isTick)
88 timer0_write(NextTimeout);
89 callbackTick();
91 else
93 NextTimeout += PhaseShift;
94 timer0_write(NextTimeout);
95 PhaseShift = 0;
96 callbackTock();
98 hwTimer::isTick = !hwTimer::isTick;
99 #endif
103 #endif