Backpack/src/module_pedal.cpp at 865dd306565236a2e186fdc25ed4841b06bd3d97 · ExpressLRS/Backpack · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
#if defined(PEDAL_BACKPACK)

#include "module_pedal.h"
#include "common.h"
#include "msptypes.h"
#if defined(PIN_LED)
  #include "devLED.h"
#endif

void sendMSPViaEspnow(mspPacket_t *packet);
bool BindingExpired(uint32_t now);

PedalModule::PedalModule()
 :  _lastTxValue(false), _lastNow(0), _lastTxMs(0), _lastPedalChangeMs(0)
{
    _btnBind.OnLongPress = std::bind(&PedalModule::button_OnLongPress, this);
}

PedalModule::~PedalModule()
{
    detachInterrupt(PIN_BUTTON_PEDAL);
}

void PedalModule::Init()
{
    _pedalSemaphore = xSemaphoreCreateBinary();
    attachInterruptArg(PIN_BUTTON_PEDAL, &PedalModule::button_interrupt, this, CHANGE);
}

void PedalModule::Loop(uint32_t now)
{
    // Need to store what the main loop thinks is "now" in case the button
    // event is going to use it to set the bindstart time, otherwise millis()
    // could be in the future (compared to now) and timeout immediately
    _lastNow = now;
    if (BindingExpired(_lastNow))
    {
        connectionState = running;
    }

    // Pause here to lower power usage (C3 drops from 100mA to 90mA)
    // If button is currently transitioning or pressed just consume the semaphore, else delay until an interrupt
    xSemaphoreTake(_pedalSemaphore, _btnPedal.isIdle() ? pdMS_TO_TICKS(20) : 0);
    _btnPedal.update();
    _btnBind.update();

    // Don't TX if binding/wifi
    if (connectionState != running)
        return;
    // Don't TX on startup
    if (_lastTxMs == 0 && _lastNow < STARTUP_MS)
        return;

    checkSendPedalPos();
}

void PedalModule::checkSendPedalPos()
{
    bool pedalPressed = _btnPedal.isPressed();
    bool pedalChanged = pedalPressed != _lastTxValue;

    if (pedalChanged)
    {
        _lastPedalChangeMs = _lastNow;
    }

    // Send the pedal position frequently after last change, until it is unchanged for one UNCHANGED_MS interval
    uint32_t txIntervalMs = (_lastNow - _lastPedalChangeMs < PEDAL_INTERVAL_UNCHANGED_MS) ? PEDAL_INTERVAL_CHANGED_MS : PEDAL_INTERVAL_UNCHANGED_MS;
    if (pedalChanged || _lastNow - _lastTxMs > txIntervalMs)
    {
        _lastTxMs = _lastNow;
        _lastTxValue = pedalPressed;
        DBGLN("%u pedal %u", _lastNow, pedalPressed);

        // Pedal is 1000us if not pressed, 2000us if pressed
        uint16_t pedalState = pedalPressed ? CRSF_CHANNEL_VALUE_2000 : CRSF_CHANNEL_VALUE_1000;
        mspPacket_t packet;
        packet.reset();
        packet.makeCommand();
        packet.function = MSP_ELRS_BACKPACK_SET_PTR;
        packet.addByte(pedalState & 0xFF); // CH0
        packet.addByte(pedalState >> 8);
        // ExpressLRS 4.0 supports only sending as many channels as needed, but for 3.x compatibility
        // 6 total payload bytes are required. A channel value of 0xffff means "do not update" in 4.0
        packet.addByte(0xff); // CH1
        packet.addByte(0xff);
        packet.addByte(0xff); // CH2
        packet.addByte(0xff);
        sendMSPViaEspnow(&packet);
        yield();

#if defined(PIN_LED)
        // Flash the LED to indicate the high-speed pedal update mode
        if (pedalPressed)
            blinkLED();
#endif
    }
}

void IRAM_ATTR PedalModule::button_interrupt(void *instance)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xSemaphoreGiveFromISR(((PedalModule *)instance)->_pedalSemaphore, &xHigherPriorityTaskWoken);
    if (xHigherPriorityTaskWoken)
        portYIELD_FROM_ISR();
}

void PedalModule::button_OnLongPress()
{
    // Long press of the bind button for 5s takes the pedal into binding mode
    if (_btnBind.getLongCount() >= 10 && connectionState == running)
    {
        bindingStart = _lastNow;
        connectionState = binding;
    }
    // Long press of bind button for 10s goes from binding to wifi
    else if (_btnBind.getLongCount() >= 20 && connectionState == binding)
    {
        connectionState = wifiUpdate;
    }
}

#endif /* PEDAL_BACKPACK */