Add non volatile storage of motor parameters

src/motor-esp32/motor_control.cpp

@@ -9,6 +9,10 @@
 #include "freertos/queue.h"
 #include "freertos/task.h"
 
+#include "esp_system.h"
+#include "nvs.h"
+#include "nvs_flash.h"
+
 #define MC_ITERATION_TIME 1.0 / MOTOR_CONTROL_PERIOD_MS
 
 #define MC_TASK_STACK_SIZE 2048
@@ -17,6 +21,9 @@
 static xQueueHandle motor_control_queue[3];
 static xQueueHandle motor_parameters_queue[3];
 
+nvs_handle_t nvs_params_handle;
+static const char *nvs_motor_param_keys[] = {"motor1_params", "motor2_params", "motor3_params"};
+
 typedef struct {
     float duty = 0.0;
     float last_error = 0.0;
@@ -119,6 +126,8 @@ void set_motor_control(motor_id_t motor_id, motor_control_t new_control) {
 
 void set_motor_params(motor_id_t motor_id, motor_parameters_t new_params) {
     xQueueOverwrite(motor_parameters_queue[motor_id], &new_params);
+    nvs_set_blob(nvs_params_handle, nvs_motor_param_keys[motor_id], &new_params, sizeof(new_params));
+    nvs_commit(nvs_params_handle);
 }
 
 motor_control_t get_motor_control(motor_id_t motor_id) {
@@ -142,6 +151,17 @@ float get_motor_duty(motor_id_t motor_id) {
 
 void motor_control_setup(void) {
 
+    // Initialize NVS
+    esp_err_t err = nvs_flash_init();
+    if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+        // NVS partition was truncated and needs to be erased
+        // Retry nvs_flash_init
+        nvs_flash_erase();
+        err = nvs_flash_init();
+    }
+
+    nvs_open("parameters", NVS_READWRITE, &nvs_params_handle);
+
     for (int m_id = 0; m_id < 3; m_id++) {
         // Allocate queues
         motor_control_queue[m_id] = xQueueCreate(1, sizeof(motor_control_t));
@@ -150,16 +170,26 @@ void motor_control_setup(void) {
 
         // Prime queues - these are single item queues, and so functions reading from them just
         // assume something is there.
-        // TODO - this is where we'd read in the saved motor params from flash.
-        motor_status_t blank_motor_status;
+
         motor_control_t default_motor_control = {.enabled = false, .desired_speed = 0.0};
         motor_parameters_t default_motor_parameters = {
             .kp = DEFAULT_KP, .ki = DEFAULT_KI, .kd = DEFAULT_KD, .min_duty = DEFAULT_MIN_DUTY};
+        motor_status_t blank_motor_status;
 
         xQueueOverwrite(motor_control_queue[m_id], &default_motor_control);
         xQueueOverwrite(motor_parameters_queue[m_id], &default_motor_parameters);
         xQueueOverwrite(motor_status_queue[m_id], &blank_motor_status);
-    }
+
+        // If stored, read motor paramters from NVS
+        motor_parameters_t stored_motor_parameters;
+        size_t nvs_blob_size;
+
+        nvs_blob_size = sizeof(motor_parameters_t);
+        err = nvs_get_blob(nvs_params_handle, nvs_motor_param_keys[m_id], &stored_motor_parameters, &nvs_blob_size);
+        if (err == ESP_OK) {
+            xQueueOverwrite(motor_parameters_queue[m_id], &stored_motor_parameters);
+        }
+        }
 
     // Setup MCPWM timers.
     mcpwm_config_t pwm_config = {