Use queue for logging, refactor hardware interface work

firmware/src/interface_task.cpp

@@ -131,6 +131,9 @@ InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, Dis
     #if SK_DISPLAY
         assert(display_task != nullptr);
     #endif
+
+    log_queue_ = xQueueCreate(10, sizeof(std::string *));
+    assert(log_queue_ != NULL);
 }
 
 InterfaceTask::~InterfaceTask() {}
@@ -179,9 +182,6 @@ void InterfaceTask::run() {
 
     motor_task_.setConfig(configs[0]);
 
-    // How far button is pressed, in range [0, 1]
-    float press_value_unit = 0;
-
     log("Interface starting.\nPress 'C' at any time to run calibration.");
 
     // Interface loop:
@@ -198,84 +198,15 @@ void InterfaceTask::run() {
                 motor_task_.runCalibration();
             }
         }
+        std::string* log_string;
+        while (xQueueReceive(log_queue_, &log_string, 0) == pdTRUE) {
+            stream_.println(log_string->c_str());
+            delete log_string;
+        }
 
-        #if SK_ALS
-            const float LUX_ALPHA = 0.005;
-            static float lux_avg;
-            float lux = veml.readLux();
-            lux_avg = lux * LUX_ALPHA + lux_avg * (1 - LUX_ALPHA);
-            static uint32_t last_als;
-            if (millis() - last_als > 1000) {
-                snprintf(buf_, sizeof(buf_), "millilux: %.2f", lux*1000);
-                log(buf_);
-                last_als = millis();
-            }
-        #endif
-
-        #if SK_STRAIN
-            if (scale.wait_ready_timeout(100)) {
-                int32_t reading = scale.read();
-
-                static uint32_t last_reading_display;
-                if (millis() - last_reading_display > 1000) {
-                    snprintf(buf_, sizeof(buf_), "HX711 reading: %d", reading);
-                    log(buf_);
-                    last_reading_display = millis();
-                }
-
-                // TODO: calibrate and track (long term moving average) zero point (lower); allow calibration of set point offset
-                const int32_t lower = 950000;
-                const int32_t upper = 1800000;
-                // Ignore readings that are way out of expected bounds
-                if (reading >= lower - (upper - lower) && reading < upper + (upper - lower)*2) {
-                    long value = CLAMP(reading, lower, upper);
-                    press_value_unit = 1. * (value - lower) / (upper - lower);
-
-                    static bool pressed;
-                    if (!pressed && press_value_unit > 0.75) {
-                        motor_task_.playHaptic(true);
-                        pressed = true;
-                        changeConfig(true);
-                    } else if (pressed && press_value_unit < 0.25) {
-                        motor_task_.playHaptic(false);
-                        pressed = false;
-                    }
-                }
-            } else {
-                log("HX711 not found.");
-
-                #if SK_LEDS
-                    for (uint8_t i = 0; i < NUM_LEDS; i++) {
-                        leds[i] = CRGB::Red;
-                    }
-                    FastLED.show();
-                #endif
-            }
-        #endif
-
-        uint16_t brightness = UINT16_MAX;
-        // TODO: brightness scale factor should be configurable (depends on reflectivity of surface)
-        #if SK_ALS
-            brightness = (uint16_t)CLAMP(lux_avg * 13000, (float)1280, (float)UINT16_MAX);
-        #endif
-
-        #if SK_DISPLAY
-            display_task_->setBrightness(brightness); // TODO: apply gamma correction
-        #endif
-
-        #if SK_LEDS
-            for (uint8_t i = 0; i < NUM_LEDS; i++) {
-                leds[i].setHSV(200 * press_value_unit, 255, brightness >> 8);
-
-                // Gamma adjustment
-                leds[i].r = dim8_video(leds[i].r);
-                leds[i].g = dim8_video(leds[i].g);
-                leds[i].b = dim8_video(leds[i].b);
-            }
-            FastLED.show();
-        #endif
+        updateHardware();
 
-        delay(10);
+        delay(1);
     }
 }
 
