Remove direct Serial usage

firmware/platformio.ini

@@ -23,9 +23,9 @@ lib_deps =
     askuric/Simple FOC @ 2.2.0
     infineon/TLV493D-Magnetic-Sensor @ 1.0.3
     bxparks/AceButton @ 1.9.1
-
 build_flags =
   -DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
+  -DMONITOR_SPEED=115200
 
 [env:view]
 extends = base_config
@@ -36,6 +36,12 @@ lib_deps =
   fastled/FastLED @ 3.5.0
   bogde/HX711 @ 0.7.5
   adafruit/Adafruit VEML7700 Library @ 1.1.1
+  bakercp/PacketSerial @ 1.4.0
+  nanopb/Nanopb @ 0.4.6   ; Ideally this would reference the nanopb submodule, but that would require
+                          ; everyone to check out submodules to just compile, so we use the library
+                          ; registry for the runtime. The submodule is available for manually updating
+                          ; the pre-compiled (checked in) .pb.h/c files when proto files change, but is
+                          ; otherwise not used during application firmware compilation.
 
 build_flags =
   ${base_config.build_flags}

firmware/src/display_task.cpp

@@ -6,7 +6,7 @@
 
 static const uint8_t LEDC_CHANNEL_LCD_BACKLIGHT = 0;
 
-DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 4048, 1, task_core} {
+DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 2048, 1, task_core} {
   knob_state_queue_ = xQueueCreate(1, sizeof(KnobState));
   assert(knob_state_queue_ != NULL);
 
@@ -19,63 +19,6 @@ DisplayTask::~DisplayTask() {
   vSemaphoreDelete(mutex_);
 }
 
-static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_t *b)
-{
-  int i;
-  float f,p,q,t;
-  
-  h = fmax(0.0, fmin(360.0, h));
-  s = fmax(0.0, fmin(100.0, s));
-  v = fmax(0.0, fmin(100.0, v));
-  
-  s /= 100;
-  v /= 100;
-  
-  if(s == 0) {
-    // Achromatic (grey)
-    *r = *g = *b = round(v*255);
-    return;
-  }
-
-  h /= 60; // sector 0 to 5
-  i = floor(h);
-  f = h - i; // factorial part of h
-  p = v * (1 - s);
-  q = v * (1 - s * f);
-  t = v * (1 - s * (1 - f));
-  switch(i) {
-    case 0:
-      *r = round(255*v);
-      *g = round(255*t);
-      *b = round(255*p);
-      break;
-    case 1:
-      *r = round(255*q);
-      *g = round(255*v);
-      *b = round(255*p);
-      break;
-    case 2:
-      *r = round(255*p);
-      *g = round(255*v);
-      *b = round(255*t);
-      break;
-    case 3:
-      *r = round(255*p);
-      *g = round(255*q);
-      *b = round(255*v);
-      break;
-    case 4:
-      *r = round(255*t);
-      *g = round(255*p);
-      *b = round(255*v);
-      break;
-    default: // case 5:
-      *r = round(255*v);
-      *g = round(255*p);
-      *b = round(255*q);
-    }
-}
-
 void DisplayTask::run() {
     tft_.begin();
     tft_.invertDisplay(1);
@@ -86,13 +29,13 @@ void DisplayTask::run() {
     ledcAttachPin(PIN_LCD_BACKLIGHT, LEDC_CHANNEL_LCD_BACKLIGHT);
     ledcWrite(LEDC_CHANNEL_LCD_BACKLIGHT, UINT16_MAX);
 
-    spr_.setColorDepth(16);
+    spr_.setColorDepth(8);
 
     if (spr_.createSprite(TFT_WIDTH, TFT_HEIGHT) == nullptr) {
-      Serial.println("ERROR: sprite allocation failed!");
+      log("ERROR: sprite allocation failed!");
       tft_.fillScreen(TFT_RED);
     } else {
-      Serial.println("Sprite created!");
+      log("Sprite created!");
       tft_.fillScreen(TFT_PURPLE);
     }
     spr_.setTextColor(0xFFFF, TFT_BLACK);
@@ -103,11 +46,6 @@ void DisplayTask::run() {
     const uint16_t FILL_COLOR = spr_.color565(90, 18, 151);
     const uint16_t DOT_COLOR = spr_.color565(80, 100, 200);
 
-    int32_t pointer_center_x = TFT_WIDTH / 2;
-    int32_t pointer_center_y = TFT_HEIGHT / 2;
-    int32_t pointer_length_short = 10;
-    int32_t pointer_length_long = TFT_WIDTH / 2 - 5;
-
     spr_.setTextDatum(CC_DATUM);
     spr_.setTextColor(TFT_WHITE);
     while(1) {
@@ -202,4 +140,14 @@ void DisplayTask::setBrightness(uint16_t brightness) {
   brightness_ = brightness;
 }
 
+void DisplayTask::setLogger(Logger* logger) {
+    logger_ = logger;
+}
+
+void DisplayTask::log(const char* msg) {
+    if (logger_ != nullptr) {
+        logger_->log(msg);
+    }
+}
+
 #endif

firmware/src/display_task.h

@@ -6,6 +6,7 @@
 #include <TFT_eSPI.h>
 
 #include "knob_data.h"
+#include "logger.h"
 #include "task.h"
 
 class DisplayTask : public Task<DisplayTask> {
@@ -18,6 +19,7 @@ class DisplayTask : public Task<DisplayTask> {
         QueueHandle_t getKnobStateQueue();
 
         void setBrightness(uint16_t brightness);
+        void setLogger(Logger* logger);
 
     protected:
         void run();
@@ -31,10 +33,10 @@ class DisplayTask : public Task<DisplayTask> {
         QueueHandle_t knob_state_queue_;
 
         KnobState state_;
-
         SemaphoreHandle_t mutex_;
-
         uint16_t brightness_;
+        Logger* logger_;
+        void log(const char* msg);
 };
 
 #else

firmware/src/interface_task.cpp

@@ -123,7 +123,11 @@ static KnobConfig configs[] = {
     },
 };
 
-InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task) : Task("Interface", 4048, 1, task_core), motor_task_(motor_task), display_task_(display_task) {
+InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task) : 
+        Task("Interface", 2048, 1, task_core),
+        stream_(),
+        motor_task_(motor_task),
+        display_task_(display_task) {
     #if SK_DISPLAY
         assert(display_task != nullptr);
     #endif
@@ -132,6 +136,8 @@ InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, Dis
 InterfaceTask::~InterfaceTask() {}
 
 void InterfaceTask::run() {
+    stream_.begin();
+
     #if PIN_BUTTON_NEXT >= 34
         pinMode(PIN_BUTTON_NEXT, INPUT);
     #else
@@ -167,7 +173,7 @@ void InterfaceTask::run() {
             veml.setGain(VEML7700_GAIN_2);
             veml.setIntegrationTime(VEML7700_IT_400MS);
         } else {
-            Serial.println("ALS sensor not found!");
+            log("ALS sensor not found!");
         }
     #endif
 
@@ -176,16 +182,20 @@ void InterfaceTask::run() {
     // 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:
     while (1) {
         button_next.check();
         #if PIN_BUTTON_PREV > -1
             button_prev.check();
         #endif
-        if (Serial.available()) {
-            int v = Serial.read();
+        if (stream_.available()) {
+            int v = stream_.read();
             if (v == ' ') {
                 changeConfig(true);
+            } else if (v == 'C') {
+                motor_task_.runCalibration();
             }
         }
 
@@ -196,26 +206,28 @@ void InterfaceTask::run() {
             lux_avg = lux * LUX_ALPHA + lux_avg * (1 - LUX_ALPHA);
             static uint32_t last_als;
             if (millis() - last_als > 1000) {
-                Serial.print("millilux: "); Serial.println(lux*1000);
+                snprintf(buf_, sizeof(buf_), "millilux: %.2f", lux*1000);
+                log(buf_);
                 last_als = millis();
             }
         #endif
 
         #if SK_STRAIN
-            // 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;
             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) {
-                    static uint32_t last_reading_display;
-                    if (millis() - last_reading_display > 1000) {
-                        Serial.print("HX711 reading: ");
-                        Serial.println(reading);
-                        last_reading_display = millis();
-                    }
                     long value = CLAMP(reading, lower, upper);
                     press_value_unit = 1. * (value - lower) / (upper - lower);
 
@@ -230,7 +242,7 @@ void InterfaceTask::run() {
                     }
                 }
             } else {
-                Serial.println("HX711 not found.");
+                log("HX711 not found.");
 
                 #if SK_LEDS
                     for (uint8_t i = 0; i < NUM_LEDS; i++) {
@@ -284,6 +296,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);
+}
+
 void InterfaceTask::changeConfig(bool next) {
     if (next) {
         current_config_ = (current_config_ + 1) % COUNT_OF(configs);
@@ -295,9 +312,8 @@ void InterfaceTask::changeConfig(bool next) {
         }
     }
     
-    Serial.print("Changing config to ");
-    Serial.print(current_config_);
-    Serial.print(" -- ");
-    Serial.println(configs[current_config_].descriptor);
+    char buf_[256];
+    snprintf(buf_, sizeof(buf_), "Changing config to %d -- %s", current_config_, configs[current_config_].descriptor);
+    log(buf_);
     motor_task_.setConfig(configs[current_config_]);
 }

firmware/src/interface_task.h

@@ -4,10 +4,12 @@
 #include <Arduino.h>
 
 #include "display_task.h"
+#include "logger.h"
 #include "motor_task.h"
+#include "serial/uart_stream.h"
 #include "task.h"
 
-class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandler {
+class InterfaceTask : public Task<InterfaceTask>, public Logger, public ace_button::IEventHandler {
     friend class Task<InterfaceTask>; // Allow base Task to invoke protected run()
 
     public:
@@ -15,13 +17,16 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
         ~InterfaceTask();
 
         void handleEvent(ace_button::AceButton* button, uint8_t event_type, uint8_t button_state) override;
+        void log(const char* msg) override;
 
     protected:
         void run();
 
     private:
+        UartStream stream_;
         MotorTask& motor_task_;
         DisplayTask* display_task_;
+        char buf_[64];
 
         int current_config_ = 0;
 

firmware/src/logger.h

@@ -1,7 +1,8 @@
+#pragma once
 
 class Logger {
     public:
         Logger() {};
         virtual ~Logger() {};
-    
-};
+        virtual void log(const char* msg) = 0;
+};

firmware/src/main.cpp

@@ -1,5 +1,4 @@
 #include <Arduino.h>
-#include <SimpleFOC.h>
 
 #include "display_task.h"
 #include "interface_task.h"
@@ -16,51 +15,51 @@ static MotorTask motor_task = MotorTask(1);
 
 InterfaceTask interface_task = InterfaceTask(0, motor_task, display_task_p);
 
-static QueueHandle_t knob_state_debug_queue;
-
 void setup() {
-  Serial.begin(115200);
-
-  motor_task.begin();
-  interface_task.begin();
-
   #if SK_DISPLAY
+  display_task.setLogger(&interface_task);
   display_task.begin();
 
   // Connect display to motor_task's knob state feed
   motor_task.addListener(display_task.getKnobStateQueue());
   #endif
 
-  // Create a queue and register it with motor_task to print knob state to serial (see loop() below)
-  knob_state_debug_queue = xQueueCreate(1, sizeof(KnobState));
-  assert(knob_state_debug_queue != NULL);
+  motor_task.setLogger(&interface_task);
+  motor_task.begin();
+  interface_task.begin();
 
-  motor_task.addListener(knob_state_debug_queue);
+  // // Create a queue and register it with motor_task to print knob state to serial (see loop() below)
+  // knob_state_debug_queue = xQueueCreate(1, sizeof(KnobState));
+  // assert(knob_state_debug_queue != NULL);
+  // motor_task.addListener(knob_state_debug_queue);
 
   // Free up the Arduino loop task
   vTaskDelete(NULL);
 }
 
-
-static KnobState state = {};
-uint32_t last_debug;
-
 void loop() {
-  // Print any new state, at most 5 times per second
-  if (millis() - last_debug > 200 && xQueueReceive(knob_state_debug_queue, &state, portMAX_DELAY) == pdTRUE) {
-    Serial.println(state.current_position);
-    last_debug = millis();
-  }
-
-  static uint32_t last_stack_debug;
-  if (millis() - last_stack_debug > 1000) {
-    Serial.println("Stack high water:");
-    Serial.printf("main: %d\n", uxTaskGetStackHighWaterMark(NULL));
-    #if SK_DISPLAY
-      Serial.printf("display: %d\n", uxTaskGetStackHighWaterMark(display_task.getHandle()));
-    #endif
-    Serial.printf("motor: %d\n", uxTaskGetStackHighWaterMark(motor_task.getHandle()));
-    Serial.printf("interface: %d\n", uxTaskGetStackHighWaterMark(interface_task.getHandle()));
-    last_stack_debug = millis();
-  }
+  // // Print any new state, at most 5 times per second
+  // if (millis() - last_debug > 200 && xQueueReceive(knob_state_debug_queue, &state, portMAX_DELAY) == pdTRUE) {
+  //   Serial.println(state.current_position);
+  //   last_debug = millis();
+  // }
+
+  // char buf[50];
+  // static uint32_t last_stack_debug;
+  // if (millis() - last_stack_debug > 1000) {
+  //   interface_task.log("Stack high water:");
+  //   snprintf(buf, sizeof(buf), "  main: %d", uxTaskGetStackHighWaterMark(NULL));
+  //   interface_task.log(buf);
+  //   #if SK_DISPLAY
+  //     snprintf(buf, sizeof(buf), "  display: %d", uxTaskGetStackHighWaterMark(display_task.getHandle()));
+  //     interface_task.log(buf);
+  //   #endif
+  //   snprintf(buf, sizeof(buf), "  motor: %d", uxTaskGetStackHighWaterMark(motor_task.getHandle()));
+  //   interface_task.log(buf);
+  //   snprintf(buf, sizeof(buf), "  interface: %d", uxTaskGetStackHighWaterMark(interface_task.getHandle()));
+  //   interface_task.log(buf);
+  //   snprintf(buf, sizeof(buf), "Heap -- free: %d, largest: %d", heap_caps_get_free_size(MALLOC_CAP_8BIT), heap_caps_get_largest_free_block(MALLOC_CAP_8BIT));
+  //   interface_task.log(buf);
+  //   last_stack_debug = millis();
+  // }
 }

firmware/src/motor_task.cpp

@@ -29,7 +29,7 @@ static const float IDLE_CORRECTION_MAX_ANGLE_RAD = 5 * PI / 180;
 static const float IDLE_CORRECTION_RATE_ALPHA = 0.0005;
 
 
-MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 2048, 1, task_core) {
+MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 2500, 1, task_core) {
     queue_ = xQueueCreate(5, sizeof(Command));
     assert(queue_ != NULL);
 }
@@ -43,16 +43,13 @@ MotorTask::~MotorTask() {}
     MT6701Sensor encoder = MT6701Sensor();
 #endif
 
-Commander command = Commander(Serial);
-
-
 void MotorTask::run() {
 
     driver.voltage_power_supply = 5;
     driver.init();
 
     #if SENSOR_TLV
-    encoder.init(Wire, false);
+    encoder.init(&Wire, false);
     #endif
 
     #if SENSOR_MT6701
@@ -82,23 +79,6 @@ void MotorTask::run() {
     motor.pole_pairs = MOTOR_POLE_PAIRS;
     motor.initFOC(ZERO_ELECTRICAL_OFFSET, FOC_DIRECTION);
 
-    bool calibrate = false;
-
-    Serial.println("Press Y to run calibration");
-    uint32_t t = millis();
-    while (millis() - t < 3000) {
-        if (Serial.read() == 'Y') {
-            calibrate = true;
-            break;
-        }
-        delay(10);
-    }
-    if (calibrate) {
-        this->calibrate();
-    }
-
-    Serial.println(motor.zero_electric_angle);
-
     motor.monitor_downsample = 0; // disable monitor at first - optional
 
     // disableCore0WDT();
@@ -115,6 +95,8 @@ void MotorTask::run() {
     uint32_t last_idle_start = 0;
     uint32_t last_publish = 0;
 
+    KnobConfig latest_config = config;
+
     while (1) {
         motor.loopFOC();
 
@@ -122,10 +104,14 @@ void MotorTask::run() {
         Command command;
         if (xQueueReceive(queue_, &command, 0) == pdTRUE) {
             switch (command.command_type) {
+                case CommandType::CALIBRATE:
+                    calibrate();
+                    break;
                 case CommandType::CONFIG: {
                     // Change haptic input mode
                     config = command.data.config;
-                    Serial.println("Got new config");
+                    latest_config = config;
+                    log("Got new config");
                     current_detent_center = motor.shaft_angle;
                     #if SK_INVERT_ROTATION
                         current_detent_center = -motor.shaft_angle;
@@ -260,6 +246,16 @@ void MotorTask::playHaptic(bool press) {
     xQueueSend(queue_, &command, portMAX_DELAY);
 }
 
+void MotorTask::runCalibration() {
+    Command command = {
+        .command_type = CommandType::CALIBRATE,
+        .data = {
+            .unused = 0,
+        }
+    };
+    xQueueSend(queue_, &command, portMAX_DELAY);
+}
+
 
 void MotorTask::addListener(QueueHandle_t queue) {
     listeners_.push_back(queue);
@@ -277,7 +273,7 @@ void MotorTask::calibrate() {
     // So this value is based on experimentation.
     // TODO: dig into SimpleFOC calibration and find/fix the issue
 
-    Serial.println("\n\n\nStarting calibration, please do not touch to motor until complete!");
+    log("\n\n\nStarting calibration, please DO NOT TOUCH MOTOR until complete!");
 
     motor.controller = MotionControlType::angle_openloop;
     motor.pole_pairs = 1;
@@ -309,16 +305,16 @@ void MotorTask::calibrate() {
     motor.voltage_limit = 0;
     motor.move(a);
 
-    Serial.println();
+    log("");
 
     // TODO: check for no motor movement!
 
-    Serial.print("Sensor measures positive for positive motor rotation: ");
+    log("Sensor measures positive for positive motor rotation:");
     if (end_sensor > start_sensor) {
-        Serial.println("YES, Direction=CW");
+        log("YES, Direction=CW");
         motor.initFOC(0, Direction::CW);
     } else {
-        Serial.println("NO, Direction=CCW");
+        log("NO, Direction=CCW");
         motor.initFOC(0, Direction::CCW);
     }
 
@@ -326,22 +322,23 @@ void MotorTask::calibrate() {
     // #### Determine pole-pairs
     // Rotate 20 electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
     uint8_t electrical_revolutions = 20;
-    Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
+    snprintf(buf_, sizeof(buf_), "Going to measure %d electrical revolutions...", electrical_revolutions);
+    log(buf_);
     motor.voltage_limit = 5;
     motor.move(a);
-    Serial.println("Going to electrical zero...");
+    log("Going to electrical zero...");
     float destination = a + _2PI;
     for (; a < destination; a += 0.03) {
         encoder.update();
         motor.move(a);
         delay(1);
     }
-    Serial.println("pause..."); // Let momentum settle...
+    log("pause..."); // Let momentum settle...
     for (uint16_t i = 0; i < 1000; i++) {
         encoder.update();
         delay(1);
     }
-    Serial.println("Measuring...");
+    log("Measuring...");
 
     start_sensor = motor.sensor_direction * encoder.getAngle();
     destination = a + electrical_revolutions * _2PI;
@@ -360,16 +357,17 @@ void MotorTask::calibrate() {
     motor.move(a);
 
     if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
-        Serial.println("ERROR: motor did not reach target!");
+        log("ERROR: motor did not reach target!");
         while(1) {}
     }
 
     float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
-    Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
-    Serial.println(electrical_per_mechanical);
+    snprintf(buf_, sizeof(buf_), "Electrical angle / mechanical angle (i.e. pole pairs) = %.2f", electrical_per_mechanical);
+    log(buf_);
 
     int measured_pole_pairs = (int)round(electrical_per_mechanical);
-    Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
+    snprintf(buf_, sizeof(buf_), "Pole pairs set to %d", measured_pole_pairs);
+    log(buf_);
 
     delay(1000);
 
@@ -396,11 +394,8 @@ void MotorTask::calibrate() {
         offset_x += cosf(offset_angle);
         offset_y += sinf(offset_angle);
 
-        Serial.print(degrees(real_electrical_angle));
-        Serial.print(", ");
-        Serial.print(degrees(measured_electrical_angle));
-        Serial.print(", ");
-        Serial.println(degrees(_normalizeAngle(offset_angle)));
+        snprintf(buf_, sizeof(buf_), "%.2f, %.2f, %.2f", degrees(real_electrical_angle), degrees(measured_electrical_angle), degrees(_normalizeAngle(offset_angle)));
+        log(buf_);
     }
     for (; a > destination2; a -= 0.4) {
         motor.move(a);
@@ -416,11 +411,8 @@ void MotorTask::calibrate() {
         offset_x += cosf(offset_angle);
         offset_y += sinf(offset_angle);
 
-        Serial.print(degrees(real_electrical_angle));
-        Serial.print(", ");
-        Serial.print(degrees(measured_electrical_angle));
-        Serial.print(", ");
-        Serial.println(degrees(_normalizeAngle(offset_angle)));
+        snprintf(buf_, sizeof(buf_), "%.2f, %.2f, %.2f", degrees(real_electrical_angle), degrees(measured_electrical_angle), degrees(_normalizeAngle(offset_angle)));
+        log(buf_);
     }
     motor.voltage_limit = 0;
     motor.move(a);
@@ -435,14 +427,39 @@ void MotorTask::calibrate() {
     motor.voltage_limit = 5;
     motor.controller = MotionControlType::torque;
 
-    Serial.print("\n\nRESULTS:\n  Update these constants at the top of " __FILE__ "\n  ZERO_ELECTRICAL_OFFSET: ");
-    Serial.println(motor.zero_electric_angle);
-    Serial.print("  FOC_DIRECTION: ");
+    log("\n\nRESULTS:\n  Update these constants at the top of " __FILE__);
+    snprintf(buf_, sizeof(buf_), "  ZERO_ELECTRICAL_OFFSET: %.2f", motor.zero_electric_angle);
+    log(buf_);
     if (motor.sensor_direction == Direction::CW) {
-        Serial.println("Direction::CW");
+        log("  FOC_DIRECTION: Direction::CW");
     } else {
-        Serial.println("Direction::CCW");
+        log("  FOC_DIRECTION: Direction::CCW");
     }
-    Serial.printf("  MOTOR_POLE_PAIRS: %d\n", motor.pole_pairs);
+    snprintf(buf_, sizeof(buf_), "  MOTOR_POLE_PAIRS: %d", motor.pole_pairs);
+    log(buf_);
     delay(2000);
 }
+
+void MotorTask::checkSensorError() {
+#if SENSOR_TLV
+    if (encoder.getAndClearError()) {
+        log("LOCKED!");
+    }
+#elif SENSOR_MT6701
+    MT6701Error error = encoder.getAndClearError();
+    if (error.error) {
+        snprintf(buf_, sizeof(buf_), "CRC error. Received %d; calculated %d", error.received_crc, error.calculated_crc);
+        log(buf_);
+    }
+#endif
+}
+
+void MotorTask::setLogger(Logger* logger) {
+    logger_ = logger;
+}
+
+void MotorTask::log(const char* msg) {
+    if (logger_ != nullptr) {
+        logger_->log(msg);
+    }
+}

firmware/src/motor_task.h

@@ -5,10 +5,12 @@
 #include <vector>
 
 #include "knob_data.h"
+#include "logger.h"
 #include "task.h"
 
 
 enum class CommandType {
+    CALIBRATE,
     CONFIG,
     HAPTIC,
 };
@@ -20,6 +22,7 @@ struct HapticData {
 struct Command {
     CommandType command_type;
     union CommandData {
+        uint8_t unused;
         KnobConfig config;
         HapticData haptic;
     };
@@ -35,16 +38,19 @@ class MotorTask : public Task<MotorTask> {
 
         void setConfig(const KnobConfig& config);
         void playHaptic(bool press);
+        void runCalibration();
 
         void addListener(QueueHandle_t queue);
+        void setLogger(Logger* logger);
 
     protected:
         void run();
 
     private:
         QueueHandle_t queue_;
-
+        Logger* logger_;
         std::vector<QueueHandle_t> listeners_;
+        char buf_[72];
 
         // BLDC motor & driver instance
         BLDCMotor motor = BLDCMotor(1);
@@ -52,4 +58,6 @@ class MotorTask : public Task<MotorTask> {
 
         void publish(const KnobState& state);
         void calibrate();
+        void checkSensorError();
+        void log(const char* msg);
 };

firmware/src/mt6701_sensor.cpp

@@ -105,7 +105,11 @@ float MT6701Sensor::getSensorAngle() {
         x_ = new_x * ALPHA + x_ * (1-ALPHA);
         y_ = new_y * ALPHA + y_ * (1-ALPHA);
       } else {
-        Serial.printf("Bad CRC. expected %d, actual %d\n", calculated_crc, received_crc);
+        error_ = {
+          .error = true,
+          .received_crc = received_crc,
+          .calculated_crc = calculated_crc,
+        };
       }
 
       last_update_ = now;
@@ -117,4 +121,10 @@ float MT6701Sensor::getSensorAngle() {
     return rad;
 }
 
+MT6701Error MT6701Sensor::getAndClearError() {
+  MT6701Error out = error_;
+  error_ = {};
+  return out;
+}
+
 #endif

firmware/src/mt6701_sensor.h

@@ -3,6 +3,12 @@
 #include <SimpleFOC.h>
 #include "driver/spi_master.h"
 
+struct MT6701Error {
+    bool error;
+    uint8_t received_crc;
+    uint8_t calculated_crc;
+};
+
 class MT6701Sensor : public Sensor {
     public:
         MT6701Sensor();
@@ -16,6 +22,8 @@ class MT6701Sensor : public Sensor {
         //    Calling this method directly does not update the base-class internal fields.
         //    Use update() when calling from outside code.
         float getSensorAngle();
+
+        MT6701Error getAndClearError();
     private:
 
         spi_device_handle_t spi_device_;
@@ -24,4 +32,6 @@ class MT6701Sensor : public Sensor {
         float x_;
         float y_;
         uint32_t last_update_;
+
+        MT6701Error error_ = {};
 };

firmware/src/serial/uart_stream.cpp

@@ -0,0 +1,64 @@
+/*
+   Copyright 2021 Scott Bezek and the splitflap contributors
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+*/
+
+#include <driver/uart.h>
+
+#include "config.h"
+#include "uart_stream.h"
+
+UartStream::UartStream() : Stream() {
+}
+
+void UartStream::begin() {
+    uart_config_t conf;
+    conf.baud_rate           = MONITOR_SPEED;
+    conf.data_bits           = UART_DATA_8_BITS;
+    conf.parity              = UART_PARITY_DISABLE;
+    conf.stop_bits           = UART_STOP_BITS_1;
+    conf.flow_ctrl           = UART_HW_FLOWCTRL_DISABLE;
+    conf.rx_flow_ctrl_thresh = 0;
+    conf.use_ref_tick        = false;
+    assert(uart_param_config(uart_port_, &conf) == ESP_OK);
+    assert(uart_driver_install(uart_port_, 32000, 32000, 0, NULL, 0) == ESP_OK);
+}
+
+int UartStream::peek() {
+    return -1;
+}
+
+int UartStream::available() {
+    size_t size = 0;
+    assert(uart_get_buffered_data_len(uart_port_, &size) == ESP_OK);
+    return size;
+}
+
+int UartStream::read() {
+    uint8_t b;
+    int res = uart_read_bytes(uart_port_, &b, 1, 0);
+    return res != 1 ? -1 : b;
+}
+
+void UartStream::flush() {
+
+}
+
+size_t UartStream::write(uint8_t b) {
+    return uart_write_bytes(uart_port_, (char*)&b, 1);
+}
+
+size_t UartStream::write(const uint8_t *buffer, size_t size) {
+    return uart_write_bytes(uart_port_, (const char*)buffer, size);
+}

firmware/src/serial/uart_stream.h

@@ -0,0 +1,47 @@
+/*
+   Copyright 2021 Scott Bezek and the splitflap contributors
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+*/
+#pragma once
+
+#include <Arduino.h>
+
+#include <driver/uart.h>
+
+/**
+ * Implementation of an Arduino Stream for UART serial communications using the esp uart driver
+ * directly, rather than the Arduino HAL which has a small fixed underlying rx FIFO size and
+ * potentially other issues that cause dropped bytes at high speeds/bursts.
+ * 
+ * This is not a full or optimized implementation; just the minimal necessary for this project.
+ */
+class UartStream : public Stream {
+    public:
+        UartStream();
+
+        void begin();
+
+        // Stream methods
+        int available() override;
+        int read() override;
+        int peek() override;
+        void flush() override;
+
+        // Print methods
+        size_t write(uint8_t b) override;
+        size_t write(const uint8_t *buffer, size_t size) override;
+
+    private:
+        const uart_port_t uart_port_ = UART_NUM_0;
+};

firmware/src/tlv_sensor.cpp

@@ -35,7 +35,7 @@ float TlvSensor::getSensorAngle() {
         }
       }
       if (all_same) {
-        Serial.println("LOCKED!");
+        error_ = true;
         init(wire_, invert_);
         // Force unique frame counts to avoid reset loop
         for (uint8_t i = 1; i < sizeof(frame_counts_); i++) {
@@ -49,3 +49,9 @@ float TlvSensor::getSensorAngle() {
     }
     return rad;
 }
+
+bool TlvSensor::getAndClearError() {
+  bool error = error_;
+  error_ = false;
+  return error;
+}

firmware/src/tlv_sensor.h

@@ -16,6 +16,8 @@ class TlvSensor : public Sensor {
         //    Calling this method directly does not update the base-class internal fields.
         //    Use update() when calling from outside code.
         float getSensorAngle();
+
+        bool getAndClearError();
     private:
         Tlv493d tlv_ = Tlv493d();
         float x_;
@@ -24,6 +26,8 @@ class TlvSensor : public Sensor {
         TwoWire* wire_;
         bool invert_;
 
+        bool error_ = false;
+
         uint8_t frame_counts_[3] = {};
         uint8_t cur_frame_count_index_ = 0;
 };