Add wheel speed sensing, remove manual pulsecount overflow

src/motor-esp32/roller_speed.cpp

@@ -43,11 +43,8 @@ static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id
         return false;
     }
 
-    // Add one to our revs counter, and overflow it if necessary
+    // Add one to our revs counter
     roller_pulses[r_id]++;
-    if ((roller_pulses[r_id] / ROLLER_PULSES_PER_REV) > REV_OVERFLOW) {
-        roller_pulses[r_id] = 0;
-    }
 
     pulse_measurement_t new_measurement;
     int64_t time_now;
@@ -57,7 +54,7 @@ static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id
     if ((roller_last_timestamp[r_id] != 0) and ((time_now - roller_last_timestamp[r_id]) < ZERO_SPEED_TIMEOUT_US)) {
         new_measurement.timestamp = time_now;
         new_measurement.period = edata->cap_value - roller_last_edge_time[r_id];
-        // Technically passing the roller passes with the rest of the measurement here means that it won't be updated
+        // Technically passing the roller pulses with the rest of the measurement here means that it won't be updated
         // for the first pulse detected after being stopped, but for the purposes of tracking total revs in a given
         // period of several seconds this shouldn't matter.
         new_measurement.pulses = roller_pulses[r_id];

src/motor-esp32/roller_speed.h

@@ -3,17 +3,19 @@
 
 #include "../common/shared_structs.h"
 
-#define REV_OVERFLOW          10000 // Number at which rev counts overflow back to 0
-#define ROLLER_PULSES_PER_REV 40    // Number of sensor pulses per roller rev
-#define ZERO_SPEED_TIMEOUT_US                                                                                          \
-    1000000 // Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds
+// Number of sensor pulses per roller rev. Note that this will overflow at about 4 billion pulses
+#define ROLLER_PULSES_PER_REV 40
+
+// Time without a pulse after which the speed will be considered to be 0.
+// Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds
+#define ZERO_SPEED_TIMEOUT_US 1000000
 
 void roller_speed_setup(void);
 
 // Returns the last calculated speed for roller in revs/second.
 float get_roller_speed(roller_id_t roller_id);
 
-// Returns the total rev count for a roller.  Overflows back to 0 at REV_OVERFLOW
+// Returns the total rev count for a roller.
 float get_roller_revs(roller_id_t roller_id);
 
 #endif

src/primary-esp32/main.cpp

@@ -4,8 +4,10 @@
 
 #include "i2c_comms.h"
 #include "io.h"
+#include "wheel_speed.h"
 
 #define MOTOR_STATUS_UPDATE_PERIOD_MS 500
+#define WHEEL_STATUS_UPDATE_PERIOD_MS 1000
 #define HEARTBEAT_PERIOD_MS           1000
 
 #define POWER_BUTTON_HOLD_MS 300
@@ -32,6 +34,7 @@ void setup() {
     Serial.setTimeout(1);
     Serial.begin(57600);
     i2c_comms_setup();
+    wheel_speed_setup();
 
     // Hopefully this lets us get missed messages during a connection.
     mqtt_client.enableMQTTPersistence();
@@ -125,12 +128,14 @@ motor_control_group_t motor_control_group;
 
 unsigned long now = 0;
 unsigned long last_motor_status = 0;
+unsigned long last_wheel_status = 0;
 unsigned long last_heartbeat = 0;
 unsigned long no_comms_timeout_start = 0;
 
 void loop() {
     now = millis();
 
+    // If we've been on for long enough, hold the system power on
     if (now > POWER_BUTTON_HOLD_MS) {
         digitalWrite(GPIO_SYS_EN, HIGH);
     }
@@ -153,11 +158,13 @@ void loop() {
         }
     }
 
+    // Periodically request a motor update from the motor ESP32 via I2C
     if (now - last_motor_status > MOTOR_STATUS_UPDATE_PERIOD_MS) {
         last_motor_status = now;
         request_motor_status_update();
     }
 
+    // Publish the motor status update when it comes back
     if (get_motor_status_update(&motor_status)) {
         // Message format is "[timestamp] [enabled] [desired_speed] [actual_speed] [revs] [motor_duty] [motor_current]".
         // [timestamp] is an unsigned integer, the number of milliseconds since power on
@@ -182,6 +189,14 @@ void loop() {
         mqtt_client.publish("airseeder/status/roller3", msg_buffer, true);
     }
 
+    // Periodically publish the wheel status
+    if (now - last_wheel_status > WHEEL_STATUS_UPDATE_PERIOD_MS) {
+        last_wheel_status = now;
+        sprintf(msg_buffer, "%lu %.2f %.2f", now, get_wheel_speed(), get_wheel_revs());
+        mqtt_client.publish("airseeder/status/wheel", msg_buffer, true);
+    }
+
+    // Periodically publish the heartbeat
     if (now - last_heartbeat > HEARTBEAT_PERIOD_MS) {
         last_heartbeat = now;
         sprintf(msg_buffer, "%lu", now);

src/primary-esp32/wheel_speed.cpp

@@ -0,0 +1,97 @@
+// Module to record roller and wheel speeds from MCPWM capture interrupts. Not yet sure if a task is required at all.
+
+#include "wheel_speed.h"
+#include "io.h"
+
+#include "driver/mcpwm.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#include "soc/rtc.h"
+
+static uint32_t wheel_pulses = 0;         // Total pulses of wheel so far
+static uint32_t wheel_last_edge_time = 0; // Precision timer value of most recent edge
+// Also store timestamp as microseconds since start to avoid overflow of the precision timer when checking for stop
+static int64_t wheel_last_timestamp = 0;
+
+// Use a single item queue for each measurement we need to distribute.
+static xQueueHandle wheel_measurement_queue;
+
+// struct to hold move our pulse measurements from our ISR via the queue
+typedef struct {
+    uint32_t period;   // period since last pulse in APB clock ticks
+    int64_t timestamp; // time measurement occurred in microseconds since boot
+    uint32_t pulses;   // Total pulses of wheel so far
+} pulse_measurement_t;
+
+// Gets called by the MCPWM capture module. Tells us which capture unit it was, which edge, and when it happened
+static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id_t cap_sig,
+                                    const cap_event_data_t *edata, void *arg) {
+
+    // Not strictly necessary to do the wakeup check here, as we shouldn't be blokcing anything. Doesn't hurt though.
+    BaseType_t high_task_wakeup = pdFALSE;
+
+
+    if (cap_sig != MCPWM_SELECT_CAP0) {
+        // Not sure how we'd ever wind up here
+        return false;
+    }
+
+    // Add one to our revs counter
+    wheel_pulses++;
+
+    pulse_measurement_t new_measurement;
+    int64_t time_now;
+
+    time_now = esp_timer_get_time();
+    // Just make sure we have a previous edge to compare to, and that it was no more than ZERO_SPEED_TIMEOUT_US ago
+    if ((wheel_last_timestamp != 0) and ((time_now - wheel_last_timestamp) < ZERO_SPEED_TIMEOUT_US)) {
+        new_measurement.timestamp = time_now;
+        new_measurement.period = edata->cap_value - wheel_last_edge_time;
+        // Like with the rollers, technically passing the wheel pulses with the rest of the measurement here means that it won't be updated
+        // for the first pulse detected after being stopped. The total count will still be correct, but will just not update for 1.
+        new_measurement.pulses = wheel_pulses;
+        xQueueOverwriteFromISR(wheel_measurement_queue, &new_measurement, &high_task_wakeup);
+    }
+
+    wheel_last_edge_time = edata->cap_value;
+    wheel_last_timestamp = time_now;
+
+    return high_task_wakeup == pdTRUE;
+}
+
+float get_wheel_speed(){
+    pulse_measurement_t pulse_measurement;
+    // Get the pulse period in APB clock ticks.
+    xQueuePeek(wheel_measurement_queue, &pulse_measurement, 0);
+    if ((pulse_measurement.period == 0) or
+        ((esp_timer_get_time() - pulse_measurement.timestamp) >= ZERO_SPEED_TIMEOUT_US)) {
+        return 0.0;
+    }
+    // Convert that to revs/sec
+    return ((float)rtc_clk_apb_freq_get() / (float)(pulse_measurement.period)) / (float)(WHEEL_PULSES_PER_REV);
+}
+
+float get_wheel_revs() {
+    pulse_measurement_t pulse_measurement;
+    xQueuePeek(wheel_measurement_queue, &pulse_measurement, 0);
+    return (float)pulse_measurement.pulses / (float)WHEEL_PULSES_PER_REV;
+}
+
+void wheel_speed_setup(void) {
+    // Create our measurement queue
+    wheel_measurement_queue = xQueueCreate(1, sizeof(pulse_measurement_t));
+
+    // Prime queue
+    pulse_measurement_t blank_measurement = {0, 0};
+    xQueueOverwrite(wheel_measurement_queue, &blank_measurement);
+
+    // Attach GPIO to capture unit 0 in MCPWM0
+    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, GPIO_WHEEL_SPEED);
+
+    // Configure and enable MCPWM capture unit
+    mcpwm_capture_config_t conf = {.cap_edge = MCPWM_POS_EDGE, // trigger on riding edge
+                                   .cap_prescale = 1,          // pulses per interrupt
+                                   .capture_cb = speed_pulse_isr_handler,
+                                   .user_data = NULL};
+    mcpwm_capture_enable_channel(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, &conf);
+}

src/primary-esp32/wheel_speed.h

@@ -0,0 +1,21 @@
+#ifndef wheel_speed_h
+#define wheel_speed_h
+
+#include "../common/shared_structs.h"
+
+// Number of sensor pulses per wheel rev. Note that this will overflow at about 4 billion pulses
+#define WHEEL_PULSES_PER_REV 40
+
+// Time without a pulse after which the speed will be considered to be 0.
+// Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds
+#define ZERO_SPEED_TIMEOUT_US 1000000
+
+void wheel_speed_setup(void);
+
+// Returns the last calculated speed for the wheel in revs/second.
+float get_wheel_speed();
+
+// Returns the total rev count for the wheel.
+float get_wheel_revs();
+
+#endif