periodic_thread_controller.h

Go to the documentation of this file.

/**
 *  @file periodic_thread_controller.h
 *
 *  @date Jan 5, 2019
 *  @author Matthew Kennedy, (c) 2019
 */
 
#pragma once
 
#include "thread_controller.h"
#include "efitime.h"
#include "perf_trace.h"
 
/**
 * @brief Base class for a controller that needs to run periodically to perform work.
 *
 * For example, if we have some PID loop that needs to run at a specified frequency,
 * inherit this class, and perform your period update in PeriodicTask.  Any one-time
 * setup work can be performed in OnStarted().
 *
 * Each instance has one underlying thread meaning that task could be blocking/synchronous.
 * This class effectively implements this functionality:
 *
 * void thread()
 * {
 *     OnStarted();
 *
 *     while(true)
 *     {
 *         PeriodicTask(getTimeNowNt());
 *         sleep();
 *     }
 * }
 */
template <int TStackSize>
class PeriodicController : public ThreadController<TStackSize>
{
private:
    // time in ChibiOS time units, see CH_CFG_ST_FREQUENCY
    systime_t m_period;
 
protected:
 
    /**
     * @brief Called before running the periodic task.  Optionally override this method to set up.
     */
    virtual void OnStarted() {};
 
    /**
     * @brief Called periodically.  Override this method to do work for your controller.
     */
    virtual void PeriodicTask(efitick_t nowNt) = 0;
 
private:
    void ThreadTask() override final
    {
        OnStarted();
 
        systime_t prev = chVTGetSystemTime();
        while(!chThdShouldTerminateX()) {
            efitick_t nowNt = getTimeNowNt();
 
            {
                ScopePerf perf(PE::PeriodicControllerPeriodicTask);
 
                // Run the controller's periodic work
                PeriodicTask(nowNt);
            }
 
            // This ensures the loop _actually_ runs at the desired frequency.
            // Suppose we want a loop speed of 500hz:
            // If the work takes 1ms, and we wait 2ms (1 / 500hz), we actually
            // get a loop at 333 hz.  We need to wait until 2ms after we START
            // doing work, so the loop runs at a predictable 500hz.
            prev = chThdSleepUntilWindowed(prev, chTimeAddX(prev, m_period));
        }
 
        criticalError("Thread died: %s", this->m_name);
    }
 
public:
    PeriodicController(const char* name, tprio_t priority, float frequencyHz)
        : ThreadController<TStackSize>(name, priority)
        // First compute the period in systime_t
        , m_period(CH_CFG_ST_FREQUENCY / frequencyHz)
    {
    }
 
    PeriodicController(const char* name) : PeriodicController (name, NORMALPRIO, 1) {
    }
 
    /**
     * sets milliseconds period
     */
    void setPeriod(int periodMs) {
        float frequencyHz = 1000.0 / periodMs;
        this->m_period = CH_CFG_ST_FREQUENCY / frequencyHz;
    }
};
 
// let's make sure period is not below specified threshold
#define NOT_TOO_OFTEN(threshold, value) maxI(threshold, value)