From a21ebe6ae1fbeea9ce23e8a2aec3bca2d82b3aed Mon Sep 17 00:00:00 2001 From: Nils Schulte Date: Sun, 20 Jul 2025 14:41:40 +0200 Subject: [PATCH] velocity working? y r 125 reg.pid_velocity 0.001 2.5 0.0 0.01 1000.0 3.0 100.0 \ && y cmd 125 restart --- fw/src/.clang-format | 3 + fw/src/main.cpp | 166 ++-- fw/src/udral_servo.hpp | 1719 ++++++++++++++++++---------------------- 3 files changed, 903 insertions(+), 985 deletions(-) create mode 100644 fw/src/.clang-format diff --git a/fw/src/.clang-format b/fw/src/.clang-format new file mode 100644 index 0000000..09ac0c6 --- /dev/null +++ b/fw/src/.clang-format @@ -0,0 +1,3 @@ +Language: Cpp +BasedOnStyle: google +ColumnLimit: 120 \ No newline at end of file diff --git a/fw/src/main.cpp b/fw/src/main.cpp index 11323cf..3cdfdcd 100644 --- a/fw/src/main.cpp +++ b/fw/src/main.cpp @@ -1,8 +1,9 @@ // #include +#include + #include "encoders/abs_inc_combine/AbsIncCombineSensor.h" #include "esp32-hal.h" -#include #define NODE_NAME "N17BLDC" #define VERSION_MAJOR 0 @@ -12,7 +13,6 @@ #include "Arduino.h" #include "pin_def.h" - #define USE_USBSERIAL #ifdef USE_USBSERIAL #include "USB.h" @@ -26,36 +26,29 @@ USBCDC usbserial; #endif // #define Serial Serial -#include "canard.c" - #include + +#include "canard.c" +#include "udral_servo.hpp" #define spi_dev SPI // #include "Wire.h" #include #include "SimpleFOCDrivers.h" -#include "encoders/calibrated/CalibratedSensor.h" #include "encoders/abs_inc_combine/AbsIncCombineSensor.h" #include "encoders/as5047/MagneticSensorAS5047.h" +#include "encoders/calibrated/CalibratedSensor.h" // #include "encoders/esp32hwencoder/ESP32HWEncoder.h" -#include "DRV8323S.hpp" +#include +#include "DRV8323S.hpp" #include "HybridStepperMotor.h" #include "esp32-hal-adc.h" #include "esp32-hal-gpio.h" -#include "udral_servo.hpp" - -#include - -#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(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 vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845) //* 20.8f/ 21033.75) @@ -74,6 +67,7 @@ USBCDC usbserial; #endif 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 @@ -83,19 +77,20 @@ 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]), encoder_calibration_lut); +CalibratedSensor encoder_calibrated(encoder_absolute, + 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 void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) { Wire.beginTransmission(addr); - Wire.write(reg); // MAN:0x2E + Wire.write(reg); // MAN:0x2E int error = Wire.endTransmission(); if (error != 0) { return; @@ -129,14 +124,32 @@ void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) { SemaphoreHandle_t mutex; struct UdralServoState servo_state = {0}; +static bool init_done = false; + 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.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_velocity.value.count = 7; + 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[2] = motor.PID_velocity.D; + 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; + } servo_state.target_force = state->target_force; servo_state.target_acceleration = state->target_acceleration; servo_state.target_velocity = state->target_velocity; servo_state.target_position = state->target_position; servo_state.arming = state->arming; - servo_state.pid = state->pid; + servo_state.pid_position = state->pid_position; + servo_state.pid_velocity = state->pid_velocity; float curr_angle = servo_state.curr_position; static float curr_angle_last = curr_angle; @@ -144,7 +157,7 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) { curr_angle_last = curr_angle; static float curr_vel_last = curr_vel; - float curr_accel =(curr_vel - curr_vel_last) * hz; + float curr_accel = (curr_vel - curr_vel_last) * hz; curr_vel_last = curr_vel; servo_state.curr_acceleration = curr_accel; @@ -166,15 +179,14 @@ void setup() { #ifdef USE_USBSERIAL Serial.begin(); USB.begin(); - Serial.setDebugOutput( - true); // sends all log_e(), log_i() messages to USB HW CDC + Serial.setDebugOutput(true); // sends all log_e(), log_i() messages to USB HW CDC Serial.setTxTimeoutMs(0); #else Serial.begin(460800, SERIAL_8N1, 44, 43); #endif - digitalWrite(LED_PIN, 0); // enable + digitalWrite(LED_PIN, 0); // enable delay(500); - digitalWrite(LED_PIN, 1); // disable + digitalWrite(LED_PIN, 1); // disable delay(100); Serial.println("Start"); @@ -189,14 +201,14 @@ void setup() { pinMode(INHC, OUTPUT); // digitalWrite(INHC, 0); // enable pinMode(INLABC, OUTPUT); - digitalWrite(INLABC, 0); // enable + digitalWrite(INLABC, 0); // enable pinMode(CAL_PIN, OUTPUT); - digitalWrite(CAL_PIN, 0); // enable + digitalWrite(CAL_PIN, 0); // enable pinMode(SPI_MISO, INPUT_PULLUP); SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC); - //SPI.setFrequency(100000); - // initialise magnetic sensor hardware + // SPI.setFrequency(100000); + // initialise magnetic sensor hardware encoder.pullup = Pullup::USE_INTERN; encoder.init(); encoder.enableInterrupts(doA1, doB1); @@ -208,7 +220,7 @@ void setup() { #ifdef MOTOR SimpleFOCDebug::enable(&Serial); drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev); - digitalWrite(INLABC, 1); // enable + digitalWrite(INLABC, 1); // enable // driver.init(&SPI); // status(); motor.linkSensor(&encoder_calibrated); @@ -217,7 +229,7 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; motor.voltage_sensor_align = 4; - motor.voltage_limit = 6; + motor.voltage_limit = max_voltage_limit; // motor.controller = MotionControlType::velocity_openloop; // motor.controller = MotionControlType::velocity; motor.controller = MotionControlType::torque; @@ -239,18 +251,14 @@ void setup() { 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)) { + 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.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)); + pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut)); Serial.println("Skipping calibration"); } @@ -266,16 +274,16 @@ void setup() { pref.end(); motor.move(0); - digitalWrite(INLABC, 0); // enable + 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) - NULL, // Parameter to pass - 10, // Task priority - &taskCommHandle, // Assign task handle - 0 // Run on the non-Arduino (1) core + xTaskCreatePinnedToCore(&comm_task, // Function name of the task + "comm", // Name of the task (e.g. for debugging) + 65536, // Stack size (bytes) + NULL, // Parameter to pass + 10, // Task priority + &taskCommHandle, // Assign task handle + 0 // Run on the non-Arduino (1) core ); // esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts // esp_task_wdt_add(NULL); // add current thread to WDT watch @@ -284,6 +292,10 @@ void setup() { int i = 0; void loop() { #ifdef MOTOR + + while (!init_done) { + delay(10); + } bool armed = false; /* adc reading */ @@ -292,17 +304,67 @@ void loop() { xSemaphoreTake(mutex, portMAX_DELAY); armed = servo_state.arming.armed; - float target = servo_state.target_force; + + float target; + if (!std::isnan(servo_state.target_position)) { + if (motor.controller != MotionControlType::angle) { + motor.controller = MotionControlType::angle; + Serial.println("MotionControlType::angle"); + } + 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)); + } 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)); + } 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; + 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; float curr_angle = encoder_calibrated.getAngle(); servo_state.curr_position = curr_angle; - servo_state.curr_force = servo_state.target_force; + servo_state.curr_force = motor.voltage.d; servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply; - servo_state.current = NAN; + 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.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.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.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 (!armed) { @@ -323,6 +385,6 @@ void loop() { motor.loopFOC(); #else - encoder.update(); // optional: Update manually if not using loopfoc() + encoder.update(); // optional: Update manually if not using loopfoc() #endif } \ No newline at end of file diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp index 6b5e8ff..7b3ea0c 100644 --- a/fw/src/udral_servo.hpp +++ b/fw/src/udral_servo.hpp @@ -14,41 +14,38 @@ /// Author: Pavel Kirienko #define NUNAVUT_ASSERT(x) assert(x) +#include "canard.h" #include "esp32-hal.h" - #include "pin_def.h" -#include "canard.h" - // #include "socketcan.h" -#include "esp32twaican.h" -#include "register.hpp" +#include #include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include #include #include -#include "reg/udral/physics/dynamics/rotation/Planar_0_1.h" -#include -#include - +#include +#include #include #include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include "USB.h" +#include "esp32twaican.h" +#include "register.hpp" extern USBCDC usbserial; #define KILO 1000L @@ -58,6 +55,8 @@ extern USBCDC usbserial; /// For CAN FD the queue can be smaller. #define CAN_TX_QUEUE_CAPACITY 100 +const unsigned int hz = 50; + /// We keep the state of the application here. Feel free to use static variables /// instead if desired. @@ -73,10 +72,10 @@ struct UdralServoState { float vcc_volts; float current; - float curr_position; ///< [meter] [radian] - float curr_velocity; ///< [meter/second] [radian/second] - float curr_acceleration; ///< [(meter/second)^2] [(radian/second)^2] - float curr_force; ///< [newton] [netwon*meter] + float curr_position; ///< [meter] [radian] + float curr_velocity; ///< [meter/second] [radian/second] + float curr_acceleration; ///< [(meter/second)^2] [(radian/second)^2] + float curr_force; ///< [newton] [netwon*meter] float motor_temperature; float controller_temperature; @@ -87,16 +86,13 @@ struct UdralServoState { /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl /// As described in the linked documentation, there are two kinds of servos /// supported: linear and rotary. Units per-kind are: LINEAR ROTARY - float target_position; ///< [meter] [radian] - float target_velocity; ///< [meter/second] [radian/second] - float target_acceleration; ///< [(meter/second)^2] [(radian/second)^2] - float target_force; ///< [newton] [netwon*meter] + float target_position; ///< [meter] [radian] + float target_velocity; ///< [meter/second] [radian/second] + float target_acceleration; ///< [(meter/second)^2] [(radian/second)^2] + float target_force; ///< [newton] [netwon*meter] - struct { - uavcan_primitive_scalar_Real32_1_0 p; - uavcan_primitive_scalar_Real32_1_0 i; - uavcan_primitive_scalar_Real32_1_0 d; - } pid; + uavcan_primitive_array_Real32_1_0 pid_position; + uavcan_primitive_array_Real32_1_0 pid_velocity; }; }; @@ -118,15 +114,14 @@ struct UdralServoInternalState { /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl struct { struct { - CanardPortID - servo_feedback; //< reg.udral.service.actuator.common.Feedback - CanardPortID servo_status; //< reg.udral.service.actuator.common.Status - CanardPortID servo_power; //< reg.udral.physics.electricity.PowerTs - CanardPortID servo_dynamics; //< (timestamped dynamics) + CanardPortID servo_feedback; //< reg.udral.service.actuator.common.Feedback + CanardPortID servo_status; //< reg.udral.service.actuator.common.Status + CanardPortID servo_power; //< reg.udral.physics.electricity.PowerTs + CanardPortID servo_dynamics; //< (timestamped dynamics) } pub; struct { - CanardPortID servo_setpoint; //< (non-timestamped dynamics) - CanardPortID servo_readiness; //< reg.udral.service.common.Readiness + CanardPortID servo_setpoint; //< (non-timestamped dynamics) + CanardPortID servo_readiness; //< reg.udral.service.common.Readiness } sub; } port_id; @@ -157,491 +152,421 @@ static volatile bool g_restart_required = false; /// Mind the difference between monotonic time and wall time. Monotonic time never changes rate or makes leaps, /// it is therefore impossible to synchronize with an external reference. Wall time can be synchronized and therefore /// it may change rate or make leap adjustments. The two kinds of time serve completely different purposes. -static CanardMicrosecond getMonotonicMicroseconds() -{ - return micros(); -} +static CanardMicrosecond getMonotonicMicroseconds() { return micros(); } -typedef enum SubjectRole -{ - SUBJECT_ROLE_PUBLISHER, - SUBJECT_ROLE_SUBSCRIBER, +typedef enum SubjectRole { + SUBJECT_ROLE_PUBLISHER, + SUBJECT_ROLE_SUBSCRIBER, } SubjectRole; /// Reads the port-ID from the corresponding standard register. The standard register schema is documented in /// the Cyphal Specification, section for the standard service uavcan.register.Access. You can also find it here: /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/register/384.Access.1.0.dsdl /// A very hands-on demo is available in Python: https://pycyphal.readthedocs.io/en/stable/pages/demo.html -static CanardPortID getSubjectID(const SubjectRole role, const char *const port_name, const char *const type_name) -{ - // Deduce the register name from port name. - const char *const role_name = (role == SUBJECT_ROLE_PUBLISHER) ? "pub" : "sub"; - char register_name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_] = {0}; - snprintf(®ister_name[0], sizeof(register_name), "uavcan.%s.%s.id", role_name, port_name); +static CanardPortID getSubjectID(const SubjectRole role, const char *const port_name, const char *const type_name) { + // Deduce the register name from port name. + const char *const role_name = (role == SUBJECT_ROLE_PUBLISHER) ? "pub" : "sub"; + char register_name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_] = {0}; + snprintf(®ister_name[0], sizeof(register_name), "uavcan.%s.%s.id", role_name, port_name); - // Set up the default value. It will be used to populate the register if it doesn't exist. - uavcan_register_Value_1_0 val = {0}; - uavcan_register_Value_1_0_select_natural16_(&val); - val.natural16.value.count = 1; - val.natural16.value.elements[0] = UINT16_MAX; // This means "undefined", per Specification, which is the default. + // Set up the default value. It will be used to populate the register if it doesn't exist. + uavcan_register_Value_1_0 val = {0}; + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = UINT16_MAX; // This means "undefined", per Specification, which is the default. - // Read the register with defensive self-checks. - registerRead(®ister_name[0], &val); - assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); - const uint16_t result = val.natural16.value.elements[0]; + // Read the register with defensive self-checks. + registerRead(®ister_name[0], &val); + assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + const uint16_t result = val.natural16.value.elements[0]; - // This part is NOT required but recommended by the Specification for enhanced introspection capabilities. It is - // very cheap to implement so all implementations should do so. This register simply contains the name of the - // type exposed at this port. It should be immutable, but it is not strictly required so in this implementation - // we take shortcuts by making it mutable since it's behaviorally simpler in this specific case. - snprintf(®ister_name[0], sizeof(register_name), "uavcan.%s.%s.type", role_name, port_name); - uavcan_register_Value_1_0_select_string_(&val); - val._string.value.count = nunavutChooseMin(strlen(type_name), uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_); - memcpy(&val._string.value.elements[0], type_name, val._string.value.count); - registerWrite(®ister_name[0], &val); // Unconditionally overwrite existing value because it's read-only. + // This part is NOT required but recommended by the Specification for enhanced introspection capabilities. It is + // very cheap to implement so all implementations should do so. This register simply contains the name of the + // type exposed at this port. It should be immutable, but it is not strictly required so in this implementation + // we take shortcuts by making it mutable since it's behaviorally simpler in this specific case. + snprintf(®ister_name[0], sizeof(register_name), "uavcan.%s.%s.type", role_name, port_name); + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = nunavutChooseMin(strlen(type_name), uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_); + memcpy(&val._string.value.elements[0], type_name, val._string.value.count); + registerWrite(®ister_name[0], &val); // Unconditionally overwrite existing value because it's read-only. - return result; + return result; } -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) -{ - 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); - } +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) { + 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); + } } 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); + 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); } /// Invoked at the rate of the fastest loop. -static void handleFastLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) -{ - // Apply control inputs if armed. - if (state->servo.arming.armed) - { - fprintf(stderr, - "\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); - } - else - { - // fprintf(stderr, "\rDISARMED \r"); - } +static void handleFastLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) { + // Apply control inputs if armed. + if (state->servo.arming.armed) { + // char buf[255]; + // snprintf(buf, sizeof(buf), - const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX; - const uint64_t servo_transfer_id = state->next_transfer_id.servo_fast_loop++; + 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); + } else { + // fprintf(stderr, "\rDISARMED \r"); + } - // 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; - // 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); - assert(err >= 0); - if (err >= 0) - { - const CanardTransferMetadata transfer = { - .priority = CanardPriorityHigh, - .transfer_kind = CanardTransferKindMessage, - .port_id = state->port_id.pub.servo_feedback, - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = (CanardTransferID)servo_transfer_id, - }; - send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); - } + const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX; + const uint64_t servo_transfer_id = state->next_transfer_id.servo_fast_loop++; + + // 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; + // 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); + assert(err >= 0); + if (err >= 0) { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityHigh, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_feedback, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); } + } - // Publish dynamics if the subject is enabled and the node is non-anonymous. - if (!anonymous && (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX)) - { - reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg = {0}; - // Our node does not synchronize its clock with the network, so we cannot timestamp our publications: - msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + // Publish dynamics if the subject is enabled and the node is non-anonymous. + if (!anonymous && (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX)) { + reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg = {0}; + // Our node does not synchronize its clock with the network, so we cannot timestamp our publications: + msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; - msg.value.kinematics.angular_position.radian = state->servo.curr_position; - msg.value.kinematics.angular_velocity.radian_per_second = state->servo.curr_velocity; - msg.value.kinematics.angular_acceleration.radian_per_second_per_second = state->servo.curr_acceleration; - msg.value._torque.newton_meter = state->servo.curr_force; - // 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); - if (err >= 0) - { - const CanardTransferMetadata transfer = { - .priority = CanardPriorityHigh, - .transfer_kind = CanardTransferKindMessage, - .port_id = state->port_id.pub.servo_dynamics, - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = (CanardTransferID)servo_transfer_id, - }; - send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); - } + msg.value.kinematics.angular_position.radian = state->servo.curr_position; + msg.value.kinematics.angular_velocity.radian_per_second = state->servo.curr_velocity; + msg.value.kinematics.angular_acceleration.radian_per_second_per_second = state->servo.curr_acceleration; + msg.value._torque.newton_meter = state->servo.curr_force; + // 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); + if (err >= 0) { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityHigh, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_dynamics, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); } + } - // Publish power if the subject is enabled and the node is non-anonymous. - if (!anonymous && (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX)) - { - reg_udral_physics_electricity_PowerTs_0_1 msg = {0}; - // Our node does not synchronize its clock with the network, so we cannot timestamp our publications: - msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; - // TODO populate real values: - msg.value.current.ampere = state->servo.current; - msg.value.voltage.volt = state->servo.vcc_volts; - // Serialize and publish the message: - uint8_t serialized[reg_udral_physics_electricity_PowerTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); - if (err >= 0) - { - const CanardTransferMetadata transfer = { - .priority = CanardPriorityHigh, - .transfer_kind = CanardTransferKindMessage, - .port_id = state->port_id.pub.servo_power, - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = (CanardTransferID)servo_transfer_id, - }; - send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); - } + // Publish power if the subject is enabled and the node is non-anonymous. + if (!anonymous && (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX)) { + reg_udral_physics_electricity_PowerTs_0_1 msg = {0}; + // Our node does not synchronize its clock with the network, so we cannot timestamp our publications: + msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + // TODO populate real values: + msg.value.current.ampere = state->servo.current; + msg.value.voltage.volt = state->servo.vcc_volts; + // Serialize and publish the message: + uint8_t serialized[reg_udral_physics_electricity_PowerTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); + if (err >= 0) { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityHigh, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_power, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); } + } } /// Invoked every second. -static void handle1HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) -{ - const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX; - // Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat. - if (!anonymous) - { - Serial.println("Heartbeat"); - uavcan_node_Heartbeat_1_0 heartbeat = {0}; - heartbeat.uptime = (uint32_t)((now_usec - state->started_at) / MEGA); - heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; - const O1HeapDiagnostics heap_diag = o1heapGetDiagnostics(state->heap); - if (heap_diag.oom_count > 0) - { - heartbeat.health.value = uavcan_node_Health_1_0_CAUTION; - } - else - { - heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; - } - - uint8_t serialized[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - const int8_t err = uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, &serialized[0], &serialized_size); - assert(err >= 0); - if (err >= 0) - { - const CanardTransferMetadata transfer = { - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_heartbeat++), - }; - send(state, - now_usec + MEGA, // Set transmission deadline 1 second, optimal for heartbeat. - &transfer, - serialized_size, - &serialized[0], - now_usec); - } - } - else // If we don't have a node-ID, obtain one by publishing allocation request messages until we get a response. - { - // The Specification says that the allocation request publication interval shall be randomized. - // We implement randomization by calling rand() at fixed intervals and comparing it against some threshold. - // There are other ways to do it, of course. See the docs in the Specification or in the DSDL definition here: - // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/pnp/8165.NodeIDAllocationData.2.0.dsdl - // Note that a high-integrity/safety-certified application is unlikely to be able to rely on this feature. - if (rand() > RAND_MAX / 2) // NOLINT - { - // Note that this will only work over CAN FD. If you need to run PnP over Classic CAN, use message v1.0. - uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; - msg.allocated_node_id.elements[0].value = UINT16_MAX; - msg.allocated_node_id.count = 0; - uint8_t id[128 / 8]; - getUniqueID(id); - memcpy((uint8_t *)&msg.unique_id_hash, id, sizeof(msg.unique_id_hash)); - Serial.print("hash: "); - Serial.println(msg.unique_id_hash, HEX); - uint8_t serialized[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - const int8_t err = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, &serialized[0], &serialized_size); - assert(err >= 0); - if (err >= 0) - { - const CanardTransferMetadata transfer = { - .priority = CanardPrioritySlow, - .transfer_kind = CanardTransferKindMessage, - .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_pnp_allocation++), - }; - send(state, // The response will arrive asynchronously eventually. - now_usec + MEGA, - &transfer, - serialized_size, - &serialized[0], - now_usec); - } - } +static void handle1HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) { + const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX; + // Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat. + if (!anonymous) { + Serial.println("Heartbeat"); + uavcan_node_Heartbeat_1_0 heartbeat = {0}; + heartbeat.uptime = (uint32_t)((now_usec - state->started_at) / MEGA); + heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; + const O1HeapDiagnostics heap_diag = o1heapGetDiagnostics(state->heap); + if (heap_diag.oom_count > 0) { + heartbeat.health.value = uavcan_node_Health_1_0_CAUTION; + } else { + heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; } - const uint64_t servo_transfer_id = state->next_transfer_id.servo_1Hz_loop++; - - if (!anonymous) - { - // Publish the servo status -- this is a low-rate message with low-severity diagnostics. - reg_udral_service_actuator_common_Status_0_1 msg = {0}; - // TODO: POPULATE THE MESSAGE: temperature, errors, etc. - msg.motor_temperature.kelvin = state->servo.motor_temperature; - msg.controller_temperature.kelvin = state->servo.controller_temperature; - uint8_t serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - const int8_t err = - reg_udral_service_actuator_common_Status_0_1_serialize_(&msg, &serialized[0], &serialized_size); - assert(err >= 0); - if (err >= 0) - { - const CanardTransferMetadata transfer = { - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = state->port_id.pub.servo_status, - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = (CanardTransferID)servo_transfer_id, - }; - send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec); - } + uint8_t serialized[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_heartbeat++), + }; + send(state, + now_usec + MEGA, // Set transmission deadline 1 second, optimal for heartbeat. + &transfer, serialized_size, &serialized[0], now_usec); } - - // 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))) + } else // If we don't have a node-ID, obtain one by publishing allocation request messages until we get a response. + { + // The Specification says that the allocation request publication interval shall be randomized. + // We implement randomization by calling rand() at fixed intervals and comparing it against some threshold. + // There are other ways to do it, of course. See the docs in the Specification or in the DSDL definition here: + // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/pnp/8165.NodeIDAllocationData.2.0.dsdl + // Note that a high-integrity/safety-certified application is unlikely to be able to rely on this feature. + if (rand() > RAND_MAX / 2) // NOLINT { - state->servo.arming.armed = false; - puts("Disarmed by timeout "); + // Note that this will only work over CAN FD. If you need to run PnP over Classic CAN, use message v1.0. + uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; + msg.allocated_node_id.elements[0].value = UINT16_MAX; + msg.allocated_node_id.count = 0; + uint8_t id[128 / 8]; + getUniqueID(id); + memcpy((uint8_t *)&msg.unique_id_hash, id, sizeof(msg.unique_id_hash)); + Serial.print("hash: "); + Serial.println(msg.unique_id_hash, HEX); + uint8_t serialized[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) { + const CanardTransferMetadata transfer = { + .priority = CanardPrioritySlow, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_pnp_allocation++), + }; + send(state, // The response will arrive asynchronously eventually. + now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec); + } } + } + + const uint64_t servo_transfer_id = state->next_transfer_id.servo_1Hz_loop++; + + if (!anonymous) { + // Publish the servo status -- this is a low-rate message with low-severity diagnostics. + reg_udral_service_actuator_common_Status_0_1 msg = {0}; + // TODO: POPULATE THE MESSAGE: temperature, errors, etc. + msg.motor_temperature.kelvin = state->servo.motor_temperature; + msg.controller_temperature.kelvin = state->servo.controller_temperature; + uint8_t serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = reg_udral_service_actuator_common_Status_0_1_serialize_(&msg, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_status, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec); + } + } + + // 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))) { + state->servo.arming.armed = false; + puts("Disarmed by timeout "); + } } /// This is needed only for constructing uavcan_node_port_List_0_1. -static void fillSubscriptions(const struct CanardTreeNode *const tree, // NOLINT(misc-no-recursion) - uavcan_node_port_SubjectIDList_0_1 *const obj) -{ - if (NULL != tree) - { - fillSubscriptions(tree->lr[0], obj); - const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree; - assert(crs->port_id <= CANARD_SUBJECT_ID_MAX); - assert(obj->sparse_list.count < uavcan_node_port_SubjectIDList_0_1_sparse_list_ARRAY_CAPACITY_); - obj->sparse_list.elements[obj->sparse_list.count++].value = crs->port_id; - fillSubscriptions(tree->lr[1], obj); - } +static void fillSubscriptions(const struct CanardTreeNode *const tree, // NOLINT(misc-no-recursion) + uavcan_node_port_SubjectIDList_0_1 *const obj) { + if (NULL != tree) { + fillSubscriptions(tree->lr[0], obj); + const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree; + assert(crs->port_id <= CANARD_SUBJECT_ID_MAX); + assert(obj->sparse_list.count < uavcan_node_port_SubjectIDList_0_1_sparse_list_ARRAY_CAPACITY_); + obj->sparse_list.elements[obj->sparse_list.count++].value = crs->port_id; + fillSubscriptions(tree->lr[1], obj); + } } /// This is needed only for constructing uavcan_node_port_List_0_1. -static void fillServers(const struct CanardTreeNode *const tree, // NOLINT(misc-no-recursion) - uavcan_node_port_ServiceIDList_0_1 *const obj) -{ - if (NULL != tree) - { - fillServers(tree->lr[0], obj); - const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree; - assert(crs->port_id <= CANARD_SERVICE_ID_MAX); - (void)nunavutSetBit(&obj->mask_bitpacked_[0], sizeof(obj->mask_bitpacked_), crs->port_id, true); - fillServers(tree->lr[1], obj); - } +static void fillServers(const struct CanardTreeNode *const tree, // NOLINT(misc-no-recursion) + uavcan_node_port_ServiceIDList_0_1 *const obj) { + if (NULL != tree) { + fillServers(tree->lr[0], obj); + const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree; + assert(crs->port_id <= CANARD_SERVICE_ID_MAX); + (void)nunavutSetBit(&obj->mask_bitpacked_[0], sizeof(obj->mask_bitpacked_), crs->port_id, true); + fillServers(tree->lr[1], obj); + } } /// Invoked every 10 seconds. -static void handle01HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) -{ - // Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous. - // The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU. - if (state->canard.node_id <= CANARD_NODE_ID_MAX) +static void handle01HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) { + // Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous. + // The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU. + if (state->canard.node_id <= CANARD_NODE_ID_MAX) { + uavcan_node_port_List_0_1 m = {0}; + uavcan_node_port_List_0_1_initialize_(&m); + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.publishers); + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.subscribers); + + // Indicate which subjects we publish to. Don't forget to keep this updated if you add new publications! { - uavcan_node_port_List_0_1 m = {0}; - uavcan_node_port_List_0_1_initialize_(&m); - uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.publishers); - uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.subscribers); - - // Indicate which subjects we publish to. Don't forget to keep this updated if you add new publications! - { - size_t *const cnt = &m.publishers.sparse_list.count; - m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_; - m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_port_List_0_1_FIXED_PORT_ID_; - if (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX) - { - m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_feedback; - } - if (state->port_id.pub.servo_status <= CANARD_SUBJECT_ID_MAX) - { - m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_status; - } - if (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX) - { - m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_power; - } - if (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX) - { - m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_dynamics; - } - } - - // Indicate which servers and subscribers we implement. - // We could construct the list manually but it's easier and more robust to just query libcanard for that. - fillSubscriptions(state->canard.rx_subscriptions[CanardTransferKindMessage], &m.subscribers); - fillServers(state->canard.rx_subscriptions[CanardTransferKindRequest], &m.servers); - fillServers(state->canard.rx_subscriptions[CanardTransferKindResponse], &m.clients); // For regularity. - - // Serialize and publish the message. Use a smaller buffer if you know that message is always small. - uint8_t serialized[uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; - if (uavcan_node_port_List_0_1_serialize_(&m, &serialized[0], &serialized_size) >= 0) - { - const CanardTransferMetadata transfer = { - .priority = CanardPriorityOptional, // Mind the priority. - .transfer_kind = CanardTransferKindMessage, - .port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_, - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_port_list++), - }; - send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec); - } + size_t *const cnt = &m.publishers.sparse_list.count; + m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_; + m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_port_List_0_1_FIXED_PORT_ID_; + if (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX) { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_feedback; + } + if (state->port_id.pub.servo_status <= CANARD_SUBJECT_ID_MAX) { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_status; + } + if (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX) { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_power; + } + if (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX) { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_dynamics; + } } + + // Indicate which servers and subscribers we implement. + // We could construct the list manually but it's easier and more robust to just query libcanard for that. + fillSubscriptions(state->canard.rx_subscriptions[CanardTransferKindMessage], &m.subscribers); + fillServers(state->canard.rx_subscriptions[CanardTransferKindRequest], &m.servers); + fillServers(state->canard.rx_subscriptions[CanardTransferKindResponse], &m.clients); // For regularity. + + // Serialize and publish the message. Use a smaller buffer if you know that message is always small. + uint8_t serialized[uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; + if (uavcan_node_port_List_0_1_serialize_(&m, &serialized[0], &serialized_size) >= 0) { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityOptional, // Mind the priority. + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_port_list++), + }; + send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec); + } + } } /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl static void processMessageServoSetpoint(struct UdralServoInternalState *const state, - const reg_udral_physics_dynamics_rotation_Planar_0_1 *const msg) -{ - Serial.println("processMessageServoSetpoint"); - state->servo.target_position = msg->kinematics.angular_position.radian; - state->servo.target_velocity = msg->kinematics.angular_velocity.radian_per_second; - state->servo.target_acceleration = msg->kinematics.angular_acceleration.radian_per_second_per_second; - state->servo.target_force = msg->_torque.newton_meter; + const reg_udral_physics_dynamics_rotation_Planar_0_1 *const msg) { + // Serial.println("processMessageServoSetpoint"); + state->servo.target_position = msg->kinematics.angular_position.radian; + state->servo.target_velocity = msg->kinematics.angular_velocity.radian_per_second; + state->servo.target_acceleration = msg->kinematics.angular_acceleration.radian_per_second_per_second; + state->servo.target_force = msg->_torque.newton_meter; } /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/common/Readiness.0.1.dsdl static void processMessageServiceReadiness(struct UdralServoInternalState *const state, const reg_udral_service_common_Readiness_0_1 *const msg, - const CanardMicrosecond monotonic_time) -{ - Serial.println("processMessageServiceReadiness"); - state->servo.arming.armed = msg->value >= reg_udral_service_common_Readiness_0_1_ENGAGED; - state->servo.arming.last_update_at = monotonic_time; + const CanardMicrosecond monotonic_time) { + // Serial.println("processMessageServiceReadiness"); + state->servo.arming.armed = msg->value >= reg_udral_service_common_Readiness_0_1_ENGAGED; + state->servo.arming.last_update_at = monotonic_time; } static void processMessagePlugAndPlayNodeIDAllocation(struct UdralServoInternalState *const state, - const uavcan_pnp_NodeIDAllocationData_1_0 *const msg) -{ - uint8_t uid[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_] = {0}; - getUniqueID(uid); - if ((msg->allocated_node_id.elements[0].value <= CANARD_NODE_ID_MAX) && (msg->allocated_node_id.count > 0) && (memcmp(uid, (uint8_t *)&msg->unique_id_hash, 6) == 0)) - { - Serial.print("Got processMessagePlugAndPlayNodeIDAllocation response "); - Serial.println(msg->allocated_node_id.elements[0].value); - // printf("Got PnP node-ID allocation: %d\n", msg->allocated_node_id.elements[0].value); - state->canard.node_id = (CanardNodeID)msg->allocated_node_id.elements[0].value; - // Store the value into the non-volatile storage. - uavcan_register_Value_1_0 reg = {0}; - uavcan_register_Value_1_0_select_natural16_(®); - reg.natural16.value.elements[0] = msg->allocated_node_id.elements[0].value; - 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_); - } - 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. + const uavcan_pnp_NodeIDAllocationData_1_0 *const msg) { + uint8_t uid[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_] = {0}; + getUniqueID(uid); + if ((msg->allocated_node_id.elements[0].value <= CANARD_NODE_ID_MAX) && (msg->allocated_node_id.count > 0) && + (memcmp(uid, (uint8_t *)&msg->unique_id_hash, 6) == 0)) { + Serial.print("Got processMessagePlugAndPlayNodeIDAllocation response "); + Serial.println(msg->allocated_node_id.elements[0].value); + // printf("Got PnP node-ID allocation: %d\n", msg->allocated_node_id.elements[0].value); + state->canard.node_id = (CanardNodeID)msg->allocated_node_id.elements[0].value; + // Store the value into the non-volatile storage. + uavcan_register_Value_1_0 reg = {0}; + uavcan_register_Value_1_0_select_natural16_(®); + reg.natural16.value.elements[0] = msg->allocated_node_id.elements[0].value; + 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_); + } 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) -{ - uavcan_node_ExecuteCommand_Response_1_1 resp = {0}; - switch (req->command) - { - case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE: - { - Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE"); - char file_name[uavcan_node_ExecuteCommand_Request_1_1_parameter_ARRAY_CAPACITY_ + 1] = {0}; - memcpy(file_name, req->parameter.elements, req->parameter.count); - file_name[req->parameter.count] = '\0'; - // TODO: invoke the bootloader with the specified file name. See https://github.com/Zubax/kocherga/ - // printf("Firmware update request; filename: '%s' \n", &file_name[0]); - resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_STATE; // This is a stub. - break; + 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: { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE"); + char file_name[uavcan_node_ExecuteCommand_Request_1_1_parameter_ARRAY_CAPACITY_ + 1] = {0}; + memcpy(file_name, req->parameter.elements, req->parameter.count); + file_name[req->parameter.count] = '\0'; + // TODO: invoke the bootloader with the specified file name. See https://github.com/Zubax/kocherga/ + // printf("Firmware update request; filename: '%s' \n", &file_name[0]); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_STATE; // This is a stub. + break; } - case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET: - { - Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET"); - registerDoFactoryReset(); - resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; - break; + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET: { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET"); + registerDoFactoryReset(); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; } - case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART: - { - Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART"); - g_restart_required = true; - resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; - break; + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART: { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART"); + g_restart_required = true; + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; } - case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES: - { - Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES"); - // If your registers are not automatically synchronized with the non-volatile storage, use this command - // to commit them to the storage explicitly. Otherwise, it is safe to remove it. - // In this demo, the registers are stored in files, so there is nothing to do. - resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; - break; + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES: { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES"); + // If your registers are not automatically synchronized with the non-volatile storage, use this command + // to commit them to the storage explicitly. Otherwise, it is safe to remove it. + // In this demo, the registers are stored in files, so there is nothing to do. + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; } - // You can add vendor-specific commands here as well. - default: - { - Serial.println("uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND"); - resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND; - break; + // You can add vendor-specific commands here as well. + default: { + Serial.println("uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND"); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND; + break; } - } - return resp; + } + return resp; } /// Performance notice: the register storage may be slow to access depending on its implementation (e.g., if it is @@ -658,518 +583,446 @@ static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand( /// - Document an operational limitation that the register interface should not be accessed while ENGAGED (armed). /// Cyphal networks usually have no service traffic while the vehicle is operational. /// -static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const uavcan_register_Access_Request_1_0 *req) -{ - char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0}; - assert(req->name.name.count < sizeof(name)); - memcpy(&name[0], req->name.name.elements, req->name.name.count); - name[req->name.name.count] = '\0'; +static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const uavcan_register_Access_Request_1_0 *req) { + char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0}; + assert(req->name.name.count < sizeof(name)); + memcpy(&name[0], req->name.name.elements, req->name.name.count); + name[req->name.name.count] = '\0'; - uavcan_register_Access_Response_1_0 resp = {0}; - // Serial.println("processRequestRegisterAccess"); - // Serial.print("name: "); - // Serial.println((char *)&req->name.name); + uavcan_register_Access_Response_1_0 resp = {0}; + // Serial.println("processRequestRegisterAccess"); + // Serial.print("name: "); + // Serial.println((char *)&req->name.name); - // 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. + // 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); + } + } - // 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. - resp._mutable = true; - resp.persistent = true; + // 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); - // Our node does not synchronize its time with the network so we can't populate the timestamp. - resp.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + // 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. + resp._mutable = true; + resp.persistent = true; - return resp; + // Our node does not synchronize its time with the network so we can't populate the timestamp. + resp.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + + return resp; } /// Constructs a response to uavcan.node.GetInfo which contains the basic information about this node. -static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo() -{ +static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo() { + Serial.println("processRequestNodeGetInfo"); + uavcan_node_GetInfo_Response_1_0 resp = {0}; + resp.protocol_version.major = CANARD_CYPHAL_SPECIFICATION_VERSION_MAJOR; + resp.protocol_version.minor = CANARD_CYPHAL_SPECIFICATION_VERSION_MINOR; - Serial.println("processRequestNodeGetInfo"); - uavcan_node_GetInfo_Response_1_0 resp = {0}; - resp.protocol_version.major = CANARD_CYPHAL_SPECIFICATION_VERSION_MAJOR; - resp.protocol_version.minor = CANARD_CYPHAL_SPECIFICATION_VERSION_MINOR; + // The hardware version is not populated in this demo because it runs on no specific hardware. + // An embedded node like a servo would usually determine the version by querying the hardware. - // The hardware version is not populated in this demo because it runs on no specific hardware. - // An embedded node like a servo would usually determine the version by querying the hardware. + resp.software_version.major = VERSION_MAJOR; + resp.software_version.minor = VERSION_MINOR; + resp.software_vcs_revision_id = VCS_REVISION_ID; - resp.software_version.major = VERSION_MAJOR; - resp.software_version.minor = VERSION_MINOR; - resp.software_vcs_revision_id = VCS_REVISION_ID; + getUniqueID(resp.unique_id); - getUniqueID(resp.unique_id); + // The node name is the name of the product like a reversed Internet domain name (or like a Java package). + resp.name.count = strlen(NODE_NAME); + memcpy(&resp.name.elements, NODE_NAME, resp.name.count); - // The node name is the name of the product like a reversed Internet domain name (or like a Java package). - resp.name.count = strlen(NODE_NAME); - memcpy(&resp.name.elements, NODE_NAME, resp.name.count); - - // The software image CRC and the Certificate of Authenticity are optional so not populated in this demo. - return resp; + // The software image CRC and the Certificate of Authenticity are optional so not populated in this demo. + return resp; } 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) - { - processMessageServoSetpoint(state, &msg); - } - } - else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) - { - reg_udral_service_common_Readiness_0_1 msg = {0}; - if (reg_udral_service_common_Readiness_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) - { - processMessageServiceReadiness(state, &msg, transfer->timestamp_usec); - } - } - else if (transfer->metadata.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) - { - uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; - if (uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) - { - processMessagePlugAndPlayNodeIDAllocation(state, &msg); - } - } - else - { - assert(false); // Seems like we have set up a port subscription without a handler -- bad implementation. - } + 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) { + processMessageServoSetpoint(state, &msg); + } + } else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) { + reg_udral_service_common_Readiness_0_1 msg = {0}; + if (reg_udral_service_common_Readiness_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) { + processMessageServiceReadiness(state, &msg, transfer->timestamp_usec); + } + } else if (transfer->metadata.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) { + uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; + if (uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) { + processMessagePlugAndPlayNodeIDAllocation(state, &msg); + } + } else { + assert(false); // Seems like we have set up a port subscription without a handler -- bad implementation. } - else if (transfer->metadata.transfer_kind == CanardTransferKindRequest) - { - if (transfer->metadata.port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) - { - // The request object is empty so we don't bother deserializing it. Just send the response. - const uavcan_node_GetInfo_Response_1_0 resp = processRequestNodeGetInfo(); - uint8_t serialized[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - const int8_t res = uavcan_node_GetInfo_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size); - if (res >= 0) - { - sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); - } - else - { - assert(false); - } + } else if (transfer->metadata.transfer_kind == CanardTransferKindRequest) { + if (transfer->metadata.port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) { + // The request object is empty so we don't bother deserializing it. Just send the response. + const uavcan_node_GetInfo_Response_1_0 resp = processRequestNodeGetInfo(); + uint8_t serialized[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t res = uavcan_node_GetInfo_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size); + if (res >= 0) { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); + } else { + assert(false); + } + } else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) { + uavcan_register_Access_Request_1_0 req = {0}; + size_t size = transfer->payload.size; + if (uavcan_register_Access_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) { + const uavcan_register_Access_Response_1_0 resp = processRequestRegisterAccess(&req); + uint8_t serialized[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + if (uavcan_register_Access_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); } - else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) - { - uavcan_register_Access_Request_1_0 req = {0}; - size_t size = transfer->payload.size; - if (uavcan_register_Access_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) - { - const uavcan_register_Access_Response_1_0 resp = processRequestRegisterAccess(&req); - uint8_t serialized[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - if (uavcan_register_Access_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) - { - sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); - } - } - } - else if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) - { + } + } else if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { + uavcan_register_List_Request_1_0 req = {0}; + size_t size = transfer->payload.size; - uavcan_register_List_Request_1_0 req = {0}; - size_t size = transfer->payload.size; - - Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_"); - if (uavcan_register_List_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) - { - const uavcan_register_List_Response_1_0 resp = {.name = registerGetNameByIndex(req.index)}; - uint8_t serialized[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - if (uavcan_register_List_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) - { - sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); - } - } - Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_ send"); + Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_"); + if (uavcan_register_List_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) { + const uavcan_register_List_Response_1_0 resp = {.name = registerGetNameByIndex(req.index)}; + uint8_t serialized[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + if (uavcan_register_List_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); } - else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_) - { - uavcan_node_ExecuteCommand_Request_1_1 req = {0}; - size_t size = transfer->payload.size; - Serial.println("uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_"); - if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) - { - const uavcan_node_ExecuteCommand_Response_1_1 resp = processRequestExecuteCommand(&req); - uint8_t serialized[uavcan_node_ExecuteCommand_Response_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - size_t serialized_size = sizeof(serialized); - if (uavcan_node_ExecuteCommand_Response_1_1_serialize_(&resp, &serialized[0], &serialized_size) >= 0) - { - sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); - } - } - } - else - { - assert(false); // Seems like we have set up a port subscription without a handler -- bad implementation. + } + Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_ send"); + } else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_) { + uavcan_node_ExecuteCommand_Request_1_1 req = {0}; + size_t size = transfer->payload.size; + Serial.println("uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_"); + if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) { + const uavcan_node_ExecuteCommand_Response_1_1 resp = processRequestExecuteCommand(&req); + uint8_t serialized[uavcan_node_ExecuteCommand_Response_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + if (uavcan_node_ExecuteCommand_Response_1_1_serialize_(&resp, &serialized[0], &serialized_size) >= 0) { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); } + } + } else { + assert(false); // Seems like we have set up a port subscription without a handler -- bad implementation. } - else - { - assert(false); // Bad implementation -- check your subscriptions. - } + } else { + assert(false); // Bad implementation -- check your subscriptions. + } } -static void *canardAllocate(void *const user_reference, const size_t amount) -{ - O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap; - assert(o1heapDoInvariantsHold(heap)); - return o1heapAllocate(heap, amount); +static void *canardAllocate(void *const user_reference, const size_t amount) { + O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap; + assert(o1heapDoInvariantsHold(heap)); + return o1heapAllocate(heap, amount); } -static void canardDeallocate(void *const user_reference, const size_t amount, void *const pointer) -{ - (void)amount; - O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap; - o1heapFree(heap, pointer); +static void canardDeallocate(void *const user_reference, const size_t amount, void *const pointer) { + (void)amount; + O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap; + o1heapFree(heap, pointer); } -int udral_loop(state_sync_f servo_state_sync_f) -{ - srand(micros()); - struct UdralServoInternalState state { - .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 - // - https://github.com/pavel-kirienko/o1heap/blob/master/README.md - // - https://forum.opencyphal.org/t/uavcanv1-libcanard-nunavut-templates-memory-usage-concerns/1118/4 - _Alignas(O1HEAP_ALIGNMENT) static uint8_t heap_arena[1024 * 20] = {0}; - state.heap = o1heapInit(heap_arena, sizeof(heap_arena), NULL, NULL); - assert(NULL != state.heap); - struct CanardMemoryResource canard_memory = { - .user_reference = &state, - .deallocate = canardDeallocate, - .allocate = canardAllocate, - }; +int udral_loop(state_sync_f servo_state_sync_f) { + srand(micros()); + struct UdralServoInternalState state{ + .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 + // - https://github.com/pavel-kirienko/o1heap/blob/master/README.md + // - https://forum.opencyphal.org/t/uavcanv1-libcanard-nunavut-templates-memory-usage-concerns/1118/4 + _Alignas(O1HEAP_ALIGNMENT) static uint8_t heap_arena[1024 * 20] = {0}; + state.heap = o1heapInit(heap_arena, sizeof(heap_arena), NULL, NULL); + assert(NULL != state.heap); + struct CanardMemoryResource canard_memory = { + .user_reference = &state, + .deallocate = canardDeallocate, + .allocate = canardAllocate, + }; - // The libcanard instance requires the allocator for managing protocol states. - state.canard = canardInit(canard_memory); - state.canard.user_reference = &state; // Make the state reachable from the canard instance. + // The libcanard instance requires the allocator for managing protocol states. + state.canard = canardInit(canard_memory); + state.canard.user_reference = &state; // Make the state reachable from the canard instance. - // Restore the node-ID from the corresponding standard register. Default to anonymous. - uavcan_register_Value_1_0 val = {0}; - 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. - 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]; + // Restore the node-ID from the corresponding standard register. Default to anonymous. + uavcan_register_Value_1_0 val = {0}; + 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. + 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]; - // 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. - uavcan_register_Value_1_0_select_string_(&val); - val._string.value.count = 0; - registerRead("uavcan.node.description", &val); // We don't need the value, we just need to ensure it exists. + // 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. + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = 0; + registerRead("uavcan.node.description", &val); // We don't need the value, we just need to ensure it exists. - // The UDRAL cookie is used to mark nodes that are auto-configured by a specific auto-configuration authority. - // We don't use this value, it is managed by remote nodes; our only responsibility is to persist it across reboots. - // This register is entirely optional though; if not provided, the node will have to be configured manually. - uavcan_register_Value_1_0_select_string_(&val); - val._string.value.count = 0; // The value should be empty by default, meaning that the node is not configured. - registerRead("udral.pnp.cookie", &val); + // The UDRAL cookie is used to mark nodes that are auto-configured by a specific auto-configuration authority. + // We don't use this value, it is managed by remote nodes; our only responsibility is to persist it across reboots. + // This register is entirely optional though; if not provided, the node will have to be configured manually. + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = 0; // The value should be empty by default, meaning that the node is not configured. + registerRead("udral.pnp.cookie", &val); - // Announce which UDRAL network services we support by populating appropriate registers. They are supposed to be - // immutable (read-only), but in this simplified demo we don't support that, so we make them mutable (do fix this). - uavcan_register_Value_1_0_select_string_(&val); - strcpy((char *)val._string.value.elements, "servo"); // The prefix in port names like "servo.feedback", etc. - val._string.value.count = strlen((const char *)val._string.value.elements); - registerWrite("reg.udral.service.actuator.servo", &val); + // Announce which UDRAL network services we support by populating appropriate registers. They are supposed to be + // immutable (read-only), but in this simplified demo we don't support that, so we make them mutable (do fix this). + uavcan_register_Value_1_0_select_string_(&val); + strcpy((char *)val._string.value.elements, "servo"); // The prefix in port names like "servo.feedback", etc. + val._string.value.count = strlen((const char *)val._string.value.elements); + registerWrite("reg.udral.service.actuator.servo", &val); - // Configure the transport by reading the appropriate standard registers. - uavcan_register_Value_1_0_select_natural16_(&val); - val.natural16.value.count = 1; - val.natural16.value.elements[0] = CANARD_MTU_CAN_CLASSIC; - registerRead("uavcan.can.mtu", &val); - assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); - // We also need the bitrate configuration register. In this demo we can't really use it but an embedded application - // shall define "uavcan.can.bitrate" of type natural32[2]; the second value is zero/ignored if CAN FD not supported. - const int sock[CAN_REDUNDANCY_FACTOR] = { - // socketcanOpen("vcan0", val.natural16.value.elements[0]) // - esp32twaicanOpen(CAN_TX, CAN_RX, NULL)}; + // PID + state.user_state_sync_f(&state.servo, hz); - for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) - { - state.canard_tx_queues[ifidx] = - canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0], canard_memory); + uavcan_register_Value_1_0_select_real32_(&val); + val.real32 = state.servo.pid_position; + registerRead("reg.pid_position", &val); + if (!uavcan_register_Value_1_0_is_real32_(&val) || val.real32.value.count < state.servo.pid_position.value.count) { + for (int i = 0; i < uavcan_register_Value_1_0_is_real32_(&val) ? val.real32.value.count : 0; i += 1) { + state.servo.pid_position.value.elements[i] = val.real32.value.elements[i]; + } + uavcan_register_Value_1_0_select_real32_(&val); + val.real32 = state.servo.pid_position; + registerWrite("reg.pid_position", &val); + } + state.servo.pid_position = val.real32; + + uavcan_register_Value_1_0_select_real32_(&val); + val.real32 = state.servo.pid_velocity; + registerRead("reg.pid_velocity", &val); + if (!uavcan_register_Value_1_0_is_real32_(&val) || val.real32.value.count < state.servo.pid_velocity.value.count) { + for (int i = 0; i < uavcan_register_Value_1_0_is_real32_(&val) ? val.real32.value.count : 0; i += 1) { + state.servo.pid_velocity.value.elements[i] = val.real32.value.elements[i]; + } + uavcan_register_Value_1_0_select_real32_(&val); + val.real32 = state.servo.pid_velocity; + registerWrite("reg.pid_velocity", &val); + } + state.servo.pid_velocity = val.real32; + + // Configure the transport by reading the appropriate standard registers. + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = CANARD_MTU_CAN_CLASSIC; + registerRead("uavcan.can.mtu", &val); + assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + // We also need the bitrate configuration register. In this demo we can't really use it but an embedded application + // shall define "uavcan.can.bitrate" of type natural32[2]; the second value is zero/ignored if CAN FD not supported. + const int sock[CAN_REDUNDANCY_FACTOR] = {// socketcanOpen("vcan0", val.natural16.value.elements[0]) // + esp32twaicanOpen(CAN_TX, CAN_RX, NULL)}; + + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) { + state.canard_tx_queues[ifidx] = canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0], canard_memory); + } + + // Load the port-IDs from the registers. You can implement hot-reloading at runtime if desired. Specification here: + // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl + // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/README.md + // 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_); + 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_); + 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_); + 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_); + // 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_); + 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_); + + // Set up subject subscriptions and RPC-service servers. + // Message subscriptions: + static const CanardMicrosecond servo_transfer_id_timeout = 100 * KILO; + if (state.canard.node_id > CANARD_NODE_ID_MAX) { + 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); + if (res < 0) { + return -res; + } + } + if (state.port_id.sub.servo_setpoint <= CANARD_SUBJECT_ID_MAX) // Do not subscribe if not configured. + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, CanardTransferKindMessage, state.port_id.sub.servo_setpoint, + reg_udral_physics_dynamics_rotation_Planar_0_1_EXTENT_BYTES_, servo_transfer_id_timeout, &rx); + if (res < 0) { + return -res; + } + } + if (state.port_id.sub.servo_readiness <= CANARD_SUBJECT_ID_MAX) // Do not subscribe if not configured. + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, CanardTransferKindMessage, state.port_id.sub.servo_readiness, + reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, servo_transfer_id_timeout, &rx); + if (res < 0) { + return -res; + } + } + // Service servers: + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, + uavcan_node_GetInfo_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx); + if (res < 0) { + return -res; + } + } + { + 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); + if (res < 0) { + return -res; + } + } + { + 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); + if (res < 0) { + return -res; + } + } + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx); + if (res < 0) { + return -res; + } + } + + // Now the node is initialized and we're ready to roll. + state.started_at = getMonotonicMicroseconds(); + const CanardMicrosecond fast_loop_period = MEGA / hz; + CanardMicrosecond next_fast_iter_at = state.started_at + fast_loop_period; + CanardMicrosecond next_1_hz_iter_at = state.started_at + MEGA; + CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10; + do { + vTaskDelay(1); + // Run a trivial scheduler polling the loops that run the business logic. + CanardMicrosecond now_usec = getMonotonicMicroseconds(); + if (now_usec >= next_fast_iter_at) { + state.user_state_sync_f(&state.servo, hz); + next_fast_iter_at += fast_loop_period; + handleFastLoop(&state, now_usec); + } + if (now_usec >= next_1_hz_iter_at) { + next_1_hz_iter_at += MEGA; + handle1HzLoop(&state, now_usec); + } + if (now_usec >= next_01_hz_iter_at) { + next_01_hz_iter_at += MEGA * 10; + handle01HzLoop(&state, now_usec); } - // Load the port-IDs from the registers. You can implement hot-reloading at runtime if desired. Specification here: - // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl - // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/README.md - // 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_); - 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_); - 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_); - 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_); - // 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_); - 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_); + // Manage CAN RX/TX per redundant interface. + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) { + // Transmit pending frames from the prioritized TX queues managed by libcanard. + struct CanardTxQueue *const que = &state.canard_tx_queues[ifidx]; + struct CanardTxQueueItem *tqi = canardTxPeek(que); // Find the highest-priority frame. + while (tqi != NULL) { + // 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 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); - // Set up subject subscriptions and RPC-service servers. - // Message subscriptions: - static const CanardMicrosecond servo_transfer_id_timeout = 100 * KILO; - if (state.canard.node_id > CANARD_NODE_ID_MAX) - { - 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); - if (res < 0) - { - return -res; + // const int16_t result = 0; + if (result == 0) { + break; // The queue is full, we will try again on the next iteration. + } + if (result < 0) { + return -result; // SocketCAN interface failure (link down?) + } } + + struct CanardTxQueueItem *const mut_tqi = canardTxPop(que, tqi); + canardTxFree(que, &state.canard, mut_tqi); + + tqi = canardTxPeek(que); + } + + // Process received frames by feeding them from SocketCAN to libcanard. + // The order in which we handle the redundant interfaces doesn't matter -- libcanard can accept incoming + // frames from any of the redundant interface in an arbitrary order. + // The internal state machine will sort them out and remove duplicates automatically. + struct CanardFrame frame = {0}; + uint8_t buf[CANARD_MTU_CAN_CLASSIC] = {0}; + const int16_t recieve_result = esp32twaicanPop(&frame, NULL, sizeof(buf), buf, 0, NULL); + + if (recieve_result == 0) // The read operation has timed out with no frames, nothing to do here. + { + break; + } + if (recieve_result < 0) // The read operation has failed. This is not a normal condition. + { + return -recieve_result; + } + // The SocketCAN adapter uses the wall clock for timestamping, but we need monotonic. + // Wall clock can only be used for time synchronization. + const CanardMicrosecond timestamp_usec = getMonotonicMicroseconds(); + struct CanardRxTransfer transfer; + memset(&transfer, 0, sizeof transfer); + 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); + } 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. + } else { + assert(false); // No other error can possibly occur at runtime. + } } - if (state.port_id.sub.servo_setpoint <= CANARD_SUBJECT_ID_MAX) // Do not subscribe if not configured. - { - static struct CanardRxSubscription rx; - const int8_t res = // - canardRxSubscribe(&state.canard, - CanardTransferKindMessage, - state.port_id.sub.servo_setpoint, - reg_udral_physics_dynamics_rotation_Planar_0_1_EXTENT_BYTES_, - servo_transfer_id_timeout, - &rx); - if (res < 0) - { - return -res; - } - } - if (state.port_id.sub.servo_readiness <= CANARD_SUBJECT_ID_MAX) // Do not subscribe if not configured. - { - static struct CanardRxSubscription rx; - const int8_t res = // - canardRxSubscribe(&state.canard, - CanardTransferKindMessage, - state.port_id.sub.servo_readiness, - reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, - servo_transfer_id_timeout, - &rx); - if (res < 0) - { - return -res; - } - } - // Service servers: - { - static struct CanardRxSubscription rx; - const int8_t res = // - canardRxSubscribe(&state.canard, - CanardTransferKindRequest, - uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, - uavcan_node_GetInfo_Request_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &rx); - if (res < 0) - { - return -res; - } - } - { - 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); - if (res < 0) - { - return -res; - } - } - { - 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); - if (res < 0) - { - return -res; - } - } - { - static struct CanardRxSubscription rx; - const int8_t res = // - canardRxSubscribe(&state.canard, - CanardTransferKindRequest, - uavcan_register_List_1_0_FIXED_PORT_ID_, - uavcan_register_List_Request_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &rx); - if (res < 0) - { - return -res; - } - } - - // Now the node is initialized and we're ready to roll. - state.started_at = getMonotonicMicroseconds(); - const unsigned int hz = 50; - const CanardMicrosecond fast_loop_period = MEGA / hz; - CanardMicrosecond next_fast_iter_at = state.started_at + fast_loop_period; - CanardMicrosecond next_1_hz_iter_at = state.started_at + MEGA; - CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10; - do - { - vTaskDelay(1); - // Run a trivial scheduler polling the loops that run the business logic. - CanardMicrosecond now_usec = getMonotonicMicroseconds(); - if (now_usec >= next_fast_iter_at) - { - state.user_state_sync_f(&state.servo, hz); - next_fast_iter_at += fast_loop_period; - handleFastLoop(&state, now_usec); - } - if (now_usec >= next_1_hz_iter_at) - { - next_1_hz_iter_at += MEGA; - handle1HzLoop(&state, now_usec); - } - if (now_usec >= next_01_hz_iter_at) - { - next_01_hz_iter_at += MEGA * 10; - handle01HzLoop(&state, now_usec); - } - - // Manage CAN RX/TX per redundant interface. - for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) - { - - // Transmit pending frames from the prioritized TX queues managed by libcanard. - struct CanardTxQueue *const que = &state.canard_tx_queues[ifidx]; - struct CanardTxQueueItem *tqi = canardTxPeek(que); // Find the highest-priority frame. - while (tqi != NULL) - { - // 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 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 = 0; - if (result == 0) - { - break; // The queue is full, we will try again on the next iteration. - } - if (result < 0) - { - return -result; // SocketCAN interface failure (link down?) - } - } - - struct CanardTxQueueItem *const mut_tqi = canardTxPop(que, tqi); - canardTxFree(que, &state.canard, mut_tqi); - - tqi = canardTxPeek(que); - } - - // Process received frames by feeding them from SocketCAN to libcanard. - // The order in which we handle the redundant interfaces doesn't matter -- libcanard can accept incoming - // frames from any of the redundant interface in an arbitrary order. - // The internal state machine will sort them out and remove duplicates automatically. - struct CanardFrame frame = {0}; - uint8_t buf[CANARD_MTU_CAN_CLASSIC] = {0}; - const int16_t recieve_result = esp32twaicanPop(&frame, NULL, sizeof(buf), buf, 0, NULL); - - if (recieve_result == 0) // The read operation has timed out with no frames, nothing to do here. - { - break; - } - if (recieve_result < 0) // The read operation has failed. This is not a normal condition. - { - return -recieve_result; - } - // The SocketCAN adapter uses the wall clock for timestamping, but we need monotonic. - // Wall clock can only be used for time synchronization. - const CanardMicrosecond timestamp_usec = getMonotonicMicroseconds(); - struct CanardRxTransfer transfer; - memset(&transfer, 0, sizeof transfer); - 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); - } - 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. - } - else - { - assert(false); // No other error can possibly occur at runtime. - } - } - } while (!g_restart_required); - return 0; + } while (!g_restart_required); + return 0; } \ No newline at end of file