diff --git a/fw/src/DRV8323S.hpp b/fw/src/DRV8323S.hpp index 6158283..322c3db 100644 --- a/fw/src/DRV8323S.hpp +++ b/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); diff --git a/fw/src/main.cpp b/fw/src/main.cpp index 3cdfdcd..fb30b1e 100644 --- a/fw/src/main.cpp +++ b/fw/src/main.cpp @@ -1,8 +1,12 @@ -// #include +/* +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 #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 #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 @@ -69,16 +70,20 @@ USBCDC usbserial; const int voltage_lpf = 50; 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, - sizeof(encoder_calibration_lut) / sizeof(encoder_calibration_lut[0]), +CalibratedSensor encoder_calibrated(abs_inc_sensor, sizeof(encoder_calibration_lut) / sizeof(encoder_calibration_lut[0]), encoder_calibration_lut); // MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36); @@ -130,10 +135,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 +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[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 +175,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 +191,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 +211,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(); @@ -250,8 +252,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) || - pref.getBytesLength(encoder_calibration_lut_str) != sizeof(encoder_calibration_lut)) { + 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); @@ -287,17 +288,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 +331,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) - ? max_voltage_limit - : std::min(max_voltage_limit, std::abs(servo_state.target_force)); + motor.voltage_limit = + std::isnan(servo_state.target_force) ? max_voltage_limit : 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) - ? max_voltage_limit - : std::min(max_voltage_limit, std::abs(servo_state.target_force)); + motor.voltage_limit = + std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force)); } else { - motor.voltage_limit = std::isnan(servo_state.target_force) - ? max_voltage_limit - : std::min(max_voltage_limit, std::abs(servo_state.target_force)); - target = servo_state.target_force; + motor.voltage_limit = + std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force)); + 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); @@ -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.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 = - 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.output_ramp = 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.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); @@ -387,4 +401,10 @@ void loop() { #else encoder.update(); // optional: Update manually if not using loopfoc() #endif +} + +void foc_task(void *parameter) { + while (true) { + foc_task_loop(); + } } \ No newline at end of file diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp index 7b3ea0c..4b7ad0f 100644 --- a/fw/src/udral_servo.hpp +++ b/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 void *const payload_data, const CanardMicrosecond now_usec) { + const CanardTransferMetadata *const metadata, const size_t payload_size, const void *const payload_data, + 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, - NULL); + (void)canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload, now_usec, NULL); } } -static void sendResponse(struct UdralServoInternalState *const state, - const struct CanardRxTransfer *const original_request_transfer, const size_t payload_size, - const void *const payload_data, const CanardMicrosecond now_usec) { +static void sendResponse(struct UdralServoInternalState *const state, const struct CanardRxTransfer *const original_request_transfer, + const size_t payload_size, 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_force); + (double)state->servo.target_velocity, (double)state->servo.target_acceleration, (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 - : reg_udral_service_common_Readiness_0_1_STANDBY; + msg.heartbeat.readiness.value = + 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 = - reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, &serialized[0], &serialized_size); + const int8_t err = 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 = - reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); + const int8_t err = 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) > - (uint64_t)(reg_udral_service_actuator_common___0_1_CONTROL_TIMEOUT * MEGA))) { + if (state->servo.arming.armed && + ((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", ®); // We no longer need the subscriber, drop it to free up the resources (both memory and CPU time). - (void)canardRxUnsubscribe(&state->canard, CanardTransferKindMessage, - uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_); + (void)canardRxUnsubscribe(&state->canard, CanardTransferKindMessage, 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( - const uavcan_node_ExecuteCommand_Request_1_1 *req) { +static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand(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, - const struct CanardRxTransfer *const transfer, const CanardMicrosecond now_usec) { +static void processReceivedTransfer(struct UdralServoInternalState *const state, const struct CanardRxTransfer *const transfer, + 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) >= - 0) { + if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 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) - ? CANARD_NODE_ID_UNSET - : (CanardNodeID)val.natural16.value.elements[0]; + state.canard.node_id = + (val.natural16.value.elements[0] > CANARD_NODE_ID_MAX) ? CANARD_NODE_ID_UNSET : (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", - reg_udral_service_actuator_common_Feedback_0_1_FULL_NAME_AND_VERSION_); + getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.feedback", 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", - reg_udral_service_actuator_common_Status_0_1_FULL_NAME_AND_VERSION_); + getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.status", 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", - reg_udral_physics_electricity_PowerTs_0_1_FULL_NAME_AND_VERSION_); + getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.power", 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", - reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_); + getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.dynamics", 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", - reg_udral_physics_dynamics_rotation_Planar_0_1_FULL_NAME_AND_VERSION_); + getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.setpoint", 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", - reg_udral_service_common_Readiness_0_1_FULL_NAME_AND_VERSION_); + getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.readiness", 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, - &rx); + uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &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, - &rx); + uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &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, - &rx); + uavcan_register_Access_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &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 = { - .extended_can_id = tqi->frame.extended_can_id, - .payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}}; + const struct CanardFrame canard_frame = {.extended_can_id = tqi->frame.extended_can_id, + .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( - &canard_frame, - 0); // Non-blocking write attempt. - // digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000); + const int16_t result = + esp32twaicanPush(&canard_frame, + 0); // Non-blocking write attempt. + // 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, - transfer.payload.data); + state.canard.memory.deallocate(state.canard.memory.user_reference, transfer.payload.allocated_size, 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. diff --git a/pcb/stepper-closed-loop.kicad_prl b/pcb/stepper-closed-loop.kicad_prl index 6b44085..6eff5ee 100644 --- a/pcb/stepper-closed-loop.kicad_prl +++ b/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,