/// ____ ______ __ __ /// / __ `____ ___ ____ / ____/_ ______ / /_ ____ / / /// / / / / __ `/ _ `/ __ `/ / / / / / __ `/ __ `/ __ `/ / /// / /_/ / /_/ / __/ / / / /___/ /_/ / /_/ / / / / /_/ / / /// `____/ .___/`___/_/ /_/`____/`__, / .___/_/ /_/`__,_/_/ /// /_/ /____/_/ /// /// A demo application showcasing the implementation of a UDRAL servo network service. /// This application is intended to run on GNU/Linux but it is trivially adaptable to baremetal environments. /// Please refer to the enclosed README for details. /// /// This software is distributed under the terms of the MIT License. /// Copyright (C) 2021 OpenCyphal /// Author: Pavel Kirienko #define NUNAVUT_ASSERT(x) assert(x) #include "canard.h" #include "esp32-hal.h" #include "pin_def.h" // #include "socketcan.h" #include #include #include #include #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 #define MEGA ((int64_t)KILO * KILO) #define CAN_REDUNDANCY_FACTOR 1 /// 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. struct UdralServoState { /// Whether the servo is supposed to actuate the load or to stay idle (safe /// low-power mode). struct { struct { bool armed; CanardMicrosecond last_update_at; } arming; 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 motor_temperature; float controller_temperature; }; struct { /// Setpoint & motion profile (unsupported constraints are to be ignored). /// 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] uavcan_primitive_array_Real32_1_0 pid_position; uavcan_primitive_array_Real32_1_0 pid_velocity; }; }; typedef void (*state_sync_f)(UdralServoState *const, const float hz); struct UdralServoInternalState { CanardMicrosecond started_at; O1HeapInstance *heap; struct CanardInstance canard; struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR]; /// The state of the business logic. UdralServoState servo; /// These values are read from the registers at startup. You can also /// implement hot reloading if desired. The subjects of the servo network /// service are defined in the UDRAL data type definitions here: /// 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) } pub; struct { CanardPortID servo_setpoint; //< (non-timestamped dynamics) CanardPortID servo_readiness; //< reg.udral.service.common.Readiness } sub; } port_id; /// A transfer-ID is an integer that is incremented whenever a new message is /// published on a given subject. It is used by the protocol for /// deduplication, message loss detection, and other critical things. For CAN, /// each value can be of type uint8_t, but we use larger types for genericity /// and for statistical purposes, as large values naturally contain the number /// of times each subject was published to. struct { uint64_t uavcan_node_heartbeat; uint64_t uavcan_node_port_list; uint64_t uavcan_pnp_allocation; // Messages published synchronously can share the same transfer-ID: uint64_t servo_fast_loop; uint64_t servo_1Hz_loop; } next_transfer_id; state_sync_f user_state_sync_f; }; /// This flag is raised when the node is requested to restart. static volatile bool g_restart_required = false; /// A deeply embedded system should sample a microsecond-resolution non-overflowing 64-bit timer. /// Here is a simple non-blocking implementation as an example: /// https://github.com/PX4/sapog/blob/601f4580b71c3c4da65cc52237e62a/firmware/src/motor/realtime/motor_timer.c#L233-L274 /// 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(); } 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); // 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]; // 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; } 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); } /// 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) { // char buf[255]; // snprintf(buf, sizeof(buf), Serial.printf("\rp=%.3f m v=%.3f m/s a=%.3f (m/s)^2 F=%.3f N \r", (double)state->servo.target_position, (double)state->servo.target_velocity, (double)state->servo.target_acceleration, (double)state->servo.target_force); } else { // fprintf(stderr, "\rDISARMED \r"); } 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; 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); } } } /// 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); } } } 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); } } /// 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); } } /// 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) { 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); } } } /// 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; } /// 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; } 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. } 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; } 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_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; } } return resp; } /// Performance notice: the register storage may be slow to access depending on its implementation (e.g., if it is /// backed by an uncached filesystem). If your register storage implementation is slow, this may disrupt real-time /// activities of the device. To avoid this, you can employ either measure: /// /// - Load registers to memory at startup, synchronize with the storage at reboot/power-down. /// To implement fast register access you can use https://github.com/pavel-kirienko/cavl. /// See also uavcan.node.ExecuteCommand.COMMAND_STORE_PERSISTENT_STATES. /// /// - If an RTOS is used (not a baremetal system), you can run a separate Cyphal processing task for /// soft-real-time blocking operations (this approach is used in PX4). /// /// - 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'; 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. uavcan_register_Value_1_0_select_empty_(&resp.value); registerRead(&name[0], &resp.value); // Currently, all registers we implement are mutable and persistent. This is an acceptable simplification, // but more advanced implementations will need to differentiate between them to support advanced features like // exposing internal states via registers, perfcounters, etc. resp._mutable = true; resp.persistent = true; // 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() { 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. resp.software_version.major = VERSION_MAJOR; resp.software_version.minor = VERSION_MINOR; resp.software_vcs_revision_id = VCS_REVISION_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 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. } } 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_List_1_0_FIXED_PORT_ID_) { 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"); } 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. } } 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); } 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. // 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 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); // PID state.user_state_sync_f(&state.servo, hz); 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); } // 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; }