nils/N17BLDC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip fw

Browse Source
This commit is contained in:
Nils Schulte 2025-08-13 21:31:18 +02:00
parent 8170663b59
commit 8af848b01b
4 changed files with 103 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
fw/src/DRV8323S.hpp
View File

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

110
fw/src/main.cpp
View File

@ -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 <cmath>
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h" #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/AS5047.h"
#include "esp32-hal.h" #include "esp32-hal.h"
#define NODE_NAME "N17BLDC" #define NODE_NAME "N17BLDC"
@ -38,9 +42,6 @@ USBCDC usbserial;
#include "encoders/abs_inc_combine/AbsIncCombineSensor.h" #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
#include "encoders/as5047/MagneticSensorAS5047.h" #include "encoders/as5047/MagneticSensorAS5047.h"
#include "encoders/calibrated/CalibratedSensor.h" #include "encoders/calibrated/CalibratedSensor.h"
// #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
#include "DRV8323S.hpp" #include "DRV8323S.hpp"
#include "HybridStepperMotor.h" #include "HybridStepperMotor.h"
@ -52,7 +53,7 @@ USBCDC usbserial;
#define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845) #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
//* 20.8f/ 21033.75) //* 20.8f/ 21033.75)
//#define ESPHWENC
#define MOTOR #define MOTOR
#define FW_NO_WATCHDOG #define FW_NO_WATCHDOG
@ -69,16 +70,20 @@ USBCDC usbserial;
const int voltage_lpf = 50; const int voltage_lpf = 50;
const float max_voltage_limit = 6; 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); MagneticSensorAS5047 encoder_absolute(18);
Encoder encoder(ENC_A, ENC_B, 4096 / 4, 0);
AbsIncCombineSensor abs_inc_sensor(encoder, encoder_absolute); AbsIncCombineSensor abs_inc_sensor(encoder, encoder_absolute);
float encoder_calibration_lut[90]; 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); encoder_calibration_lut);
// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36); // MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
@ -130,10 +135,13 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
xSemaphoreTake(mutex, portMAX_DELAY); xSemaphoreTake(mutex, portMAX_DELAY);
if (!init_done) { if (!init_done) {
init_done = true; 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[0] = motor.P_angle.P;
state->pid_position.value.elements[1] = motor.P_angle.I; 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[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.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;
@ -141,7 +149,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[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_force = state->target_force;
servo_state.target_acceleration = state->target_acceleration; servo_state.target_acceleration = state->target_acceleration;
@ -167,12 +175,11 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
xSemaphoreGive(mutex); xSemaphoreGive(mutex);
} }
TaskHandle_t taskFocHandle;
void foc_task(void *parameter);
TaskHandle_t taskCommHandle; TaskHandle_t taskCommHandle;
void comm_task(void *parameter) { udral_loop(mutexed_state_sync); } void comm_task(void *parameter) { udral_loop(mutexed_state_sync); }
void doA1() { encoder.handleA(); }
void doB1() { encoder.handleB(); }
void setup() { void setup() {
pinMode(LED_PIN, OUTPUT); pinMode(LED_PIN, OUTPUT);
mutex = xSemaphoreCreateMutex(); mutex = xSemaphoreCreateMutex();
@ -184,13 +191,6 @@ void setup() {
#else #else
Serial.begin(460800, SERIAL_8N1, 44, 43); Serial.begin(460800, SERIAL_8N1, 44, 43);
#endif #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); pinMode(SPI_DRV_SC, OUTPUT);
@ -211,7 +211,9 @@ void setup() {
// initialise magnetic sensor hardware // initialise magnetic sensor hardware
encoder.pullup = Pullup::USE_INTERN; encoder.pullup = Pullup::USE_INTERN;
encoder.init(); encoder.init();
#ifndef ESPHWENC
encoder.enableInterrupts(doA1, doB1); encoder.enableInterrupts(doA1, doB1);
#endif
encoder_absolute.init(&spi_dev); encoder_absolute.init(&spi_dev);
abs_inc_sensor.init(); abs_inc_sensor.init();
@ -250,8 +252,7 @@ void setup() {
const char *zero_electric_angle_str = "zero_el_angle"; const char *zero_electric_angle_str = "zero_el_angle";
const char *sensor_direction_str = "enc_dir_str"; const char *sensor_direction_str = "enc_dir_str";
const int settle_time_ms = 150; const int settle_time_ms = 150;
if (!pref.isKey(encoder_calibration_lut_str) || if (!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); encoder_calibrated.calibrate(motor, settle_time_ms);
pref.putBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut)); 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.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
@ -287,17 +288,33 @@ void setup() {
); );
// esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts // esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts
// esp_task_wdt_add(NULL); // add current thread to WDT watch // 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) { while (!init_done) {
delay(10); 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 */ /* adc reading */
drv8323s.focdriver->voltage_power_supply = vdrive_read; drv8323s.focdriver->voltage_power_supply = vdrive_read;
const float curr_pcb_temp_kelvin = temp_pcb + 273.15f; const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
@ -314,23 +331,20 @@ void loop() {
target = servo_state.target_position; 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)) { } else if (!std::isnan(servo_state.target_velocity)) {
if (motor.controller != MotionControlType::velocity) { if (motor.controller != MotionControlType::velocity) {
motor.controller = MotionControlType::velocity; motor.controller = MotionControlType::velocity;
Serial.println("MotionControlType::velocity"); Serial.println("MotionControlType::velocity");
} }
target = servo_state.target_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 { } 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) { if (motor.controller != MotionControlType::torque) {
motor.controller = MotionControlType::torque; motor.controller = MotionControlType::torque;
Serial.printf("MotionControlType::torque %f\n", target); Serial.printf("MotionControlType::torque %f\n", target);
@ -351,18 +365,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.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 =
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.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.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 // 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 */ /* 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 // setting the limits
xSemaphoreGive(mutex); xSemaphoreGive(mutex);
@ -387,4 +401,10 @@ void loop() {
#else #else
encoder.update(); // optional: Update manually if not using loopfoc() encoder.update(); // optional: Update manually if not using loopfoc()
#endif #endif
}
void foc_task(void *parameter) {
while (true) {
foc_task_loop();
}
} }

92
fw/src/udral_servo.hpp
View File

@ -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, 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++) { for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
const struct CanardPayload payload = {.size = payload_size, .data = payload_data}; 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; CanardTransferMetadata meta = original_request_transfer->metadata;
meta.transfer_kind = CanardTransferKindResponse; meta.transfer_kind = CanardTransferKindResponse;
send(state, original_request_transfer->timestamp_usec + MEGA, &meta, payload_size, payload_data, now_usec); 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), // 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, 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 { } else {
// fprintf(stderr, "\rDISARMED \r"); // 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. // 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)) { if (!anonymous && (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX)) {
reg_udral_service_actuator_common_Feedback_0_1 msg = {0}; 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: // If there are any hardware or configuration issues, report them here:
msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
// Serialize and publish the message: // Serialize and publish the message:
uint8_t serialized[reg_udral_service_actuator_common_Feedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; uint8_t serialized[reg_udral_service_actuator_common_Feedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); 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); assert(err >= 0);
if (err >= 0) { if (err >= 0) {
const CanardTransferMetadata transfer = { const CanardTransferMetadata transfer = {
@ -266,8 +262,7 @@ static void handleFastLoop(struct UdralServoInternalState *const state, const Ca
// Serialize and publish the message: // Serialize and publish the message:
uint8_t serialized[reg_udral_physics_dynamics_rotation_PlanarTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; uint8_t serialized[reg_udral_physics_dynamics_rotation_PlanarTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); 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) { if (err >= 0) {
const CanardTransferMetadata transfer = { const CanardTransferMetadata transfer = {
.priority = CanardPriorityHigh, .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. // 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; state->servo.arming.armed = false;
puts("Disarmed by timeout "); puts("Disarmed by timeout ");
} }
@ -517,16 +512,14 @@ static void processMessagePlugAndPlayNodeIDAllocation(struct UdralServoInternalS
reg.natural16.value.count = 1; reg.natural16.value.count = 1;
registerWrite("uavcan.node.id", &reg); registerWrite("uavcan.node.id", &reg);
// We no longer need the subscriber, drop it to free up the resources (both memory and CPU time). // 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 { } else {
Serial.println("Ignoring processMessagePlugAndPlayNodeIDAllocation response"); Serial.println("Ignoring processMessagePlugAndPlayNodeIDAllocation response");
} }
// Otherwise, ignore it: either it is a request from another node or it is a response to another node. // 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}; uavcan_node_ExecuteCommand_Response_1_1 resp = {0};
switch (req->command) { switch (req->command) {
case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE: { 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; 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) { if (transfer->metadata.transfer_kind == CanardTransferKindMessage) {
size_t size = transfer->payload.size; size_t size = transfer->payload.size;
if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) { if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) {
reg_udral_physics_dynamics_rotation_Planar_0_1 msg = {0}; 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); processMessageServoSetpoint(state, &msg);
} }
} else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) { } 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); uavcan_register_Value_1_0_select_natural16_(&val);
val.natural16.value.count = 1; val.natural16.value.count = 1;
val.natural16.value.elements[0] = UINT16_MAX; // This means undefined (anonymous), per Specification/libcanard. 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)); 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. // 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. // 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". // As follows from the Specification, the register group name prefix can be arbitrary; here we just use "servo".
// Publications: // Publications:
state.port_id.pub.servo_feedback = // High-rate status information: all good or not, engaged or sleeping. 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. 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). 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. 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: // Subscriptions:
state.port_id.sub.servo_setpoint = // This message actually commands the servo setpoint with the motion profile. 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. 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. // Set up subject subscriptions and RPC-service servers.
// Message subscriptions: // Message subscriptions:
@ -867,8 +852,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
static struct CanardRxSubscription rx; static struct CanardRxSubscription rx;
const int8_t res = // const int8_t res = //
canardRxSubscribe(&state.canard, CanardTransferKindMessage, uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, 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) { if (res < 0) {
return -res; return -res;
} }
@ -907,8 +891,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
static struct CanardRxSubscription rx; static struct CanardRxSubscription rx;
const int8_t res = // const int8_t res = //
canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_, 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) { if (res < 0) {
return -res; return -res;
} }
@ -917,8 +900,7 @@ int udral_loop(state_sync_f servo_state_sync_f) {
static struct CanardRxSubscription rx; static struct CanardRxSubscription rx;
const int8_t res = // const int8_t res = //
canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_register_Access_1_0_FIXED_PORT_ID_, 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) { if (res < 0) {
return -res; 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. // 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. // Otherwise just drop it and move on to the next one.
if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) { 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 = 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; // const int16_t result = 0;
if (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); const int8_t canard_result = canardRxAccept(&state.canard, timestamp_usec, &frame, ifidx, &transfer, NULL);
if (canard_result > 0) { if (canard_result > 0) {
processReceivedTransfer(&state, &transfer, now_usec); 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)) { } 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. (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. // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API.

2
pcb/stepper-closed-loop.kicad_prl
View File

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