Prettier way of driving the control loop

.vscode/settings.json

@@ -3,6 +3,7 @@
     //"C_Cpp.intelliSenseEngine": "Default" // Does not work with "Go to definition" etc.
     "C_Cpp.intelliSenseEngine": "Tag Parser",
     "files.associations": {
-        "cmath": "cpp"
+        "cmath": "cpp",
+        "queue": "cpp"
     }
 }

main.cpp

@@ -19,8 +19,6 @@ using namespace control;
 
 EventQueue queue;
 
-Ticker queueReader;
-
 
 // MPU setup
 SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
@@ -38,8 +36,17 @@ Servo servo(PA_0);
 InterruptIn gyroINT(PC_4);
 
 // Draft of some control function
-void controlFunc(int a)
+// this runs in the context of eventThread and is triggered by the gyro ISR pin
+void controlFunc()
 {
+    static int i = 0;
+    i++;
+    if (i > 100)
+    {
+        i = 0;
+        ledOrg = !ledOrg;
+    }
+
     static incrementalLPF filter;
     double accY = 1;
     accY = imu.read_acc(1);
@@ -66,27 +73,6 @@ void pulseLedContext()
     }
 }
 
-void readQueue()
-{
-    //queue.dispatch(0);
-}
-
-// Interrupt driven function
-// At the moment its running at 100hz
-void ISRGYRO() 
-{
-    static int i = 0;
-    i++;
-    if (i > 100)
-    {
-        i = 0;
-        ledOrg = !ledOrg;
-    }
-    // Report that there is new data to read
-    // TODO: This hack has to be done in a better way
-    queue.call(controlFunc, 0);
-}
-
 int i = 0;
 
 // main() runs in its own thread
@@ -98,8 +84,13 @@ int main() {
     Thread ledPulseThread;
     ledPulseThread.start(callback(&pulseLedContext));
 
-    // Register a event queue reader at set interval
-    queueReader.attach(&readQueue, 0.001);
+    // 
+    Thread eventThread;
+    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
+
+    // Function to attach to gyro interrupt
+    gyroINT.rise(queue.event(&controlFunc));
+
 
     // Enable motor controllers (Will power the motors with no movement)
     motor1.enable();
@@ -109,8 +100,6 @@ int main() {
     // (To test that timers don't interfeer with each other)
     motor2.setSpeed(90.0);
 
-    // Function to attach to gyro interrupt
-    gyroINT.rise(&ISRGYRO);
 
     // MPU startup
     if(imu.init(10,BITS_DLPF_CFG_188HZ)){
@@ -132,13 +121,7 @@ int main() {
 
     servo.sweep(1.0, 1.5, 3);
 
-    servo.setPosition(-0.5);
+    servo.setPosition(-0.2);
 
-    while (true) 
-    {
-        // HACK!!!
-        // Dispatch any content of the event queue.
-        queue.dispatch(0);
-        Thread::wait(1);
-    }
+    wait(osWaitForever);
 }