Add bin level and empty sensors

src/primary-esp32/bin_level.cpp

@@ -0,0 +1,116 @@
+// Module to record roller and wheel speeds from MCPWM capture interrupts. Not yet sure if a task is required at all.
+
+#include "bin_level.h"
+#include "io.h"
+
+#include "driver/mcpwm.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#include "soc/rtc.h"
+
+// Precision timer value of the start of our pulse.
+static uint32_t bin_level_pulse_timer_start[3] = {0, 0, 0};
+
+// Store state of each pulse channel
+static bool bin_level_pulse_started[3] = {false, false, false};
+
+// Also store timestamp as microseconds since start to avoid overflow of the precision timer when checking for stop
+static int64_t bin_level_last_timestamp[3] = {0, 0, 0};
+
+// Use a single item queue for each measurement we need to distribute.
+static xQueueHandle bin_level_measurement_queue[3];
+
+// struct to hold move our pulse measurements from our ISR via the queue
+typedef struct {
+    uint32_t length;   // pulse length in APB clock ticks
+    int64_t timestamp; // time measurement occurred in microseconds since boot
+} bin_level_measurement_t;
+
+// Gets called by the MCPWM capture module. Tells us which capture unit it was, which edge, and when it happened
+// Our ultrasound depth sensor pulses are active low (so start on neg edge, finish on pos edge)
+static bool bin_level_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;
+
+    int b_id = 0;
+
+    if (cap_sig == MCPWM_SELECT_CAP0) {
+        b_id = 0;
+    } else if (cap_sig == MCPWM_SELECT_CAP1) {
+        b_id = 1;
+    } else if (cap_sig == MCPWM_SELECT_CAP2) {
+        b_id = 2;
+    } else {
+        return false;
+    }
+
+    int64_t time_now;
+    time_now = esp_timer_get_time();
+
+    if (edata->cap_edge == MCPWM_NEG_EDGE) {
+        // start our pulse measurement. Note that this will roll around every minute or so, so comparison logic needs to
+        // handle that
+        bin_level_pulse_timer_start[b_id] = edata->cap_value;
+        bin_level_last_timestamp[b_id] = time_now;
+        bin_level_pulse_started[b_id] = true;
+    } else {
+        // Only a valid end of pulse if we had detected a start, and it was less than MAX_PULSE_LENGTH_MS ago
+        if (bin_level_pulse_started[b_id] and((bin_level_last_timestamp[b_id] - time_now) < pdMS_TO_TICKS(MAX_PULSE_LENGTH_MS))) {
+            bin_level_measurement_t new_measurement;
+            new_measurement.timestamp = time_now;
+            new_measurement.length = edata->cap_value - bin_level_pulse_timer_start[b_id];
+            xQueueOverwriteFromISR(bin_level_measurement_queue[b_id], &new_measurement, &high_task_wakeup);
+        }
+        // Reset pulse start detection
+        bin_level_pulse_started[b_id] = false;
+    }
+
+    return high_task_wakeup == pdTRUE;
+}
+
+float get_bin_level(bin_id_t bin_id) {
+    bin_level_measurement_t pulse_measurement;
+    // Get the pulse period in APB clock ticks.
+    xQueuePeek(bin_level_measurement_queue[bin_id], &pulse_measurement, 0);
+
+    // Return 0 if the pulse measurement is too old
+    if ((pulse_measurement.length == 0) or
+        ((esp_timer_get_time() - pulse_measurement.timestamp) >= (RECENT_PULSE_TIMEOUT_MS*1000))) {
+        return 0.0;
+    }
+
+    //return 1000.0 * ((float)(pulse_measurement.length) / (float)rtc_clk_apb_freq_get());
+
+    // Convert pulse length to distance in meters
+    return ((1000.0 * ((float)(pulse_measurement.length) / (float)rtc_clk_apb_freq_get())) - (float)(BIN_LEVEL_OFFSET_MS)) / (float)(BIN_LEVEL_MS_PER_METER);
+}
+
+void bin_level_setup(void) {
+    // Create our measurement queues
+    bin_level_measurement_queue[0] = xQueueCreate(1, sizeof(bin_level_measurement_t));
+    bin_level_measurement_queue[1] = xQueueCreate(1, sizeof(bin_level_measurement_t));
+    bin_level_measurement_queue[2] = xQueueCreate(1, sizeof(bin_level_measurement_t));
+
+    // Prime queues
+    bin_level_measurement_t blank_measurement = {0, 0};
+    xQueueOverwrite(bin_level_measurement_queue[0], &blank_measurement);
+    xQueueOverwrite(bin_level_measurement_queue[1], &blank_measurement);
+    xQueueOverwrite(bin_level_measurement_queue[2], &blank_measurement);
+
+    // Attach GPIO to capture units in MCPWM. 
+    // Use MCPWM unit 1, as we're using unit 0 for the wheel speed
+    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM_CAP_0, GPIO_BIN1_LEVEL);
+    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM_CAP_1, GPIO_BIN2_LEVEL);
+    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM_CAP_2, GPIO_BIN3_LEVEL);
+
+    // Configure and enable MCPWM capture units
+    mcpwm_capture_config_t conf = {.cap_edge = MCPWM_BOTH_EDGE, // trigger on both edges
+                                   .cap_prescale = 1,          // pulses per interrupt
+                                   .capture_cb = bin_level_pulse_isr_handler,
+                                   .user_data = NULL};
+    mcpwm_capture_enable_channel(MCPWM_UNIT_1, MCPWM_SELECT_CAP0, &conf);
+    mcpwm_capture_enable_channel(MCPWM_UNIT_1, MCPWM_SELECT_CAP1, &conf);
+    mcpwm_capture_enable_channel(MCPWM_UNIT_1, MCPWM_SELECT_CAP2, &conf);
+}

