wip doenst work

fw/src/.clang-format

@@ -1,3 +1,3 @@
 Language: Cpp
 BasedOnStyle:  google
-ColumnLimit: 120
+ColumnLimit: 140

fw/src/DRV8323S.hpp

@@ -199,6 +199,9 @@ void drv8323s_set_sen_lvl(struct drv8323s_foc_driver *dev,
 
 void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
                    int CSn, int en, SPIClass *spi) {
+  if(dev == NULL || spi == NULL) {
+    return;
+  }
   dev->CSn = CSn;
   dev->spi = spi;
   dev->en = en;

fw/src/main.cpp

@@ -1,8 +1,13 @@
-// #include <Preferences.h>
+/*
+reg.pid_position: [380.0, 0.0, 0.20000000298023224, 4.999999873689376e-05, 10000.0, 100.0]
+  reg.pid_velocity: [0.05000000074505806, 5.5, 0.0, 0.009999999776482582, 10000.0, 6.0, 6.0]
+*/
 
 #include <cmath>
 
+#include "BLDCMotor.h"
 #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
+#include "encoders/as5047/AS5047.h"
 #include "esp32-hal.h"
 #define NODE_NAME "N17BLDC"
 
@@ -11,7 +16,7 @@
 #define VCS_REVISION_ID 0
 
 #include "Arduino.h"
-#include "pin_def.h"
+#include "pin_def_v5.h"
 
 #define USE_USBSERIAL
 #ifdef USE_USBSERIAL
@@ -31,28 +36,26 @@ USBCDC usbserial;
 #include "canard.c"
 #include "udral_servo.hpp"
 #define spi_dev SPI
+SPIClass spi_drv(FSPI);
 // #include "Wire.h"
 #include <SimpleFOC.h>
 
+#include "DRV8323S.hpp"
+#include "HybridStepperMotor.h"
 #include "SimpleFOCDrivers.h"
 #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
 #include "encoders/as5047/MagneticSensorAS5047.h"
 #include "encoders/calibrated/CalibratedSensor.h"
-// #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
-
-#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
-
-#include "DRV8323S.hpp"
-#include "HybridStepperMotor.h"
 #include "esp32-hal-adc.h"
 #include "esp32-hal-gpio.h"
 
 #define TEMP(v0, b, rt, r1, vdd) ((1.0 / ((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - 273.15)
 #define temp_pcb (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3))
+#define temp_mot (analogRead(TERMISTOR_EXT) > 0.95 ? NAN : (TEMP(analogRead(TERMISTOR_EXT) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3)))
 
 #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
-//* 20.8f/ 21033.75)
 
+// #define ESPHWENC
 #define MOTOR
 #define FW_NO_WATCHDOG
 
@@ -66,27 +69,32 @@ USBCDC usbserial;
 // };
 #endif
 
-const int voltage_lpf = 50;
+float max_voltage_limit = 0;
-const float max_voltage_limit = 6;
-
-// ESP32HWEncoder encoder =
-// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
 
+#ifdef ESPHWENC
+#include "encoders/esp32hwencoder/ESP32HWEncoder.h"
+ESP32HWEncoder encoder(ENC_A, ENC_B, 4096 / 4);  // The Index pin can be omitted
+#else
+Encoder encoder(ENC_A, ENC_B, 4096 / 4);
+void doA1() { encoder.handleA(); }
+void doB1() { encoder.handleB(); }
+#endif
 MagneticSensorAS5047 encoder_absolute(18);
-Encoder encoder(ENC_A, ENC_B, 4096 / 4, 0);
 AbsIncCombineSensor abs_inc_sensor(encoder, encoder_absolute);
 
 float encoder_calibration_lut[90];
-CalibratedSensor encoder_calibrated(encoder_absolute,
+CalibratedSensor encoder_calibrated(abs_inc_sensor, sizeof(encoder_calibration_lut) / sizeof(encoder_calibration_lut[0]),
-                                    sizeof(encoder_calibration_lut) / sizeof(encoder_calibration_lut[0]),
                                     encoder_calibration_lut);
 // MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
 
-#ifdef MOTOR
 struct drv8323s_foc_driver drv8323s;
 // BLDCMotor motor = BLDCMotor(100); // 24N22P
-HybridStepperMotor motor = HybridStepperMotor(50);  //, 3.7, 10, 4.5 / 1000);
+
-#endif
+HybridStepperMotor motorHybridStepper = HybridStepperMotor(50);  //, 3.7, 10, 4.5 / 1000);
+BLDCMotor motorBLDC = BLDCMotor(7, 0.7, 360, 0.15);
+FOCMotor *motor = NULL;
+// BLDCMotor motor = BLDCMotor(7);
+// BLDCDriver3PWM driver = BLDCDriver3PWM(16, 5, 17, 4);
 
 void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
   Wire.beginTransmission(addr);
@@ -130,18 +138,25 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
   xSemaphoreTake(mutex, portMAX_DELAY);
   if (!init_done) {
     init_done = true;
-    state->pid_position.value.count = 3;
+    motor = state->motor_type == MOTOR_TYPE_HYBRID_STEPPER ? (FOCMotor *)&motorHybridStepper : (FOCMotor *)&motorBLDC;
-    state->pid_position.value.elements[0] = motor.P_angle.P;
+    state->max_voltage.value.count = 2;
-    state->pid_position.value.elements[1] = motor.P_angle.I;
+    state->max_voltage.value.elements[0] = 0.01; /* max voltage*/
-    state->pid_position.value.elements[2] = motor.P_angle.D;
+    state->max_voltage.value.elements[1] = 0.01; /* aligning voltage */
+    state->pid_position.value.count = 6;
+    state->pid_position.value.elements[0] = motor->P_angle.P;
+    state->pid_position.value.elements[1] = motor->P_angle.I;
+    state->pid_position.value.elements[2] = motor->P_angle.D;
+    state->pid_position.value.elements[3] = motor->LPF_angle.Tf;
+    state->pid_position.value.elements[4] = motor->P_angle.output_ramp;
+    state->pid_position.value.elements[5] = motor->P_angle.limit;
     state->pid_velocity.value.count = 7;
-    state->pid_velocity.value.elements[0] = motor.PID_velocity.P;
+    state->pid_velocity.value.elements[0] = motor->PID_velocity.P;
-    state->pid_velocity.value.elements[1] = motor.PID_velocity.I;
+    state->pid_velocity.value.elements[1] = motor->PID_velocity.I;
-    state->pid_velocity.value.elements[2] = motor.PID_velocity.D;
+    state->pid_velocity.value.elements[2] = motor->PID_velocity.D;
-    state->pid_velocity.value.elements[3] = motor.LPF_velocity.Tf;
+    state->pid_velocity.value.elements[3] = motor->LPF_velocity.Tf;
-    state->pid_velocity.value.elements[4] = motor.PID_velocity.output_ramp;
+    state->pid_velocity.value.elements[4] = motor->PID_velocity.output_ramp;
-    state->pid_velocity.value.elements[5] = motor.PID_velocity.limit;
+    state->pid_velocity.value.elements[5] = motor->PID_velocity.limit;
-    motor.motion_downsample = state->pid_velocity.value.elements[6] = 0;
+    state->pid_velocity.value.elements[6] = motor->motion_downsample;
   }
   servo_state.target_force = state->target_force;
   servo_state.target_acceleration = state->target_acceleration;
@@ -167,12 +182,11 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
   xSemaphoreGive(mutex);
 }
 
+TaskHandle_t taskFocHandle;
+void foc_task(void *parameter);
 TaskHandle_t taskCommHandle;
 void comm_task(void *parameter) { udral_loop(mutexed_state_sync); }
 
-void doA1() { encoder.handleA(); }
-void doB1() { encoder.handleB(); }
-
 void setup() {
   pinMode(LED_PIN, OUTPUT);
   mutex = xSemaphoreCreateMutex();
@@ -184,13 +198,6 @@ void setup() {
 #else
   Serial.begin(460800, SERIAL_8N1, 44, 43);
 #endif
-  digitalWrite(LED_PIN, 0);  // enable
-  delay(500);
-  digitalWrite(LED_PIN, 1);  // disable
-  delay(100);
-
-  Serial.println("Start");
-  delay(1000);
 
   pinMode(SPI_DRV_SC, OUTPUT);
 
@@ -206,77 +213,35 @@ void setup() {
   digitalWrite(CAL_PIN, 0);  // enable
 
   pinMode(SPI_MISO, INPUT_PULLUP);
-  SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
+  spi_dev.begin(SPI_CLK, SPI_MISO, SPI_MOSI);
+  spi_drv.begin(SPI_DRV_CLK, SPI_DRV_MISO, SPI_DRV_MOSI, SPI_DRV_SC);
   // SPI.setFrequency(100000);
   //  initialise magnetic sensor hardware
   encoder.pullup = Pullup::USE_INTERN;
   encoder.init();
+#ifndef ESPHWENC
   encoder.enableInterrupts(doA1, doB1);
+#endif
   encoder_absolute.init(&spi_dev);
 
   abs_inc_sensor.init();
 
+  drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN,&spi_drv); //&spi_drv);
+
+  motorHybridStepper.linkSensor(&encoder_absolute);
+  motorHybridStepper.linkDriver(drv8323s.focdriver);
+  motorBLDC.linkSensor(&encoder_absolute);
+  motorBLDC.linkDriver(drv8323s.focdriver);
+
   Serial.printf("offset: %f\nvalue: %d", abs_inc_sensor.offset, abs_inc_sensor.getAngle());
-#ifdef MOTOR
+
   SimpleFOCDebug::enable(&Serial);
-  drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev);
+  digitalWrite(LED_PIN, 1);  // disable
-  digitalWrite(INLABC, 1);  // enable
+  delay(100);
-  // driver.init(&SPI);
+  digitalWrite(LED_PIN, 0);  // enable
-  // status();
+  delay(400);
-  motor.linkSensor(&encoder_calibrated);
+  digitalWrite(LED_PIN, 1);  // disable
-  motor.linkDriver(drv8323s.focdriver);
+  delay(4000);
-  // motor.foc_modulation = FOCModulationType::SinePWM;
-  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
-
-  motor.voltage_sensor_align = 4;
-  motor.voltage_limit = max_voltage_limit;
-  // motor.controller = MotionControlType::velocity_openloop;
-  // motor.controller = MotionControlType::velocity;
-  motor.controller = MotionControlType::torque;
-  motor.torque_controller = TorqueControlType::voltage;
-  motor.useMonitoring(Serial);
-  drv8323s.focdriver->voltage_power_supply = vdrive_read;
-  motor.init();
-  delay(1000);
-
-  static Preferences pref;
-  pref.begin("simpleFOC");
-
-  // set voltage to run calibration
-  encoder_calibrated.voltage_calibration = 3;
-  encoder_calibrated.init();
-  // Running calibration
-  const char *encoder_calibration_lut_str = "enc_cal_lut";
-  const char *zero_electric_angle_str = "zero_el_angle";
-  const char *sensor_direction_str = "enc_dir_str";
-  const int settle_time_ms = 150;
-  if (!pref.isKey(encoder_calibration_lut_str) ||
-      pref.getBytesLength(encoder_calibration_lut_str) != sizeof(encoder_calibration_lut)) {
-    encoder_calibrated.calibrate(motor, settle_time_ms);
-    pref.putBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
-    pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
-    pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
-
-  } else {
-    pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
-    Serial.println("Skipping calibration");
-  }
-
-  if (!pref.isKey(zero_electric_angle_str)) {
-    motor.initFOC();
-    pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
-    pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
-  } else {
-    motor.zero_electric_angle = pref.getFloat(zero_electric_angle_str);
-    motor.sensor_direction = (Direction)pref.getInt(sensor_direction_str);
-    motor.initFOC();
-  }
-  pref.end();
-
-  motor.move(0);
-  digitalWrite(INLABC, 0);  // enable
-#endif
-
   xTaskCreatePinnedToCore(&comm_task,       // Function name of the task
                           "comm",           // Name of the task (e.g. for debugging)
                           65536,            // Stack size (bytes)
@@ -287,104 +252,207 @@ void setup() {
   );
   // esp_task_wdt_init(WDT_TIMEOUT_s, true);  // enable panic so ESP32 restarts
   // esp_task_wdt_add(NULL);                  // add current thread to WDT watch
-}
 
-int i = 0;
-void loop() {
-#ifdef MOTOR
 
   while (!init_done) {
     delay(10);
   }
-  bool armed = false;
 
+  xTaskCreatePinnedToCore(&foc_task,       // Function name of the task
+                          "foc",           // Name of the task (e.g. for debugging)
+                          65536,           // Stack size (bytes)
+                          NULL,            // Parameter to pass
+                          10,              // Task priority
+                          &taskFocHandle,  // Assign task handle
+                          1                // Run on the non-Arduino (1) core
+  );
+}
+
+bool init_motor() {
+#ifndef MOTOR
+  return false;
+#endif
+  if (motor == NULL) {
+    return false;
+  }
+  static bool is_init = false;
+  if (!is_init) {
+    if (motor->voltage_limit <= 0 || motor->voltage_sensor_align <= 0) {
+      return false;
+    }
+    is_init = true;
+  } else {
+    return true;
+  }
+
+  digitalWrite(INLABC, 1);  // enable
+  // driver.init(&SPI);
+  // status();
+  // motor->foc_modulation = FOCModulationType::SinePWM;
+  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+  // motor->controller = MotionControlType::velocity_openloop;
+  // motor->controller = MotionControlType::velocity;
+  motor->controller = MotionControlType::torque;
+  motor->torque_controller = TorqueControlType::voltage;
+  motor->useMonitoring(Serial);
+  drv8323s.focdriver->voltage_power_supply = vdrive_read;
+  motor->init();
+  delay(1000);
+
+  static Preferences pref;
+  pref.begin("simpleFOC");
+
+  // set voltage to run calibration
+  encoder_calibrated.voltage_calibration = motor->voltage_sensor_align;
+  encoder_calibrated.init();
+  // Running calibration
+  const char *encoder_calibration_lut_str = "enc_cal_lut";
+  const char *zero_electric_angle_str = "zero_el_angle";
+  const char *sensor_direction_str = "enc_dir_str";
+  const int settle_time_ms = 150;
+
+  const bool calibration_saveable = motor->sensor == &encoder_calibrated;
+
+  if (calibration_saveable &&
+      (!pref.isKey(encoder_calibration_lut_str) || pref.getBytesLength(encoder_calibration_lut_str) != sizeof(encoder_calibration_lut))) {
+    encoder_calibrated.calibrate(*motor, settle_time_ms);
+    pref.putBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
+    pref.putFloat(zero_electric_angle_str, motor->zero_electric_angle);
+    pref.putInt(sensor_direction_str, (int)motor->sensor_direction);
+  } else {
+    pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
+    Serial.println("Skipping calibration");
+  }
+
+  if (pref.isKey(zero_electric_angle_str) && calibration_saveable) {
+    motor->zero_electric_angle = pref.getFloat(zero_electric_angle_str);
+    motor->sensor_direction = (Direction)pref.getInt(sensor_direction_str);
+    motor->initFOC();
+  } else {
+    motor->initFOC();
+    if (calibration_saveable) {
+      pref.putFloat(zero_electric_angle_str, motor->zero_electric_angle);
+      pref.putInt(sensor_direction_str, (int)motor->sensor_direction);
+    }
+  }
+  pref.end();
+
+  motor->move(0);
+  digitalWrite(INLABC, 0);  // enable
+  return true;
+}
+
+int i = 0;
+void loop() {}
+
+void foc_task_loop() {
+  if(motor == NULL) {
+    delay(10);
+    return;
+  }
+  bool armed = false;
   /* adc reading */
   drv8323s.focdriver->voltage_power_supply = vdrive_read;
   const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
+  const float curr_mot_temp_kelvin = temp_mot + 273.15f;
 
   xSemaphoreTake(mutex, portMAX_DELAY);
   armed = servo_state.arming.armed;
+  max_voltage_limit = servo_state.max_voltage.value.count <= 0 ? 0 : servo_state.max_voltage.value.elements[0];
+  motor->voltage_sensor_align = servo_state.max_voltage.value.count <= 1 ? 0 : servo_state.max_voltage.value.elements[1];
 
   float target;
   if (!std::isnan(servo_state.target_position)) {
-    if (motor.controller != MotionControlType::angle) {
+    auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::angle_openloop : MotionControlType::angle);
-      motor.controller = MotionControlType::angle;
+    if (motor->controller != MotionControlType::angle) {
-      Serial.println("MotionControlType::angle");
+      motor->controller = MotionControlType::angle;
+      Serial.printf("MotionControlType::angle%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
     }
     target = servo_state.target_position;
 
-    motor.velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
+    motor->velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
-    motor.voltage_limit = std::isnan(servo_state.target_force)
+    motor->voltage_limit =
-                              ? max_voltage_limit
+        std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
-                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
   } else if (!std::isnan(servo_state.target_velocity)) {
-    if (motor.controller != MotionControlType::velocity) {
+    auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::velocity_openloop : MotionControlType::velocity);
-      motor.controller = MotionControlType::velocity;
+    if (motor->controller != target_control_type) {
-      Serial.println("MotionControlType::velocity");
+      motor->controller = target_control_type;
+      Serial.printf("MotionControlType::velocity%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
     }
     target = servo_state.target_velocity;
-    motor.voltage_limit = std::isnan(servo_state.target_force)
+    motor->voltage_limit =
-                              ? max_voltage_limit
+        std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
-                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
   } else {
-    motor.voltage_limit = std::isnan(servo_state.target_force)
+    if (is_openloop(servo_state.motor_type)) {
-                              ? max_voltage_limit
+      armed = false;
-                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
+    }
-    target = servo_state.target_force;
+    motor->voltage_limit =
-    if (motor.controller != MotionControlType::torque) {
+        std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
-      motor.controller = MotionControlType::torque;
+    target = std::isnan(servo_state.target_force) ? 0 : servo_state.target_force;
+    if (motor->controller != MotionControlType::torque) {
+      motor->controller = MotionControlType::torque;
       Serial.printf("MotionControlType::torque %f\n", target);
     }
   }
 
   servo_state.controller_temperature = curr_pcb_temp_kelvin;
-  servo_state.motor_temperature = NAN;
+  servo_state.motor_temperature = curr_mot_temp_kelvin;
 
-  float curr_angle = encoder_calibrated.getAngle();
+  float curr_angle = motor->sensor->getAngle();
   servo_state.curr_position = curr_angle;
-  servo_state.curr_force = motor.voltage.d;
+  servo_state.curr_force = motor->voltage.d;
 
   servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
-  servo_state.current = motor.current.d;
+  servo_state.current = motor->current.d;
 
-  motor.PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0];
+  motor->PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0];
-  motor.PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1];
+  motor->PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1];
-  motor.PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2];
+  motor->PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2];
-  motor.LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3];
+  motor->LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3];
-  motor.PID_velocity.output_ramp =
+  motor->PID_velocity.output_ramp = servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4];
-      servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4];
+  motor->PID_velocity.limit = servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
-  motor.PID_velocity.limit =
+  motor->motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6];
-      servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
-  motor.motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6];
 
