diff --git a/modules/uavcan/libcanard b/modules/uavcan/libcanard index 2da1c49b..e6ded1b0 160000 --- a/modules/uavcan/libcanard +++ b/modules/uavcan/libcanard @@ -1 +1 @@ -Subproject commit 2da1c49b50106b2820a4efaa1123ba6d040de357 +Subproject commit e6ded1b05f48c5413bce677dc74c57a2b76ab7bf diff --git a/modules/uavcan/uavcan.c b/modules/uavcan/uavcan.c index 37e1f7f7..fd53edc8 100644 --- a/modules/uavcan/uavcan.c +++ b/modules/uavcan/uavcan.c @@ -157,6 +157,17 @@ static void uavcan_init(uint8_t can_dev_idx) { chSysHalt(NULL); } +void uavcan_forget_nodeid(uint8_t uavcan_idx) { + struct uavcan_instance_s* uavcan_instance; + if (!(uavcan_instance = uavcan_get_instance(uavcan_idx))) { goto fail; } + CanardInstance* canard_instance= &uavcan_instance->canard; + canardForgetLocalNodeID(canard_instance); + return; + +fail: + chSysHalt(NULL); +} + static bool uavcan_iterate_instances(struct uavcan_instance_s** instance_ptr) { if (!instance_ptr) { return false; diff --git a/modules/uavcan/uavcan.h b/modules/uavcan/uavcan.h index 2fde3972..ef271dce 100644 --- a/modules/uavcan/uavcan.h +++ b/modules/uavcan/uavcan.h @@ -37,6 +37,7 @@ uint8_t uavcan_get_num_instances(void); uint8_t uavcan_get_node_id(uint8_t uavcan_idx); void uavcan_set_node_id(uint8_t uavcan_idx, uint8_t node_id); +void uavcan_forget_nodeid(uint8_t uavcan_idx); uint16_t uavcan_get_message_data_type_id(uint8_t uavcan_idx, const struct uavcan_message_descriptor_s* msg_descriptor); diff --git a/modules/uavcan_allocatee/uavcan_allocatee.c b/modules/uavcan_allocatee/uavcan_allocatee.c index f55438af..9a42bf91 100644 --- a/modules/uavcan_allocatee/uavcan_allocatee.c +++ b/modules/uavcan_allocatee/uavcan_allocatee.c @@ -16,6 +16,7 @@ WORKER_THREAD_DECLARE_EXTERN(WT) struct allocatee_instance_s; +static void allocation_init(void); static float getRandomFloat(void); static void allocation_message_handler(size_t msg_size, const void* buf, void* ctx); static void allocation_start_request_timer(struct allocatee_instance_s* instance); @@ -28,15 +29,17 @@ struct allocatee_instance_s { uint32_t unique_id_offset; struct worker_thread_timer_task_s request_transmit_task; struct worker_thread_listener_task_s allocation_listener_task; + struct allocatee_instance_s* next; }; +static struct allocatee_instance_s* allocatee_instance_list_head; RUN_AFTER(UAVCAN_INIT) { - for (uint8_t i=0; iuavcan_idx = i; instance->unique_id_offset = 0; - worker_thread_add_listener_task(&WT, &instance->allocation_listener_task, allocation_topic, allocation_message_handler, instance); - allocation_start_request_timer(instance); + LINKED_LIST_APPEND(struct allocatee_instance_s, allocatee_instance_list_head, instance); + if (uavcan_get_node_id(i) == 0) { + struct pubsub_topic_s* allocation_topic = uavcan_get_message_topic(i, &uavcan_protocol_dynamic_node_id_Allocation_descriptor); + worker_thread_add_listener_task(&WT, &instance->allocation_listener_task, allocation_topic, allocation_message_handler, instance); + allocation_start_request_timer(instance); + } + } +} + +static struct allocatee_instance_s* allocation_get_instance(uint8_t idx) { + struct allocatee_instance_s* instance = allocatee_instance_list_head; + while (instance && idx != 0) { + idx--; + instance = instance->next; + } + return instance; +} + +uint8_t allocation_get_num_instances(void) { + struct allocatee_instance_s* instance = allocatee_instance_list_head; + uint8_t count = 0; + while (instance) { + count++; + instance = instance->next; + } + return count; +} + +void allocation_forget_nodeid(void) { + for (uint8_t allocatee_idx=0; allocatee_idxuavcan_idx); + if (nodeId_uavcan_instance != 0) { + uavcan_forget_nodeid(instance->uavcan_idx); + struct pubsub_topic_s* allocation_topic = uavcan_get_message_topic(instance->uavcan_idx, &uavcan_protocol_dynamic_node_id_Allocation_descriptor); + worker_thread_add_listener_task(&WT, &instance->allocation_listener_task, allocation_topic, allocation_message_handler, instance); + allocation_start_request_timer(instance); + } } } diff --git a/modules/uavcan_allocatee/uavcan_allocatee.h b/modules/uavcan_allocatee/uavcan_allocatee.h new file mode 100644 index 00000000..be0eb124 --- /dev/null +++ b/modules/uavcan_allocatee/uavcan_allocatee.h @@ -0,0 +1,3 @@ +#pragma once + +void allocation_forget_nodeid(void);