PR3_robot/Scheduler.hpp at main · Cantonplas/PR3_robot · 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
#pragma once
#include <Arduino.h>
#include <atomic>

#define MAX_TASKS 32
typedef void (*TaskCallback)();

class Scheduler {
private:
    inline static  std::atomic<uint32_t> sys_ticks{0};
    inline static hw_timer_t *timer{nullptr};

    inline static portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;

    inline static constinit uint32_t active_tasks = 0;
    inline static constinit uint32_t oneshot_tasks = 0;

    inline static constinit uint32_t next_run[MAX_TASKS];
    inline static constinit uint32_t periods[MAX_TASKS];

    inline static constinit TaskCallback callbacks[MAX_TASKS];

    static void ARDUINO_ISR_ATTR onTimer() {
        sys_ticks++;
    }

public:
    inline static constexpr uint32_t INVALID_ID = 255;
    static uint32_t get_global_time()
    {
        return sys_ticks;
    }

    static void start() {
        timer = timerBegin(1000000);
        timerAttachInterrupt(timer, &onTimer);
        timerAlarm(timer, 1000, true, 0);  //1000us de prescaler da 1ms por cada tick :p
    }

    static uint32_t register_task(uint32_t period_ms,TaskCallback cb) {
        if (active_tasks == 0xFFFFFFFF) return INVALID_ID;
        portENTER_CRITICAL(&timerMux);
        uint32_t id = __builtin_ctz(~active_tasks);

        active_tasks |= (1 << id);
        oneshot_tasks &= ~(1 << id);
        portEXIT_CRITICAL(&timerMux);
        periods[id] = period_ms;
        next_run[id] = sys_ticks + period_ms;
        callbacks[id] = cb;

        return id;
    }

    static uint32_t set_timeout(uint32_t delay_ms,TaskCallback cb) {
        if (active_tasks == 0xFFFFFFFF) return INVALID_ID;

        portENTER_CRITICAL(&timerMux);
        uint32_t id = __builtin_ctz(~active_tasks);

        active_tasks |= (1 << id);
        oneshot_tasks |= (1 << id);
        portEXIT_CRITICAL(&timerMux);


        periods[id] = delay_ms;
        next_run[id] = sys_ticks + delay_ms;
        callbacks[id] = cb;

        return id;
    }

    static void unregister_task(int id) {
        if (id >= 0 && id < MAX_TASKS) {
          portENTER_CRITICAL(&timerMux);
            active_tasks &= ~(1 << id);
            callbacks[id] = nullptr;
          portEXIT_CRITICAL(&timerMux);
        }
    }

    static void update() {
        uint32_t current_ticks = sys_ticks;
        uint32_t pending_map = active_tasks;

        while (pending_map) {
            int i = __builtin_ctz(pending_map);
            pending_map &= ~(1 << i);

            if ((active_tasks & (1 << i)) == 0) {
                continue;
            }

            if ((int32_t)(current_ticks - next_run[i]) >= 0) {
                next_run[i] = current_ticks + periods[i];

                if (callbacks[i] != nullptr) {
                    callbacks[i]();
                }

                if (oneshot_tasks & (1 << i)) {
                    portENTER_CRITICAL(&timerMux);
                    active_tasks &= ~(1 << i);
                    callbacks[i] = nullptr;
                    portEXIT_CRITICAL(&timerMux);
                }
            }
        }
    }
};