| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | /** | |||
| 2 | * @file periodic_thread_controller.h | |||
| 3 | * | |||
| 4 | * @date Jan 5, 2019 | |||
| 5 | * @author Matthew Kennedy, (c) 2019 | |||
| 6 | */ | |||
| 7 | ||||
| 8 | #pragma once | |||
| 9 | ||||
| 10 | #include "thread_controller.h" | |||
| 11 | #include "efitime.h" | |||
| 12 | #include "perf_trace.h" | |||
| 13 | ||||
| 14 | /** | |||
| 15 | * @brief Base class for a controller that needs to run periodically to perform work. | |||
| 16 | * | |||
| 17 | * For example, if we have some PID loop that needs to run at a specified frequency, | |||
| 18 | * inherit this class, and perform your period update in PeriodicTask. Any one-time | |||
| 19 | * setup work can be performed in OnStarted(). | |||
| 20 | * | |||
| 21 | * Each instance has one underlying thread meaning that task could be blocking/synchronous. | |||
| 22 | * This class effectively implements this functionality: | |||
| 23 | * | |||
| 24 | * void thread() | |||
| 25 | * { | |||
| 26 | * OnStarted(); | |||
| 27 | * | |||
| 28 | * while(true) | |||
| 29 | * { | |||
| 30 | * PeriodicTask(getTimeNowNt()); | |||
| 31 | * sleep(); | |||
| 32 | * } | |||
| 33 | * } | |||
| 34 | */ | |||
| 35 | template <int TStackSize> | |||
| 36 | class PeriodicController : public ThreadController<TStackSize> | |||
| 37 | { | |||
| 38 | private: | |||
| 39 | // time in ChibiOS time units, see CH_CFG_ST_FREQUENCY | |||
| 40 | systime_t m_period; | |||
| 41 | ||||
| 42 | protected: | |||
| 43 | ||||
| 44 | /** | |||
| 45 | * @brief Called before running the periodic task. Optionally override this method to set up. | |||
| 46 | */ | |||
| 47 | ✗ | virtual void OnStarted() {}; | ||
| 48 | ||||
| 49 | /** | |||
| 50 | * @brief Called periodically. Override this method to do work for your controller. | |||
| 51 | */ | |||
| 52 | virtual void PeriodicTask(efitick_t nowNt) = 0; | |||
| 53 | ||||
| 54 | private: | |||
| 55 | ✗ | void ThreadTask() override final | ||
| 56 | { | |||
| 57 | ✗ | OnStarted(); | ||
| 58 | ||||
| 59 | ✗ | systime_t prev = chVTGetSystemTime(); | ||
| 60 | ✗ | while(!chThdShouldTerminateX()) { | ||
| 61 | ✗ | efitick_t nowNt = getTimeNowNt(); | ||
| 62 | ||||
| 63 | { | |||
| 64 | ✗ | ScopePerf perf(PE::PeriodicControllerPeriodicTask); | ||
| 65 | ||||
| 66 | // Run the controller's periodic work | |||
| 67 | ✗ | PeriodicTask(nowNt); | ||
| 68 | } | |||
| 69 | ||||
| 70 | // This ensures the loop _actually_ runs at the desired frequency. | |||
| 71 | // Suppose we want a loop speed of 500hz: | |||
| 72 | // If the work takes 1ms, and we wait 2ms (1 / 500hz), we actually | |||
| 73 | // get a loop at 333 hz. We need to wait until 2ms after we START | |||
| 74 | // doing work, so the loop runs at a predictable 500hz. | |||
| 75 | ✗ | prev = chThdSleepUntilWindowed(prev, chTimeAddX(prev, m_period)); | ||
| 76 | } | |||
| 77 | ||||
| 78 | ✗ | criticalError("Thread died: %s", this->m_name); | ||
| 79 | ✗ | } | ||
| 80 | ||||
| 81 | public: | |||
| 82 | 3 | PeriodicController(const char* name, tprio_t priority, float frequencyHz) | ||
| 83 | : ThreadController<TStackSize>(name, priority) | |||
| 84 | // First compute the period in systime_t | |||
| 85 | 3 | , m_period(CH_CFG_ST_FREQUENCY / frequencyHz) | ||
| 86 | { | |||
| 87 | 3 | } | ||
| 88 | ||||
| 89 | 1 | PeriodicController(const char* name) : PeriodicController (name, NORMALPRIO, 1) { | ||
| 90 | 1 | } | ||
| 91 | ||||
| 92 | /** | |||
| 93 | * sets milliseconds period | |||
| 94 | */ | |||
| 95 | void setPeriod(int periodMs) { | |||
| 96 | float frequencyHz = 1000.0 / periodMs; | |||
| 97 | this->m_period = CH_CFG_ST_FREQUENCY / frequencyHz; | |||
| 98 | } | |||
| 99 | }; | |||
| 100 | ||||
| 101 | // let's make sure period is not below specified threshold | |||
| 102 | #define NOT_TOO_OFTEN(threshold, value) maxI(threshold, value) | |||
| 103 |