@@ -297,8 +228,11 @@ void InterfaceTask::handleEvent(AceButton* button, uint8_t event_type, uint8_t b
 }
 
 void InterfaceTask::log(const char* msg) {
-    // TODO: send to queue to make this threadsafe
-    stream_.println(msg);
+    // Allocate a string for the duration it's in the queue; it is free'd by the queue consumer
+    std::string* msg_str = new std::string(msg);
+
+    // Put string in queue (or drop if full to avoid blocking)
+    xQueueSendToBack(log_queue_, &msg_str, 0);
 }
 
 void InterfaceTask::changeConfig(bool next) {
@@ -317,3 +251,84 @@ void InterfaceTask::changeConfig(bool next) {
     log(buf_);
     motor_task_.setConfig(configs[current_config_]);
 }
+
+void InterfaceTask::updateHardware() {
+    // How far button is pressed, in range [0, 1]
+    float press_value_unit = 0;
+
+    #if SK_ALS
+        const float LUX_ALPHA = 0.005;
+        static float lux_avg;
+        float lux = veml.readLux();
+        lux_avg = lux * LUX_ALPHA + lux_avg * (1 - LUX_ALPHA);
+        static uint32_t last_als;
+        if (millis() - last_als > 1000) {
+            snprintf(buf_, sizeof(buf_), "millilux: %.2f", lux*1000);
+            log(buf_);
+            last_als = millis();
+        }
+    #endif
+
+    #if SK_STRAIN
+        if (scale.wait_ready_timeout(100)) {
+            int32_t reading = scale.read();
+
+            static uint32_t last_reading_display;
+            if (millis() - last_reading_display > 1000) {
+                snprintf(buf_, sizeof(buf_), "HX711 reading: %d", reading);
+                log(buf_);
+                last_reading_display = millis();
+            }
+
+            // TODO: calibrate and track (long term moving average) zero point (lower); allow calibration of set point offset
+            const int32_t lower = 950000;
+            const int32_t upper = 1800000;
+            // Ignore readings that are way out of expected bounds
+            if (reading >= lower - (upper - lower) && reading < upper + (upper - lower)*2) {
+                long value = CLAMP(reading, lower, upper);
+                press_value_unit = 1. * (value - lower) / (upper - lower);
+
+                static bool pressed;
+                if (!pressed && press_value_unit > 0.75) {
+                    motor_task_.playHaptic(true);
+                    pressed = true;
+                    changeConfig(true);
+                } else if (pressed && press_value_unit < 0.25) {
+                    motor_task_.playHaptic(false);
+                    pressed = false;
+                }
+            }
+        } else {
+            log("HX711 not found.");
+
+            #if SK_LEDS
+                for (uint8_t i = 0; i < NUM_LEDS; i++) {
+                    leds[i] = CRGB::Red;
+                }
+                FastLED.show();
+            #endif
+        }
+    #endif
+
+    uint16_t brightness = UINT16_MAX;
+    // TODO: brightness scale factor should be configurable (depends on reflectivity of surface)
+    #if SK_ALS
+        brightness = (uint16_t)CLAMP(lux_avg * 13000, (float)1280, (float)UINT16_MAX);
+    #endif
+
+    #if SK_DISPLAY
+        display_task_->setBrightness(brightness); // TODO: apply gamma correction
+    #endif
+
+    #if SK_LEDS
+        for (uint8_t i = 0; i < NUM_LEDS; i++) {
+            leds[i].setHSV(200 * press_value_unit, 255, brightness >> 8);
+
+            // Gamma adjustment
+            leds[i].r = dim8_video(leds[i].r);
+            leds[i].g = dim8_video(leds[i].g);
+            leds[i].b = dim8_video(leds[i].b);
+        }
+        FastLED.show();
+    #endif
+}

firmware/src/interface_task.h

@@ -30,5 +30,8 @@ class InterfaceTask : public Task<InterfaceTask>, public Logger, public ace_butt
 
         int current_config_ = 0;
 
+        QueueHandle_t log_queue_;
+
         void changeConfig(bool next);
+        void updateHardware();
 };