src/primary-esp32/bin_level.h

@@ -0,0 +1,22 @@
+#ifndef bin_level_h
+#define bin_level_h
+
+#include "../common/shared_structs.h"
+
+// Ultrasound sensor pulse output will be BIN_LEVEL_OFFSET_MS + (depth * BIN_LEVEL_METERS_PER_MS)
+#define BIN_LEVEL_MS_PER_METER 15.5
+#define BIN_LEVEL_OFFSET_MS 4.24
+
+// Time without a pulse after which the level will be considered to be 0 (to catch problems with the sensor)
+#define RECENT_PULSE_TIMEOUT_MS 5000
+
+// Time after which a pulse will be considered invalid
+#define MAX_PULSE_LENGTH_MS 60
+
+void bin_level_setup(void);
+
+// Returns the last calculated speed for the wheel in revs/second.
+float get_bin_level(bin_id_t bin_id);
+
+
+#endif

src/primary-esp32/main.cpp

@@ -8,12 +8,14 @@ that's been bundled out into its own FreeRTOS module (using the ESP IDF librarie
 
 #include "EspMQTTClient.h"
 
+#include "bin_level.h"
 #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 BIN_STATUS_UPDATE_PERIOD_MS   3000
 #define HEARTBEAT_PERIOD_MS           1000
 
 // Rate at which the current wheel speed will be used to update the desired roller speed
@@ -40,6 +42,14 @@ void setup() {
     digitalWrite(GPIO_ACT_LED, LOW);
     pinMode(GPIO_ACT_LED, OUTPUT);
 
+    pinMode(GPIO_BIN1_EMPTY, INPUT);
+    pinMode(GPIO_BIN2_EMPTY, INPUT);
+    pinMode(GPIO_BIN3_EMPTY, INPUT);
+
+    i2c_comms_setup();
+    wheel_speed_setup();
+    bin_level_setup();
+
     Serial.setTimeout(1);
     Serial.begin(57600);
 
@@ -173,6 +183,7 @@ 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_bin_status = 0;
 unsigned long last_heartbeat = 0;
 unsigned long no_comms_timeout_start = 0;
 
@@ -244,6 +255,20 @@ void loop() {
         mqtt_client.publish("airseeder/status/wheel", msg_buffer, true);
     }
 
+    //  `airseeder/status/bin1` - `[timestamp] [empty] [distance]
+    // Periodically publish the bin status
+    if (now - last_bin_status > BIN_STATUS_UPDATE_PERIOD_MS) {
+        last_bin_status = now;
+        sprintf(msg_buffer, "%lu %d %.2f", now, digitalRead(GPIO_BIN1_EMPTY), get_bin_level(BIN_1));
+        mqtt_client.publish("airseeder/status/bin1", msg_buffer, true);
+
+        sprintf(msg_buffer, "%lu %d %.2f", now, digitalRead(GPIO_BIN2_EMPTY), get_bin_level(BIN_2));
+        mqtt_client.publish("airseeder/status/bin2", msg_buffer, true);
+
+        sprintf(msg_buffer, "%lu %d %.2f", now, digitalRead(GPIO_BIN3_EMPTY), get_bin_level(BIN_3));
+        mqtt_client.publish("airseeder/status/bin3", msg_buffer, true);
+    }
+
     // Periodically publish the heartbeat
     if (now - last_heartbeat > HEARTBEAT_PERIOD_MS) {
         last_heartbeat = now;

src/primary-esp32/wheel_speed.h

@@ -1,7 +1,6 @@
 #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