Implement on demand I2C task on primary ESP32

src/common/shared_structs.h

@@ -29,6 +29,6 @@ typedef struct {
     motor_parameters_t motor_1_params;
     motor_parameters_t motor_2_params;
     motor_parameters_t motor_3_params;
-} i2c_status_packet_t;
+} motor_status_packet_t;
 
 #endif

src/motor-esp32/i2c_comms.cpp

@@ -11,7 +11,7 @@
 
 
 
-i2c_status_packet_t i2c_reply;
+motor_status_packet_t i2c_reply;
 
 void on_I2C_request() {
     // Hopefully because most of our data is pulled in with queues, rather than larger locks,

src/primary-esp32/i2c_comms.cpp

@@ -0,0 +1,67 @@
+
+#include "Arduino.h"
+#include "Wire.h"
+#include "io.h"
+
+#include "i2c_comms.h"
+
+/*
+This module is mostly a task that waits until any I2C reads or writes are required, then performs them
+while avoiding blocking the main networking activity.
+*/
+
+#define MOTOR_ESP32_SLAVE_I2C_ADDR 0x55
+#define I2C_FREQUENCY              100000
+
+#define I2C_TASK_STACK_SIZE 2048
+
+static TaskHandle_t i2c_comms_task_handle = NULL;
+
+static SemaphoreHandle_t motor_status_semaphore;
+static xQueueHandle motor_status_queue;
+
+motor_status_packet_t i2c_reply;
+
+// Ask the I2C task to get get a status update from the motor ESP32
+void request_motor_status_update() {
+    xSemaphoreGive(motor_status_semaphore);
+    xTaskNotifyGive(i2c_comms_task_handle);
+}
+
+// Get a motor status update, if one exists. Returns True if a new update was copied out.
+bool get_motor_status_update(motor_status_packet_t *status_packet) {
+    return (xQueueReceive(motor_status_queue, status_packet, 0) == pdTRUE);
+}
+
+static void i2c_comms_task(void *pvParameters) {
+    uint8_t reply_bytecount;
+
+    for (;;) {
+        // Only need to run this task when there is something to do on the I2C bus
+        ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
+
+        // Get a motor status update if requested
+        if (xSemaphoreTake(motor_status_semaphore, 0) == pdTRUE) {
+            reply_bytecount = Wire.requestFrom(MOTOR_ESP32_SLAVE_I2C_ADDR, sizeof(i2c_reply));
+            if (reply_bytecount == sizeof(i2c_reply)) {
+                Wire.readBytes((byte *)&i2c_reply, reply_bytecount);
+                // Pass the motor status back to the main task
+                xQueueOverwrite(motor_status_queue, &i2c_reply);
+            } else {
+                // I2C read error of some sort
+            }
+        }
+    }
+}
+
+void i2c_comms_setup(void) {
+    // Initialise the I2C master
+    Wire.begin(GPIO_I2C_SDA, GPIO_I2C_SCL, (uint32_t)I2C_FREQUENCY);
+
+    // Initialise our queues and semaphores
+    motor_status_semaphore = xSemaphoreCreateBinary();
+    motor_status_queue = xQueueCreate(1, sizeof(motor_status_packet_t));
+
+    // Kick off our I2C comms task
+    xTaskCreate(i2c_comms_task, "MotorCtrl", I2C_TASK_STACK_SIZE, NULL, 5, &i2c_comms_task_handle);
+}

src/primary-esp32/i2c_comms.h

@@ -0,0 +1,11 @@
+#ifndef i2c_comms_h
+#define i2c_comms_h
+
+#include "../common/shared_structs.h"
+
+void i2c_comms_setup(void);
+
+bool get_motor_status_update(motor_status_packet_t *status_packet);
+void request_motor_status_update();
+
+#endif

src/primary-esp32/main.cpp

@@ -1,40 +1,34 @@
+#include "i2c_comms.h"
 #include <Arduino.h>
-#include "Wire.h"
-#include "io.h"
-#include "../common/shared_structs.h"
-
-#define MOTOR_ESP32_SLAVE_I2C_ADDR 0x55
-#define I2C_FREQUENCY 100000
-
-#define MOTOR_SPEED_SENSE 5
 
 void setup() {
-  Wire.begin(GPIO_I2C_SDA, GPIO_I2C_SCL, (uint32_t)I2C_FREQUENCY);
-
-  Serial.setTimeout(1);
-  Serial.begin(57600);
+    Serial.setTimeout(1);
+    Serial.begin(57600);
+    i2c_comms_setup();
 }
 
-uint8_t bytecount;
-i2c_status_packet_t i2c_reply;
+motor_status_packet_t motor_status;
 
-void loop() {
-  delay(100);
-  bytecount = Wire.requestFrom(MOTOR_ESP32_SLAVE_I2C_ADDR, sizeof i2c_reply);
-  if(bytecount == sizeof i2c_reply){
-    Wire.readBytes((byte*)&i2c_reply, bytecount);
-
-    Serial.print("Dsr:");
-    Serial.print(i2c_reply.motor_1_settings.desired_speed);
-    Serial.print(" Rsp:");
-    Serial.print(i2c_reply.roller_1_speed);
-    Serial.print("   Err:");
-    Serial.print(i2c_reply.motor_1_settings.desired_speed-i2c_reply.roller_1_speed);
-    Serial.print(" Mdty:");
-    Serial.println(i2c_reply.motor_1_duty);
+unsigned long now = 0;
+unsigned long last_motor_status = 0;
 
-  } else {
-    Serial.println(bytecount);
-  }
-  
+void loop() {
+    now = millis();
+
+    if (now - last_motor_status > 500) {
+        last_motor_status = now;
+        request_motor_status_update();
+        Serial.println("--Rq motor update--");
+    }
+
+    if (get_motor_status_update(&motor_status)) {
+        Serial.print("Dsr:");
+        Serial.print(motor_status.motor_1_settings.desired_speed);
+        Serial.print(" Rsp:");
+        Serial.print(motor_status.roller_1_speed);
+        Serial.print("   Err:");
+        Serial.print(motor_status.motor_1_settings.desired_speed - motor_status.roller_1_speed);
+        Serial.print(" Mdty:");
+        Serial.println(motor_status.motor_1_duty);
+    }
 }