diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c index 9c0fee744d..38d2a6bffa 100644 --- a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c +++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c @@ -58,7 +58,9 @@ struct uavcan_iface_t uavcan1 = { .thread_tx_wa_size = sizeof(uavcan1_tx_wa), .node_id = UAVCAN_CAN1_NODE_ID, .transfer_id = 0, - .initialized = false + .initialized = false, + .cnt_act_raw = 0, + .cnt_act_cmd = 0 }; #endif @@ -84,7 +86,9 @@ struct uavcan_iface_t uavcan2 = { .thread_tx_wa_size = sizeof(uavcan2_tx_wa), .node_id = UAVCAN_CAN2_NODE_ID, .transfer_id = 0, - .initialized = false + .initialized = false, + .cnt_act_raw = 0, + .cnt_act_cmd = 0 }; #endif diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.h b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h index 2920324034..46898c837c 100644 --- a/sw/airborne/arch/chibios/modules/uavcan/uavcan.h +++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h @@ -51,6 +51,9 @@ struct uavcan_iface_t { uint8_t transfer_id; bool initialized; + + uint16_t cnt_act_raw; + uint16_t cnt_act_cmd; }; /** Generic uavcan callback definition */ @@ -79,4 +82,4 @@ void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_eve void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void *payload, uint16_t payload_len); -#endif /* MODULES_UAVCAN_ARCH_H */ +#endif /* MODULES_UAVCAN_ARCH_H */ \ No newline at end of file diff --git a/sw/airborne/modules/actuators/actuators_uavcan.c b/sw/airborne/modules/actuators/actuators_uavcan.c index 5b862399a9..d538de07e4 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan.c +++ b/sw/airborne/modules/actuators/actuators_uavcan.c @@ -454,9 +454,14 @@ void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint offset += 14; } - // Broadcast the raw command message on the interface - RunOnceEvery(ACTUATORS_UAVCAN_RAW_DIV, uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID, - CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8)); + iface->cnt_act_raw++; + if(iface->cnt_act_raw >= ACTUATORS_UAVCAN_RAW_DIV) { + iface->cnt_act_raw = 0; + + // Broadcast the raw command message on the interface + uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID, + CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8); + } } /** @@ -488,7 +493,12 @@ void actuators_uavcan_cmd_commit(struct uavcan_iface_t *iface, int16_t *values, offset += 16; } - // Broadcast the raw command message on the interface - RunOnceEvery(ACTUATORS_UAVCAN_CMD_DIV, uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID, - CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8)); + iface->cnt_act_cmd++; + if(iface->cnt_act_cmd >= ACTUATORS_UAVCAN_CMD_DIV) { + iface->cnt_act_cmd = 0; + + // Broadcast the raw command message on the interface + uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID, + CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8); + } }