lars bldc parameter

fw/src/DRV8323S.hpp

@@ -211,7 +211,7 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
   /* Set 3PWM Mode */
   drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM);
   delay(1);
-  drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
+  // drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
   delay(1);
   drv8323s_set_sen_lvl(dev, DRV8323S_SEN_LVL_0_25V);
   delay(1);

fw/src/main.cpp

@@ -1,8 +1,12 @@
-// #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 "encoders/abs_inc_combine/AbsIncCombineSensor.h"
+#include "encoders/as5047/AS5047.h"
 #include "esp32-hal.h"
 #define NODE_NAME "N17BLDC"
 
@@ -38,9 +42,6 @@ USBCDC usbserial;
 #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"
@@ -52,7 +53,7 @@ USBCDC usbserial;
 
 #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
 //* 20.8f/ 21033.75)
-
+//#define ESPHWENC
 #define MOTOR
 #define FW_NO_WATCHDOG
 
@@ -67,27 +68,35 @@ USBCDC usbserial;
 #endif
 
 const int voltage_lpf = 50;
-const float max_voltage_limit = 6;
+const float max_voltage_limit = 2.3;
 
-// 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);
+//HybridStepperMotor motor = HybridStepperMotor(50);  //, 3.7, 10, 4.5 / 1000);
-#endif
 
+
+BLDCMotor motor = BLDCMotor(7, 0.7, 360, 0.15);
+// BLDCMotor motor = BLDCMotor(7);
+//BLDCDriver3PWM driver = BLDCDriver3PWM(16, 5, 17, 4);
+#endif
 void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
   Wire.beginTransmission(addr);
   Wire.write(reg);  // MAN:0x2E
@@ -130,10 +139,13 @@ 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;
+    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[1] = motor.PID_velocity.I;
@@ -141,7 +153,7 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
     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[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 +179,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 +195,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);
 
@@ -211,7 +215,9 @@ void setup() {
   //  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();
@@ -223,12 +229,13 @@ void setup() {
   digitalWrite(INLABC, 1);  // enable
   // driver.init(&SPI);
   // status();
-  motor.linkSensor(&encoder_calibrated);
+  motor.linkSensor(&encoder_absolute);
+  //motor.linkSensor(&encoder_calibrated);
   motor.linkDriver(drv8323s.focdriver);
   // motor.foc_modulation = FOCModulationType::SinePWM;
   motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
 
-  motor.voltage_sensor_align = 4;
+  motor.voltage_sensor_align = 1.5;
   motor.voltage_limit = max_voltage_limit;
   // motor.controller = MotionControlType::velocity_openloop;
   // motor.controller = MotionControlType::velocity;
@@ -250,8 +257,7 @@ void setup() {
   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) ||
+  if (motor.sensor == &encoder_calibrated && (!pref.isKey(encoder_calibration_lut_str) || pref.getBytesLength(encoder_calibration_lut_str) != sizeof(encoder_calibration_lut))) {
-      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);
@@ -262,7 +268,7 @@ void setup() {
     Serial.println("Skipping calibration");
   }
 
-  if (!pref.isKey(zero_electric_angle_str)) {
+  if (!pref.isKey(zero_electric_angle_str) || motor.sensor != &encoder_calibrated) {
     motor.initFOC();
     pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
     pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
@@ -287,17 +293,33 @@ 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
 
+  digitalWrite(LED_PIN, 1);  // disable
+  delay(100);
+  digitalWrite(LED_PIN, 0);  // enable
+  delay(400);
+  digitalWrite(LED_PIN, 1);  // disable
+  delay(100);
   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
+  );
+}
+
+int i = 0;
+void loop() {}
+
+void foc_task_loop() {
+#ifdef MOTOR
+  bool armed = false;
   /* adc reading */
   drv8323s.focdriver->voltage_power_supply = vdrive_read;
   const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
@@ -314,23 +336,20 @@ void loop() {
     target = servo_state.target_position;
 
     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) {
       motor.controller = MotionControlType::velocity;
       Serial.println("MotionControlType::velocity");
     }
     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)
+    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));
+    target = std::isnan(servo_state.target_force) ? 0 : servo_state.target_force;
-    target = servo_state.target_force;
     if (motor.controller != MotionControlType::torque) {
       motor.controller = MotionControlType::torque;
       Serial.printf("MotionControlType::torque %f\n", target);
@@ -340,7 +359,7 @@ void loop() {
   servo_state.controller_temperature = curr_pcb_temp_kelvin;
   servo_state.motor_temperature = NAN;
 
-  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;
 
@@ -351,18 +370,18 @@ void loop() {
   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.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 =
-      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.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.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);
@@ -388,3 +407,9 @@ void loop() {
   encoder.update();  // optional: Update manually if not using loopfoc()
 #endif
 }
+
+void foc_task(void *parameter) {
+  while (true) {
+    foc_task_loop();
+  }
+}

fw/src/udral_servo.hpp

@@ -194,18 +194,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 +217,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 +228,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 +262,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 +393,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 +512,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: {
@@ -645,14 +638,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) {
@@ -765,11 +757,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.
@@ -841,24 +832,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 +852,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 +891,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 +900,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 +948,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 +995,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,