Implement common for controllers (#645)

* controller

* remove stopping thread

* move microsecond timer watchdog

* docs

* typo snuck in

* go back to the old way

firmware/controllers/system/ControllerBase.h

@@ -0,0 +1,23 @@
+/**
+ *  @file ControllerBase.h
+ * 
+ *  @date Jan 5, 2019
+ *  @author Matthew Kennedy, (c) 2019
+ */
+
+#pragma once
+
+// Sparse for now...
+class ControllerBase
+{
+private:
+    const char* m_name;
+
+public:
+    ControllerBase(const char* name)
+        : m_name(name)
+    {
+    }
+
+    const char* GetName() const { return m_name; }
+};

firmware/controllers/system/PeriodicController.h

@@ -0,0 +1,88 @@
+/**
+ *  @file PeriodicController.h
+ * 
+ *  @date Jan 5, 2019
+ *  @author Matthew Kennedy, (c) 2019
+ */
+
+#pragma once
+
+#include "ThreadController.h"
+#include "efitime.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().
+ * 
+ * This class effectively implements this funtionality:
+ * 
+ * void thread()
+ * {
+ *     OnStarted();
+ * 
+ *     while(true)
+ *     {
+ *         PeriodicTask(getTimeNowNt());
+ *         sleep();
+ *     }
+ * }
+ */
+template <int TStackSize>
+class PeriodicController : public ThreadController<TStackSize>
+{
+private:
+    const systime_t m_period;
+    
+protected:
+    /**
+     * The target period between calls to PeriodicTask.
+     */
+    const float m_periodSeconds;
+
+    /**
+     * @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(efitime_t nowNt) = 0;
+
+private:
+    void ThreadTask() override final
+    {
+        OnStarted();
+
+        while(true)
+        {
+            systime_t before = chVTGetSystemTime();
+            efitime_t nowNt = getTimeNowNt();
+
+            // 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.
+            chThdSleepUntilWindowed(before, before + m_period);
+        }
+    }
+
+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)
+        // Then compute the float period off of the integer one to
+        //  get the ACTUAL period, which may be slightly different than requested.
+        , m_periodSeconds(m_period / (float)CH_CFG_ST_FREQUENCY)
+    {
+    }
+};

firmware/controllers/system/ThreadController.h

@@ -0,0 +1,62 @@
+/**
+ *  @file ThreadController.h
+ * 
+ *  @date Jan 5, 2019
+ *  @author Matthew Kennedy, (c) 2019
+ */
+
+#pragma once
+
+#include "ControllerBase.h"
+
+#include "ch.h"
+
+/**
+ * @brief A base class for a controller that requires its own thread.
+ * 
+ * Inherit from ThreadController.  Implement ThreadTask with the logic required for your thread.
+ * The template parameter specifies the size of the stack used for the thread.  (because we have to
+ * allocate the stack at compile time, it has to be a template parameter instead of a normal parameter)
+ */
+template <int TStackSize>
+class ThreadController : public ControllerBase
+{
+private:
+    THD_WORKING_AREA(m_threadstack, TStackSize);
+    const tprio_t m_prio;
+
+    /**
+     * The OS can only call a function with a single void* param.  We have
+     * to convert back to an instance of ThreadController, and call the task to run.
+     */
+    static void StaticThreadTaskAdapter(void* thread)
+    {
+        ThreadController* t = static_cast<ThreadController*>(thread);
+
+        // Run our thread task
+        t->ThreadTask();
+    }
+
+protected:
+    /**
+     * Override this function to implement your controller's thread's behavior.
+     */
+    virtual void ThreadTask() = 0;
+    thread_t* m_thread;
+
+public:
+    ThreadController(const char* name, tprio_t priority)
+        : ControllerBase(name)
+        , m_prio(priority)
+    {
+    }
+
+    /**
+     * @brief Start the thread.
+     */
+    virtual void Start()
+    {
+        m_thread = chThdCreateStatic(m_threadstack, sizeof(m_threadstack), m_prio, StaticThreadTaskAdapter, this);
+        m_thread->name = GetName();
+    }
+};

firmware/hw_layer/microsecond_timer.cpp

@@ -15,6 +15,7 @@
 #include "microsecond_timer.h"
 #include "scheduler.h"
 #include "rfiutil.h"
+#include "PeriodicController.h"
 
 // https://my.st.com/public/STe2ecommunities/mcu/Lists/cortex_mx_stm32/Flat.aspx?RootFolder=https%3a%2f%2fmy.st.com%2fpublic%2fSTe2ecommunities%2fmcu%2fLists%2fcortex_mx_stm32%2fInterrupt%20on%20CEN%20bit%20setting%20in%20TIM7&FolderCTID=0x01200200770978C69A1141439FE559EB459D7580009C4E14902C3CDE46A77F0FFD06506F5B&currentviews=474
 
@@ -113,36 +114,34 @@ static void callback(GPTDriver *gptp) {
 	}
 }
 
-static void usTimerWatchDog(void) {
-	if (getTimeNowNt() >= lastSetTimerTimeNt + 2 * CORE_CLOCK) {
-		strcpy(buff, "no_event");
-		itoa10(&buff[8], lastSetTimerValue);
-		firmwareError(CUSTOM_ERR_SCHEDULING_ERROR, buff);
-		return;
+
+class MicrosecondTimerWatchdogController : public PeriodicController<UTILITY_THREAD_STACK_SIZE>
+{
+public:
+	MicrosecondTimerWatchdogController()
+		: PeriodicController("timer watchdog", NORMALPRIO, 1.0f)
+	{
 	}
 
-	msg = isTimerPending ? "No_cb too long" : "Timer not awhile";
-	// 2 seconds of inactivity would not look right
-	efiAssertVoid(CUSTOM_ERR_6682, getTimeNowNt() < lastSetTimerTimeNt + 2 * CORE_CLOCK, msg);
+private:
+	void PeriodicTask(efitime_t nowNt) override
+	{
+		if (nowNt >= lastSetTimerTimeNt + 2 * CORE_CLOCK) {
+			strcpy(buff, "no_event");
+			itoa10(&buff[8], lastSetTimerValue);
+			firmwareError(CUSTOM_ERR_SCHEDULING_ERROR, buff);
+			return;
+		}
 
-}
-
-static msg_t mwThread(int param) {
-	(void)param;
-	chRegSetThreadName("timer watchdog");
-
-	while (true) {
-		chThdSleepMilliseconds(1000); // once a second is enough
-		usTimerWatchDog();
+		msg = isTimerPending ? "No_cb too long" : "Timer not awhile";
+		// 2 seconds of inactivity would not look right
+		efiAssertVoid(CUSTOM_ERR_6682, nowNt < lastSetTimerTimeNt + 2 * CORE_CLOCK, msg);
 	}
-#if defined __GNUC__
-	return -1;
-#endif        
-}
+};
 
-//static const GPTConfig gpt5cfg;
+MicrosecondTimerWatchdogController watchdogControllerInstance;
 
-static const GPTConfig gpt5cfg = { 1000000, /* 1 MHz timer clock.*/
+static constexpr GPTConfig gpt5cfg = { 1000000, /* 1 MHz timer clock.*/
 callback, /* Timer callback.*/
 0, 0 };
 
@@ -152,7 +151,7 @@ void initMicrosecondTimer(void) {
 
 	lastSetTimerTimeNt = getTimeNowNt();
 #if EFI_EMULATE_POSITION_SENSORS
-	chThdCreateStatic(mwThreadStack, sizeof(mwThreadStack), NORMALPRIO, (tfunc_t)(void*) mwThread, NULL);
+	watchdogControllerInstance.Start();
 #endif /* EFI_ENGINE_EMULATOR */
 
 //	// test code