-  motor.P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0];
+  motor->P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0];
-  motor.P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1];
+  motor->P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1];
-  motor.P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2];
+  motor->P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2];
-  // motor.P_angle.output_ramp = 10000; // default 1e6 rad/s^2
+  motor->LPF_angle.Tf = servo_state.pid_position.value.count <= 3 ? 0 : servo_state.pid_position.value.elements[3];
+  motor->P_angle.output_ramp = servo_state.pid_position.value.count <= 4 ? 1e6 : servo_state.pid_position.value.elements[4];
+  motor->P_angle.limit = servo_state.pid_position.value.count <= 5 ? 1000 : servo_state.pid_position.value.elements[5];
+
+  // motor->P_angle.output_ramp = 10000; // default 1e6 rad/s^2
   /* angle low pass filtering, use only for very noisy position sensors - try to avoid and keep the values very small */
-  motor.LPF_angle.Tf = 0;  // default 0[=disabled]
   // setting the limits
 
   xSemaphoreGive(mutex);
-
+  if (init_motor()) {
-  if (!armed) {
+    if (!armed) {
-    if (motor.enabled) {
+      if (motor->enabled) {
-      motor.move(0);
+        motor->move(0);
-      motor.disable();
+        motor->disable();
-      digitalWrite(INLABC, 0);
+        digitalWrite(INLABC, 0);
+        digitalWrite(LED_PIN, HIGH);
+      }
+    } else {
+      if (!motor->enabled) {
+        digitalWrite(INLABC, 1);
+        motor->enable();
+        digitalWrite(LED_PIN, LOW);
+      }
+      motor->move(target);
     }
+    motor->loopFOC();
   } else {
-    if (!motor.enabled) {
+    digitalWrite(LED_PIN, HIGH);
-      digitalWrite(INLABC, 1);
+    delay(10);
-      motor.enable();
+    //if(motor && motor->sensor) {
-    }
+    //  motor->sensor->update();
-    motor.move(target);
+    //}
+  }
+}
+void foc_task(void *parameter) {
+  while (true) {
+    foc_task_loop();
   }
-
-  digitalWrite(LED_PIN, armed ? LOW : HIGH);
-
-  motor.loopFOC();
-#else
-  encoder.update();  // optional: Update manually if not using loopfoc()
-#endif
 }

fw/src/pin_def_v5.h

@@ -0,0 +1,35 @@
+#pragma once
+#define ENC_A 11
+#define ENC_B 8
+
+#define SPI_ACC_nCS 0
+#define SPI_ENC_nCS 18
+
+#define SPI_CLK 40
+#define SPI_MISO 38
+#define SPI_MOSI 39
+
+#define DRV_nFAULT 15
+#define DRVEN 16
+#define TERMISTOR_PCB 3
+#define TERMISTOR_EXT 9
+#define SOA 1
+#define SOB 2
+#define SOC 10
+
+#define INHA 14
+#define INHB 12
+#define INHC 13
+#define INLABC 46
+
+#define CAN_TX 6
+#define CAN_RX 7
+
+#define SPI_DRV_MISO 45
+#define SPI_DRV_MOSI 48
+#define SPI_DRV_CLK 47
+#define SPI_DRV_SC 21
+
+#define LED_PIN 42
+#define VSENSE_PIN 5
+#define CAL_PIN 17

fw/src/ringbuf.hpp

@@ -0,0 +1,67 @@
+/*
+ *
+ * Copyright (c) [2022] by InvenSense, Inc.
+ * 
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+#include "ICM42670P.h"
+
+// Instantiate an ICM42670 with SPI interface and CS on pin 8
+ICM42670 IMU(SPI,8);
+
+void setup() {
+  int ret;
+  Serial.begin(115200);
+  while(!Serial) {}
+
+  // Initializing the ICM42670
+  ret = IMU.begin();
+  if (ret != 0) {
+    Serial.print("ICM42670 initialization failed: ");
+    Serial.println(ret);
+    while(1);
+  }
+  // Accel ODR = 100 Hz and Full Scale Range = 16G
+  IMU.startAccel(100,16);
+  // Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
+  IMU.startGyro(100,2000);
+  // Wait IMU to start
+  delay(100);
+}
+
+void loop() {
+
+  inv_imu_sensor_event_t imu_event;
+
+  // Get last event
+  IMU.getDataFromRegisters(imu_event);
+
+  // Format data for Serial Plotter
+  Serial.print("AccelX:");
+  Serial.println(imu_event.accel[0]);
+  Serial.print("AccelY:");
+  Serial.println(imu_event.accel[1]);
+  Serial.print("AccelZ:");
+  Serial.println(imu_event.accel[2]);
+  Serial.print("GyroX:");
+  Serial.println(imu_event.gyro[0]);
+  Serial.print("GyroY:");
+  Serial.println(imu_event.gyro[1]);
+  Serial.print("GyroZ:");
+  Serial.println(imu_event.gyro[2]);
+  Serial.print("Temperature:");
+  Serial.println(imu_event.temperature);
+
+  // Run @ ODR 100Hz
+  delay(10);
+}

fw/src/udral_servo.hpp

@@ -16,7 +16,7 @@
 #define NUNAVUT_ASSERT(x) assert(x)
 #include "canard.h"
 #include "esp32-hal.h"
-#include "pin_def.h"
+#include "pin_def_v5.h"
 
 // #include "socketcan.h"
 #include <assert.h>
@@ -60,6 +60,17 @@ const unsigned int hz = 50;
 /// We keep the state of the application here. Feel free to use static variables
 /// instead if desired.
 
+enum motor_type_t {
+  MOTOR_TYPE_BLDC_OPENLOOP = 0,
+  MOTOR_TYPE_BLDC = 1,
+  MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP = 2,
+  MOTOR_TYPE_HYBRID_STEPPER = 3,
+};
+
+bool is_openloop(const enum motor_type_t t) {
+  return !(((int)t)&0b1);
+}
+
 struct UdralServoState {
   /// Whether the servo is supposed to actuate the load or to stay idle (safe
   /// low-power mode).
@@ -93,6 +104,9 @@ struct UdralServoState {
 
     uavcan_primitive_array_Real32_1_0 pid_position;
     uavcan_primitive_array_Real32_1_0 pid_velocity;
+
+    uavcan_primitive_array_Real32_1_0 max_voltage;
+    enum motor_type_t motor_type;
   };
 };
 
@@ -143,6 +157,8 @@ struct UdralServoInternalState {
   state_sync_f user_state_sync_f;
 };
 
+static struct UdralServoInternalState state;
+
 /// This flag is raised when the node is requested to restart.
 static volatile bool g_restart_required = false;
 
@@ -194,18 +210,16 @@ static CanardPortID getSubjectID(const SubjectRole role, const char *const port_
 }
 
 static void send(struct UdralServoInternalState *const state, const CanardMicrosecond tx_deadline_usec,
-                 const CanardTransferMetadata *const metadata, const size_t payload_size,
+                 const CanardTransferMetadata *const metadata, const size_t payload_size, const void *const payload_data,
-                 const void *const payload_data, const CanardMicrosecond now_usec) {
+                 const CanardMicrosecond now_usec) {
   for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
     const struct CanardPayload payload = {.size = payload_size, .data = payload_data};
-    (void)canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload, now_usec,
+    (void)canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload, now_usec, NULL);
-                       NULL);
   }
 }
 
-static void sendResponse(struct UdralServoInternalState *const state,
+static void sendResponse(struct UdralServoInternalState *const state, const struct CanardRxTransfer *const original_request_transfer,
-                         const struct CanardRxTransfer *const original_request_transfer, const size_t payload_size,
+                         const size_t payload_size, const void *const payload_data, const CanardMicrosecond now_usec) {
-                         const void *const payload_data, const CanardMicrosecond now_usec) {
   CanardTransferMetadata meta = original_request_transfer->metadata;
   meta.transfer_kind = CanardTransferKindResponse;
   send(state, original_request_transfer->timestamp_usec + MEGA, &meta, payload_size, payload_data, now_usec);
@@ -219,8 +233,7 @@ static void handleFastLoop(struct UdralServoInternalState *const state, const Ca
     // snprintf(buf, sizeof(buf),
 
     Serial.printf("\rp=%.3f m    v=%.3f m/s    a=%.3f (m/s)^2    F=%.3f N    \r", (double)state->servo.target_position,
-                  (double)state->servo.target_velocity, (double)state->servo.target_acceleration,
+                  (double)state->servo.target_velocity, (double)state->servo.target_acceleration, (double)state->servo.target_force);
-                  (double)state->servo.target_force);
   } else {
     // fprintf(stderr, "\rDISARMED    \r");
   }
@@ -231,15 +244,14 @@ static void handleFastLoop(struct UdralServoInternalState *const state, const Ca
   // Publish feedback if the subject is enabled and the node is non-anonymous.
   if (!anonymous && (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX)) {
     reg_udral_service_actuator_common_Feedback_0_1 msg = {0};
-    msg.heartbeat.readiness.value = state->servo.arming.armed ? reg_udral_service_common_Readiness_0_1_ENGAGED
+    msg.heartbeat.readiness.value =
-                                                              : reg_udral_service_common_Readiness_0_1_STANDBY;
+        state->servo.arming.armed ? reg_udral_service_common_Readiness_0_1_ENGAGED : reg_udral_service_common_Readiness_0_1_STANDBY;
     // If there are any hardware or configuration issues, report them here:
     msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
     // Serialize and publish the message:
     uint8_t serialized[reg_udral_service_actuator_common_Feedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
     size_t serialized_size = sizeof(serialized);
-    const int8_t err =
+    const int8_t err = reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, &serialized[0], &serialized_size);
-        reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, &serialized[0], &serialized_size);
     assert(err >= 0);
     if (err >= 0) {
       const CanardTransferMetadata transfer = {
@@ -266,8 +278,7 @@ static void handleFastLoop(struct UdralServoInternalState *const state, const Ca
     // Serialize and publish the message:
     uint8_t serialized[reg_udral_physics_dynamics_rotation_PlanarTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
     size_t serialized_size = sizeof(serialized);
-    const int8_t err =
+    const int8_t err = reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
-        reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
     if (err >= 0) {
       const CanardTransferMetadata transfer = {
           .priority = CanardPriorityHigh,
@@ -398,8 +409,8 @@ static void handle1HzLoop(struct UdralServoInternalState *const state, const Can
   }
 
   // Disarm automatically if the arming subject has not been updated in a while.
-  if (state->servo.arming.armed && ((now_usec - state->servo.arming.last_update_at) >
+  if (state->servo.arming.armed &&
-                                    (uint64_t)(reg_udral_service_actuator_common___0_1_CONTROL_TIMEOUT * MEGA))) {
+      ((now_usec - state->servo.arming.last_update_at) > (uint64_t)(reg_udral_service_actuator_common___0_1_CONTROL_TIMEOUT * MEGA))) {
     state->servo.arming.armed = false;
     puts("Disarmed by timeout ");
   }
@@ -517,16 +528,14 @@ static void processMessagePlugAndPlayNodeIDAllocation(struct UdralServoInternalS
     reg.natural16.value.count = 1;
     registerWrite("uavcan.node.id", &reg);
     // We no longer need the subscriber, drop it to free up the resources (both memory and CPU time).
-    (void)canardRxUnsubscribe(&state->canard, CanardTransferKindMessage,
+    (void)canardRxUnsubscribe(&state->canard, CanardTransferKindMessage, uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_);
-                              uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_);
   } else {
     Serial.println("Ignoring processMessagePlugAndPlayNodeIDAllocation response");
   }
   // Otherwise, ignore it: either it is a request from another node or it is a response to another node.
 }
 
-static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand(
+static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand(const uavcan_node_ExecuteCommand_Request_1_1 *req) {
-    const uavcan_node_ExecuteCommand_Request_1_1 *req) {
   uavcan_node_ExecuteCommand_Response_1_1 resp = {0};
   switch (req->command) {
     case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE: {
@@ -590,25 +599,58 @@ static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const ua
   name[req->name.name.count] = '\0';
 
   uavcan_register_Access_Response_1_0 resp = {0};
-  // Serial.println("processRequestRegisterAccess");
+  Serial.printf("processRequestRegisterAccess name: %s\n\r", (char *)&req->name.name);
-  // Serial.print("name: ");
+  if (strcmp((char*)&req->name.name.elements[0], "reg.motor_type") == 0) {
-  // Serial.println((char *)&req->name.name);
+    uavcan_register_Value_1_0_select_string_(&resp.value);
+    if (!uavcan_register_Value_1_0_is_string_(&req->value)) {
+      if (state.servo.motor_type == MOTOR_TYPE_HYBRID_STEPPER) {
+        strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper");
+      } else if (state.servo.motor_type == MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP) {
+        strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop");
+      } else if (state.servo.motor_type == MOTOR_TYPE_BLDC) {
+        strcpy((char*)&resp.value._string.value.elements[0], "BLDC");
+      } else {
+        strcpy((char*)&resp.value._string.value.elements[0], "BLDC_openloop");
+      }
+      resp.value._string.value.count = strlen((char*)&resp.value._string.value.elements[0]);
 
-  // If we're asked to write a new value, do it now:
+      //Serial.printf("resp reg.motor_type: %s (%d)\n\r",(char*)&resp.value._string.value.elements[0], resp.value._string.value.count);
-  if (!uavcan_register_Value_1_0_is_empty_(&req->value)) {
+      Serial.printf("req  reg.motor_type: %s (%d)\n\r",(char*)&req->value._string.value.elements[0], req->value._string.value.count);
+      registerWrite("reg.motor_type", &resp.value);
+    }
+    if (strcmp((char*)&resp.value._string.value.elements[0], "HybridStepper") == 0) {
+      state.servo.motor_type = MOTOR_TYPE_HYBRID_STEPPER;
+      strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper");
+
+    } else  if (strcmp((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop") == 0) {
+      state.servo.motor_type = MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP;
+      strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop");
+
+    } else  if (strcmp((char*)&resp.value._string.value.elements[0], "BLDC") == 0) {
+      state.servo.motor_type = MOTOR_TYPE_BLDC;
+      strcpy((char*)&resp.value._string.value.elements[0], "BLDC");
+    } else {
+      state.servo.motor_type = MOTOR_TYPE_BLDC_OPENLOOP;
+      strcpy((char*)&resp.value._string.value.elements[0], "BLDC_openloop");
+    }
+    resp.value._string.value.count = strlen((char*)&resp.value._string.value.elements[0]);
+    registerWrite("reg.motor_type", &resp.value);
+  } else {
+    // If we're asked to write a new value, do it now:
+    if (!uavcan_register_Value_1_0_is_empty_(&req->value)) {
+      uavcan_register_Value_1_0_select_empty_(&resp.value);
+      registerRead(&name[0], &resp.value);
+      // If such register exists and it can be assigned from the request value:
+      if (!uavcan_register_Value_1_0_is_empty_(&resp.value) && registerAssign(&resp.value, &req->value)) {
+        registerWrite(&name[0], &resp.value);
+      }
+    }
+
+    // Regardless of whether we've just wrote a value or not, we need to read the current one and return it.
+    // The client will determine if the write was successful or not by comparing the request value with response.
     uavcan_register_Value_1_0_select_empty_(&resp.value);
     registerRead(&name[0], &resp.value);
-    // If such register exists and it can be assigned from the request value:
-    if (!uavcan_register_Value_1_0_is_empty_(&resp.value) && registerAssign(&resp.value, &req->value)) {
-      registerWrite(&name[0], &resp.value);
-    }
   }
-
-  // Regardless of whether we've just wrote a value or not, we need to read the current one and return it.
-  // The client will determine if the write was successful or not by comparing the request value with response.
-  uavcan_register_Value_1_0_select_empty_(&resp.value);
-  registerRead(&name[0], &resp.value);
-
   // Currently, all registers we implement are mutable and persistent. This is an acceptable simplification,
   // but more advanced implementations will need to differentiate between them to support advanced features like
   // exposing internal states via registers, perfcounters, etc.
@@ -645,14 +687,13 @@ static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo() {
   return resp;
 }
 
-static void processReceivedTransfer(struct UdralServoInternalState *const state,
+static void processReceivedTransfer(struct UdralServoInternalState *const state, const struct CanardRxTransfer *const transfer,
-                                    const struct CanardRxTransfer *const transfer, const CanardMicrosecond now_usec) {
+                                    const CanardMicrosecond now_usec) {
   if (transfer->metadata.transfer_kind == CanardTransferKindMessage) {
     size_t size = transfer->payload.size;
     if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) {
       reg_udral_physics_dynamics_rotation_Planar_0_1 msg = {0};
-      if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >=
+      if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) {
-          0) {
         processMessageServoSetpoint(state, &msg);
       }
     } else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) {
@@ -739,9 +780,7 @@ static void canardDeallocate(void *const user_reference, const size_t amount, vo
 
 int udral_loop(state_sync_f servo_state_sync_f) {
   srand(micros());
-  struct UdralServoInternalState state{
+  state.user_state_sync_f = servo_state_sync_f;
-      .user_state_sync_f = servo_state_sync_f,
-  };
   // A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack.
   // For the background and related theory refer to the following resources:
   // - https://github.com/OpenCyphal/libcanard/blob/master/README.md
@@ -765,11 +804,10 @@ int udral_loop(state_sync_f servo_state_sync_f) {
   uavcan_register_Value_1_0_select_natural16_(&val);
   val.natural16.value.count = 1;
   val.natural16.value.elements[0] = UINT16_MAX;  // This means undefined (anonymous), per Specification/libcanard.
-  registerRead("uavcan.node.id", &val);  // The names of the standard registers are regulated by the Specification.
+  registerRead("uavcan.node.id", &val);          // The names of the standard registers are regulated by the Specification.
   assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1));
-  state.canard.node_id = (val.natural16.value.elements[0] > CANARD_NODE_ID_MAX)
+  state.canard.node_id =
-                             ? CANARD_NODE_ID_UNSET
+      (val.natural16.value.elements[0] > CANARD_NODE_ID_MAX) ? CANARD_NODE_ID_UNSET : (CanardNodeID)val.natural16.value.elements[0];
-                             : (CanardNodeID)val.natural16.value.elements[0];
 
   // The description register is optional but recommended because it helps constructing/maintaining large networks.
   // It simply keeps a human-readable description of the node that should be empty by default.
@@ -791,6 +829,12 @@ int udral_loop(state_sync_f servo_state_sync_f) {
   val._string.value.count = strlen((const char *)val._string.value.elements);
   registerWrite("reg.udral.service.actuator.servo", &val);
 
+  uavcan_register_Access_Request_1_0 req;
+  strcpy((char*)&req.name.name.elements[0], "reg.motor_type");
+  req.name.name.count = strlen((char*)&req.name.name.elements[0]);
+  uavcan_register_Value_1_0_select_empty_(&req.value);
+  processRequestRegisterAccess(&req);
+
   // PID
   state.user_state_sync_f(&state.servo, hz);
 
@@ -820,6 +864,22 @@ int udral_loop(state_sync_f servo_state_sync_f) {
   }
   state.servo.pid_velocity = val.real32;
 
+  uavcan_register_Value_1_0_select_real32_(&val);
+  val.real32 = state.servo.max_voltage;
+  registerRead("reg.max_voltage", &val);
+    Serial.printf("reg.max_voltage count %d %d\n\r", state.servo.max_voltage.value.count, val.real32.value.count);
+  if (!uavcan_register_Value_1_0_is_real32_(&val) || val.real32.value.count < state.servo.max_voltage.value.count) {
+    for (int i = 0; i < uavcan_register_Value_1_0_is_real32_(&val) ? val.real32.value.count : 0; i += 1) {
+      state.servo.max_voltage.value.elements[i] = val.real32.value.elements[i];
+    }
+    uavcan_register_Value_1_0_select_real32_(&val);
+    val.real32 = state.servo.max_voltage;
+    registerWrite("reg.max_voltage", &val);
+  }
+  state.servo.max_voltage = val.real32;
+  // The description register is optional but recommended because it helps constructing/maintaining large networks.
+  // It simply keeps a human-readable description of the node that should be empty by default.
+
   // Configure the transport by reading the appropriate standard registers.
   uavcan_register_Value_1_0_select_natural16_(&val);
   val.natural16.value.count = 1;
@@ -841,24 +901,18 @@ int udral_loop(state_sync_f servo_state_sync_f) {
   // As follows from the Specification, the register group name prefix can be arbitrary; here we just use "servo".
   // Publications:
   state.port_id.pub.servo_feedback =  // High-rate status information: all good or not, engaged or sleeping.
-      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.feedback",
+      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.feedback", reg_udral_service_actuator_common_Feedback_0_1_FULL_NAME_AND_VERSION_);
-                   reg_udral_service_actuator_common_Feedback_0_1_FULL_NAME_AND_VERSION_);
   state.port_id.pub.servo_status =  // A low-rate high-level status overview: temperatures, fault flags, errors.
-      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.status",
+      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.status", reg_udral_service_actuator_common_Status_0_1_FULL_NAME_AND_VERSION_);
-                   reg_udral_service_actuator_common_Status_0_1_FULL_NAME_AND_VERSION_);
   state.port_id.pub.servo_power =  // Electric power input measurements (voltage and current).
-      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.power",
+      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.power", reg_udral_physics_electricity_PowerTs_0_1_FULL_NAME_AND_VERSION_);
-                   reg_udral_physics_electricity_PowerTs_0_1_FULL_NAME_AND_VERSION_);
   state.port_id.pub.servo_dynamics =  // Position/speed/acceleration/force feedback.
-      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.dynamics",
+      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.dynamics", reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_);
-                   reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_);
   // Subscriptions:
   state.port_id.sub.servo_setpoint =  // This message actually commands the servo setpoint with the motion profile.
-      getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.setpoint",
+      getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.setpoint", reg_udral_physics_dynamics_rotation_Planar_0_1_FULL_NAME_AND_VERSION_);
-                   reg_udral_physics_dynamics_rotation_Planar_0_1_FULL_NAME_AND_VERSION_);
   state.port_id.sub.servo_readiness =  // Arming subject: whether to act upon the setpoint or to stay idle.
-      getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.readiness",
+      getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.readiness", reg_udral_service_common_Readiness_0_1_FULL_NAME_AND_VERSION_);
-                   reg_udral_service_common_Readiness_0_1_FULL_NAME_AND_VERSION_);
 
   // Set up subject subscriptions and RPC-service servers.
   // Message subscriptions:
@@ -867,8 +921,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
     static struct CanardRxSubscription rx;
     const int8_t res =  //
         canardRxSubscribe(&state.canard, CanardTransferKindMessage, uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
-                          uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+                          uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
-                          &rx);
     if (res < 0) {
       return -res;
     }
@@ -907,8 +960,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
     static struct CanardRxSubscription rx;
     const int8_t res =  //
         canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
-                          uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+                          uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
-                          &rx);
     if (res < 0) {
       return -res;
     }
@@ -917,8 +969,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
     static struct CanardRxSubscription rx;
     const int8_t res =  //
         canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_register_Access_1_0_FIXED_PORT_ID_,
-                          uavcan_register_Access_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+                          uavcan_register_Access_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
-                          &rx);
     if (res < 0) {
       return -res;
     }
@@ -966,14 +1017,13 @@ int udral_loop(state_sync_f servo_state_sync_f) {
         // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
         // Otherwise just drop it and move on to the next one.
         if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) {
-          const struct CanardFrame canard_frame = {
+          const struct CanardFrame canard_frame = {.extended_can_id = tqi->frame.extended_can_id,
-              .extended_can_id = tqi->frame.extended_can_id,
+                                                   .payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}};
-              .payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}};
           // const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0);  // Non-blocking write attempt.
-          const int16_t result = esp32twaicanPush(
+          const int16_t result =
-              &canard_frame,
+              esp32twaicanPush(&canard_frame,
-              0);  // Non-blocking write attempt.
+                               0);  // Non-blocking write attempt.
-                   // digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
+                                    // digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);
 
           // const int16_t result = 0;
           if (result == 0) {
@@ -1014,8 +1064,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
       const int8_t canard_result = canardRxAccept(&state.canard, timestamp_usec, &frame, ifidx, &transfer, NULL);
       if (canard_result > 0) {
         processReceivedTransfer(&state, &transfer, now_usec);
-        state.canard.memory.deallocate(state.canard.memory.user_reference, transfer.payload.allocated_size,
+        state.canard.memory.deallocate(state.canard.memory.user_reference, transfer.payload.allocated_size, transfer.payload.data);
-                                       transfer.payload.data);
       } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) {
         (void)0;  // The frame did not complete a transfer so there is nothing to do.
                   // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API.

pcb/stepper-closed-loop.kicad_prl

@@ -5,7 +5,7 @@
     "auto_track_width": false,
     "hidden_netclasses": [],
     "hidden_nets": [],
-    "high_contrast_mode": 0,
+    "high_contrast_mode": 1,
     "net_color_mode": 1,
     "opacity": {
       "images": 0.6,