Implement persistent configuration/calibration storage using FFAT (#124)

Reintroduces persistent configuration from #122, but using FFAT instead
of SPIFFS, since SPIFFS had bad performance impacts on the motor control
loop.

firmware/partitions-4MB-spiffs.csv → firmware/partitions-4MB-fat.csv

@@ -8,4 +8,4 @@ otadata,  data, ota,      0xe000,  8K,
 ota_0,    0,    ota_0,   0x10000,  1408K,
 ota_1,    0,    ota_1,  0x170000,  1408K,
 uf2,      app,  factory,0x2d0000,  256K,
-spiffs,   data, spiffs, 0x310000,  960K,
+ffat,     data, fat,    0x310000,  960K,

firmware/src/configuration.cpp

@@ -0,0 +1,129 @@
+#include <FFat.h>
+
+#include "pb_decode.h"
+#include "pb_encode.h"
+
+#include "proto_gen/smartknob.pb.h"
+#include "semaphore_guard.h"
+
+#include "configuration.h"
+
+static const char* CONFIG_PATH = "/config.pb";
+
+Configuration::Configuration() {
+  mutex_ = xSemaphoreCreateMutex();
+  assert(mutex_ != NULL);
+}
+
+Configuration::~Configuration() {
+  vSemaphoreDelete(mutex_);
+}
+
+bool Configuration::loadFromDisk() {
+    SemaphoreGuard lock(mutex_);
+    FatGuard fatGuard(logger_);
+    if (!fatGuard.mounted_) {
+        return false;
+    }
+
+    File f = FFat.open(CONFIG_PATH);
+    if (!f) {
+        log("Failed to read config file");
+        return false;
+    }
+
+    size_t read = f.readBytes((char*)buffer_, sizeof(buffer_));
+    f.close();
+
+    pb_istream_t stream = pb_istream_from_buffer(buffer_, read);
+    if (!pb_decode(&stream, PB_PersistentConfiguration_fields, &pb_buffer_)) {
+        char buf[200];
+        snprintf(buf, sizeof(buf), "Decoding failed: %s", PB_GET_ERROR(&stream));
+        log(buf);
+        return false;
+    }
+
+    if (pb_buffer_.version != PERSISTENT_CONFIGURATION_VERSION) {
+        char buf[200];
+        snprintf(buf, sizeof(buf), "Invalid config version. Expected %u, received %u", PERSISTENT_CONFIGURATION_VERSION, pb_buffer_.version);
+        log(buf);
+        return false;
+    }
+    loaded_ = true;
+
+    char buf[200];
+    snprintf(
+        buf,
+        sizeof(buf),
+        "Motor calibration: calib=%u, pole_pairs=%u, zero_offset=%.2f, cw=%u",
+        pb_buffer_.motor.calibrated,
+        pb_buffer_.motor.pole_pairs,
+        pb_buffer_.motor.zero_electrical_offset,
+        pb_buffer_.motor.direction_cw
+    );
+    log(buf);
+    return true;
+}
+
+bool Configuration::saveToDisk() {
+    SemaphoreGuard lock(mutex_);
+
+    pb_ostream_t stream = pb_ostream_from_buffer(buffer_, sizeof(buffer_));
+    pb_buffer_.version = PERSISTENT_CONFIGURATION_VERSION;
+    if (!pb_encode(&stream, PB_PersistentConfiguration_fields, &pb_buffer_)) {
+        char buf[200];
+        snprintf(buf, sizeof(buf), "Encoding failed: %s", PB_GET_ERROR(&stream));
+        log(buf);
+        return false;
+    }
+
+    FatGuard fatGuard(logger_);
+    if (!fatGuard.mounted_) {
+        return false;
+    }
+    File f = FFat.open(CONFIG_PATH, FILE_WRITE);
+    if (!f) {
+        log("Failed to read config file");
+        return false;
+    }
+    size_t written = f.write(buffer_, stream.bytes_written);
+    f.close();
+
+    char buf[20];
+    snprintf(buf, sizeof(buf), "Wrote %d bytes", written);
+    log(buf);
+
+    if (written != stream.bytes_written) {
+        log("Failed to write all bytes to file");
+        return false;
+    }
+
+    return true;
+}
+
+PB_PersistentConfiguration Configuration::get() {
+    SemaphoreGuard lock(mutex_);
+    if (!loaded_) {
+        return PB_PersistentConfiguration();
+    }
+    return pb_buffer_;
+}
+
+bool Configuration::setMotorCalibrationAndSave(PB_MotorCalibration& motor_calibration) {
+    {
+        SemaphoreGuard lock(mutex_);
+        pb_buffer_.motor = motor_calibration;
+        pb_buffer_.has_motor = true;
+    }
+    return saveToDisk();
+}
+
+void Configuration::setLogger(Logger* logger) {
+    logger_ = logger;
+}
+
+void Configuration::log(const char* msg) {
+    if (logger_ != nullptr) {
+        logger_->log(msg);
+    }
+}

firmware/src/configuration.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include <FFat.h>
+#include <PacketSerial.h>
+
+#include "proto_gen/smartknob.pb.h"
+
+#include "logger.h"
+
+const uint32_t PERSISTENT_CONFIGURATION_VERSION = 1;
+
+class Configuration {
+    public:
+        Configuration();
+        ~Configuration();
+
+        void setLogger(Logger* logger);
+        bool loadFromDisk();
+        bool saveToDisk();
+        PB_PersistentConfiguration get();
+        bool setMotorCalibrationAndSave(PB_MotorCalibration& motor_calibration);
+
+    private:
+        SemaphoreHandle_t mutex_;
+
+        Logger* logger_ = nullptr;
+        bool loaded_ = false;
+        PB_PersistentConfiguration pb_buffer_ = {};
+
+        uint8_t buffer_[PB_PersistentConfiguration_size];
+
+        void log(const char* msg);
+};
+class FatGuard {
+    public:
+        FatGuard(Logger* logger) : logger_(logger) {
+            if (!FFat.begin(true)) {
+                if (logger_ != nullptr) {
+                    logger_->log("Failed to mount FFat");
+                }
+                return;
+            }
+            if (logger_ != nullptr) {
+                logger_->log("Mounted FFat");
+            }
+            mounted_ = true;
+        }
+        ~FatGuard() {
+            if (mounted_) {
+                FFat.end();
+                if (logger_ != nullptr) {
+                    logger_->log("Unmounted FFat");
+                }
+            }
+        }
+        FatGuard(FatGuard const&)=delete;
+        FatGuard& operator=(FatGuard const&)=delete;
+
+        bool mounted_ = false;
+
+    private:
+        Logger* logger_;
+};

firmware/src/main.cpp

@@ -1,16 +1,19 @@
 #include <Arduino.h>
 
+#include "configuration.h"
 #include "display_task.h"
 #include "interface_task.h"
 #include "motor_task.h"
 
+Configuration config;
+
 #if SK_DISPLAY
 static DisplayTask display_task(0);
 static DisplayTask* display_task_p = &display_task;
 #else
 static DisplayTask* display_task_p = nullptr;
 #endif
-static MotorTask motor_task(1);
+static MotorTask motor_task(1, config);
 
 
 InterfaceTask interface_task(0, motor_task, display_task_p);
@@ -24,9 +27,13 @@ void setup() {
   motor_task.addListener(display_task.getKnobStateQueue());
   #endif
 
+  interface_task.begin();
+
+  config.setLogger(&interface_task);
+  config.loadFromDisk();
+
   motor_task.setLogger(&interface_task);
   motor_task.begin();
-  interface_task.begin();
 
   // Free up the Arduino loop task
   vTaskDelete(NULL);

firmware/src/motor_task.cpp

@@ -12,15 +12,6 @@
 #include "motors/motor_config.h"
 #include "util.h"
 
-// #### 
-// Hardware-specific motor calibration constants.
-// Run calibration once at startup, then update these constants with the calibration results.
-static const float ZERO_ELECTRICAL_OFFSET = 7.61;
-static const Direction FOC_DIRECTION = Direction::CW;
-static const int MOTOR_POLE_PAIRS = 7;
-// ####
-
-
 static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
 static const float DEAD_ZONE_RAD = 1 * _PI / 180;
 
@@ -31,7 +22,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", 2500, 1, task_core) {
+MotorTask::MotorTask(const uint8_t task_core, Configuration& configuration) : Task("Motor", 4000, 1, task_core), configuration_(configuration) {
     queue_ = xQueueCreate(5, sizeof(Command));
     assert(queue_ != NULL);
 }
@@ -86,8 +77,9 @@ void MotorTask::run() {
     encoder.update();
     delay(10);
 
-    motor.pole_pairs = MOTOR_POLE_PAIRS;
-    motor.initFOC(ZERO_ELECTRICAL_OFFSET, FOC_DIRECTION);
+    PB_PersistentConfiguration c = configuration_.get();
+    motor.pole_pairs = c.motor.calibrated ? c.motor.pole_pairs : 7;
+    motor.initFOC(c.motor.zero_electrical_offset, c.motor.direction_cw ? Direction::CW : Direction::CCW);
 
     motor.monitor_downsample = 0; // disable monitor at first - optional
 
@@ -524,13 +516,13 @@ void MotorTask::calibrate() {
 
 
     // #### Apply settings
-    // TODO: save to non-volatile storage
     motor.pole_pairs = measured_pole_pairs;
     motor.zero_electric_angle = avg_offset_angle + _3PI_2;
     motor.voltage_limit = FOC_VOLTAGE_LIMIT;
     motor.controller = MotionControlType::torque;
 
-    log("\n\nRESULTS:\n  Update these constants at the top of " __FILE__);
+    log("");
+    log("RESULTS:");
     snprintf(buf_, sizeof(buf_), "  ZERO_ELECTRICAL_OFFSET: %.2f", motor.zero_electric_angle);
     log(buf_);
     if (motor.sensor_direction == Direction::CW) {
@@ -540,7 +532,18 @@ void MotorTask::calibrate() {
     }
     snprintf(buf_, sizeof(buf_), "  MOTOR_POLE_PAIRS: %d", motor.pole_pairs);
     log(buf_);
-    delay(2000);
+
+    log("");
+    log("Saving to persistent configuration...");
+    PB_MotorCalibration calibration = {
+        .calibrated = true,
+        .zero_electrical_offset = motor.zero_electric_angle,
+        .direction_cw = motor.sensor_direction == Direction::CW,
+        .pole_pairs = motor.pole_pairs,
+    };
+    if (configuration_.setMotorCalibrationAndSave(calibration)) {
+        log("Success!");
+    }
 }
 
 void MotorTask::checkSensorError() {

firmware/src/motor_task.h

@@ -4,6 +4,7 @@
 #include <SimpleFOC.h>
 #include <vector>
 
+#include "configuration.h"
 #include "logger.h"
 #include "proto_gen/smartknob.pb.h"
 #include "task.h"
@@ -33,7 +34,7 @@ class MotorTask : public Task<MotorTask> {
     friend class Task<MotorTask>; // Allow base Task to invoke protected run()
 
     public:
-        MotorTask(const uint8_t task_core);
+        MotorTask(const uint8_t task_core, Configuration& configuration);
         ~MotorTask();
 
         void setConfig(const PB_SmartKnobConfig& config);
@@ -47,6 +48,7 @@ class MotorTask : public Task<MotorTask> {
         void run();
 
     private:
+        Configuration& configuration_;
         QueueHandle_t queue_;
         Logger* logger_;
         std::vector<QueueHandle_t> listeners_;

firmware/src/proto_gen/smartknob.pb.c

@@ -27,4 +27,10 @@ PB_BIND(PB_SmartKnobConfig, PB_SmartKnobConfig, AUTO)
 PB_BIND(PB_RequestState, PB_RequestState, AUTO)
 
 
+PB_BIND(PB_PersistentConfiguration, PB_PersistentConfiguration, AUTO)
+
+
+PB_BIND(PB_MotorCalibration, PB_MotorCalibration, AUTO)
+
+
 

firmware/src/proto_gen/smartknob.pb.h

@@ -73,6 +73,19 @@ typedef struct _PB_ToSmartknob {
     } payload;
 } PB_ToSmartknob;
 
+typedef struct _PB_MotorCalibration {
+    bool calibrated;
+    float zero_electrical_offset;
+    bool direction_cw;
+    uint32_t pole_pairs;
+} PB_MotorCalibration;
+
+typedef struct _PB_PersistentConfiguration {
+    uint32_t version;
+    bool has_motor;
+    PB_MotorCalibration motor;
+} PB_PersistentConfiguration;
+
 
 #ifdef __cplusplus
 extern "C" {
@@ -86,6 +99,8 @@ extern "C" {
 #define PB_SmartKnobState_init_default           {0, 0, false, PB_SmartKnobConfig_init_default}
 #define PB_SmartKnobConfig_init_default          {0, 0, 0, 0, 0, 0, 0, 0, 0, "", 0, {0, 0, 0, 0, 0}, 0}
 #define PB_RequestState_init_default             {0}
+#define PB_PersistentConfiguration_init_default  {0, false, PB_MotorCalibration_init_default}
+#define PB_MotorCalibration_init_default         {0, 0, 0, 0}
 #define PB_FromSmartKnob_init_zero               {0, 0, {PB_Ack_init_zero}}
 #define PB_ToSmartknob_init_zero                 {0, 0, 0, {PB_RequestState_init_zero}}
 #define PB_Ack_init_zero                         {0}
@@ -93,6 +108,8 @@ extern "C" {
 #define PB_SmartKnobState_init_zero              {0, 0, false, PB_SmartKnobConfig_init_zero}
 #define PB_SmartKnobConfig_init_zero             {0, 0, 0, 0, 0, 0, 0, 0, 0, "", 0, {0, 0, 0, 0, 0}, 0}
 #define PB_RequestState_init_zero                {0}
+#define PB_PersistentConfiguration_init_zero     {0, false, PB_MotorCalibration_init_zero}
+#define PB_MotorCalibration_init_zero            {0, 0, 0, 0}
 
 /* Field tags (for use in manual encoding/decoding) */
 #define PB_Ack_nonce_tag                         1
@@ -120,6 +137,12 @@ extern "C" {
 #define PB_ToSmartknob_nonce_tag                 2
 #define PB_ToSmartknob_request_state_tag         3
 #define PB_ToSmartknob_smartknob_config_tag      4
+#define PB_MotorCalibration_calibrated_tag       1
+#define PB_MotorCalibration_zero_electrical_offset_tag 2
+#define PB_MotorCalibration_direction_cw_tag     3
+#define PB_MotorCalibration_pole_pairs_tag       4
+#define PB_PersistentConfiguration_version_tag   1
+#define PB_PersistentConfiguration_motor_tag     2
 
 /* Struct field encoding specification for nanopb */
 #define PB_FromSmartKnob_FIELDLIST(X, a) \
@@ -182,6 +205,21 @@ X(a, STATIC,   SINGULAR, FLOAT,    snap_point_bias,  12)
 #define PB_RequestState_CALLBACK NULL
 #define PB_RequestState_DEFAULT NULL
 
+#define PB_PersistentConfiguration_FIELDLIST(X, a) \
+X(a, STATIC,   SINGULAR, UINT32,   version,           1) \
+X(a, STATIC,   OPTIONAL, MESSAGE,  motor,             2)
+#define PB_PersistentConfiguration_CALLBACK NULL
+#define PB_PersistentConfiguration_DEFAULT NULL
+#define PB_PersistentConfiguration_motor_MSGTYPE PB_MotorCalibration
+
+#define PB_MotorCalibration_FIELDLIST(X, a) \
+X(a, STATIC,   SINGULAR, BOOL,     calibrated,        1) \
+X(a, STATIC,   SINGULAR, FLOAT,    zero_electrical_offset,   2) \
+X(a, STATIC,   SINGULAR, BOOL,     direction_cw,      3) \
+X(a, STATIC,   SINGULAR, UINT32,   pole_pairs,        4)
+#define PB_MotorCalibration_CALLBACK NULL
+#define PB_MotorCalibration_DEFAULT NULL
+
 extern const pb_msgdesc_t PB_FromSmartKnob_msg;
 extern const pb_msgdesc_t PB_ToSmartknob_msg;
 extern const pb_msgdesc_t PB_Ack_msg;
@@ -189,6 +227,8 @@ extern const pb_msgdesc_t PB_Log_msg;
 extern const pb_msgdesc_t PB_SmartKnobState_msg;
 extern const pb_msgdesc_t PB_SmartKnobConfig_msg;
 extern const pb_msgdesc_t PB_RequestState_msg;
+extern const pb_msgdesc_t PB_PersistentConfiguration_msg;
+extern const pb_msgdesc_t PB_MotorCalibration_msg;
 
 /* Defines for backwards compatibility with code written before nanopb-0.4.0 */
 #define PB_FromSmartKnob_fields &PB_FromSmartKnob_msg
@@ -198,11 +238,15 @@ extern const pb_msgdesc_t PB_RequestState_msg;
 #define PB_SmartKnobState_fields &PB_SmartKnobState_msg
 #define PB_SmartKnobConfig_fields &PB_SmartKnobConfig_msg
 #define PB_RequestState_fields &PB_RequestState_msg
+#define PB_PersistentConfiguration_fields &PB_PersistentConfiguration_msg
+#define PB_MotorCalibration_fields &PB_MotorCalibration_msg
 
 /* Maximum encoded size of messages (where known) */
 #define PB_Ack_size                              6
 #define PB_FromSmartKnob_size                    264
 #define PB_Log_size                              258
+#define PB_MotorCalibration_size                 15
+#define PB_PersistentConfiguration_size          23
 #define PB_RequestState_size                     0
 #define PB_SmartKnobConfig_size                  173
 #define PB_SmartKnobState_size                   192

platformio.ini

@@ -40,6 +40,7 @@ build_flags =
 [env:view]
 extends = base_config
 board = esp32doit-devkit-v1
+board_build.partitions = default_ffat.csv
 lib_deps =
   ${base_config.lib_deps}
   askuric/Simple FOC @ 2.2.0
@@ -128,8 +129,9 @@ build_flags =
 
 [env:nanofoc]
 extends = base_config
-platform = espressif32
+platform = espressif32@6.3.1
 board = adafruit_feather_esp32s3
+board_build.partitions = firmware/partitions-4MB-fat.csv
 lib_deps =
   ${base_config.lib_deps}
   askuric/Simple FOC@^2.3.0

proto/smartknob.proto

@@ -70,3 +70,14 @@ message SmartKnobConfig {
 
 message RequestState {}
 
+message PersistentConfiguration {
+    uint32 version = 1;
+    MotorCalibration motor = 2;
+}
+
+message MotorCalibration {
+    bool calibrated = 1;
+    float zero_electrical_offset = 2;
+    bool direction_cw = 3;
+    uint32 pole_pairs = 4;
+}