diff --git a/.gitmodules b/.gitmodules index c24c8584..89820ffe 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,11 @@ [submodule "libcanard"] path = modules/uavcan/libcanard - url = git@github.com:OpenMotorDrive/libcanard.git + url = git@github.com:matternet/libcanard.git + branch = forget-uavcan-nodeid [submodule "dsdl"] path = dsdl url = git@github.com:UAVCAN/dsdl.git [submodule "ChibiOS"] path = ChibiOS url = git@github.com:OpenMotorDrive/ChibiOS.git + branch = framework diff --git a/dsdl b/dsdl index 103745af..192295c4 160000 --- a/dsdl +++ b/dsdl @@ -1 +1 @@ -Subproject commit 103745afb42f15f83d49fc23530f7fe2c11358b8 +Subproject commit 192295c4f9b67f4a20b0eabf74757b6597415f2b diff --git a/modules/pin_change_publisher/pin_change_publisher.c b/modules/pin_change_publisher/pin_change_publisher.c index c7dd9b29..10ac10b3 100644 --- a/modules/pin_change_publisher/pin_change_publisher.c +++ b/modules/pin_change_publisher/pin_change_publisher.c @@ -20,6 +20,7 @@ WORKER_THREAD_DECLARE_EXTERN(WT) struct pin_change_publisher_topic_s { expchannel_t channel; + bool mask_until_handled; struct pubsub_topic_s* topic; struct pin_change_publisher_topic_s* next; }; @@ -37,11 +38,22 @@ RUN_ON(PUBSUB_TOPIC_INIT) { MEMORYPOOL_DECL(pin_change_publisher_topic_list_pool, sizeof(struct pin_change_publisher_topic_s), chCoreAllocAlignedI); +static bool pin_change_publisher_enable_pin_with_mask_option(uint32_t line, enum pin_change_type_t mode, struct pubsub_topic_s* topic, bool mask_until_handled); static struct pin_change_publisher_topic_s* pin_change_publisher_find_irq_topic(expchannel_t channel); static void pin_change_publisher_common_handler(EXTDriver *extp, expchannel_t channel); static bool pin_change_publisher_get_mode(uint32_t line, enum pin_change_type_t mode, uint32_t* ret); bool pin_change_publisher_enable_pin(uint32_t line, enum pin_change_type_t mode, struct pubsub_topic_s* topic) { + bool mask_until_handled = false; + return pin_change_publisher_enable_pin_with_mask_option(line, mode, topic, mask_until_handled); +} + +bool pin_change_publisher_enable_pin_oneshot(uint32_t line, enum pin_change_type_t mode, struct pubsub_topic_s* topic) { + bool mask_until_handled = true; + return pin_change_publisher_enable_pin_with_mask_option(line, mode, topic, mask_until_handled); +} + +static bool pin_change_publisher_enable_pin_with_mask_option(uint32_t line, enum pin_change_type_t mode, struct pubsub_topic_s* topic, bool mask_until_handled) { chSysLock(); expchannel_t channel = PAL_PAD(line); @@ -65,6 +77,7 @@ bool pin_change_publisher_enable_pin(uint32_t line, enum pin_change_type_t mode, irq_topic->channel = channel; irq_topic->topic = topic; + irq_topic->mask_until_handled = mask_until_handled; LINKED_LIST_APPEND(struct pin_change_publisher_topic_s, irq_topic_list_head, irq_topic); @@ -111,6 +124,9 @@ static void pin_change_publisher_common_handler(EXTDriver *extp, expchannel_t ch if (irq_topic) { chSysLockFromISR(); + if (irq_topic->mask_until_handled) { + extChannelDisableI(extp, channel); + } struct pin_change_msg_s msg = {chVTGetSystemTimeX()}; worker_thread_publisher_task_publish_I(&publisher_task, irq_topic->topic, sizeof(struct pin_change_msg_s), pubsub_copy_writer_func, &msg); chSysUnlockFromISR(); @@ -191,3 +207,9 @@ static bool pin_change_publisher_get_mode(uint32_t line, enum pin_change_type_t return true; } + +void pin_change_publisher_unmask_pin(uint32_t line) { + expchannel_t channel = PAL_PAD(line); + + extChannelEnable(&EXTD1, channel); +} diff --git a/modules/pin_change_publisher/pin_change_publisher.h b/modules/pin_change_publisher/pin_change_publisher.h index c7865ba1..e7037e21 100644 --- a/modules/pin_change_publisher/pin_change_publisher.h +++ b/modules/pin_change_publisher/pin_change_publisher.h @@ -17,4 +17,6 @@ struct pin_change_msg_s { }; bool pin_change_publisher_enable_pin(uint32_t line, enum pin_change_type_t mode, struct pubsub_topic_s* topic); +bool pin_change_publisher_enable_pin_oneshot(uint32_t line, enum pin_change_type_t mode, struct pubsub_topic_s* topic); // Masks the interrupt until a subscriber re-enables it. void pin_change_publisher_disable_pin(uint32_t line); +void pin_change_publisher_unmask_pin(uint32_t line); diff --git a/modules/timer_input_capture_publisher/module.mk b/modules/timer_input_capture_publisher/module.mk new file mode 100644 index 00000000..dc99f8f0 --- /dev/null +++ b/modules/timer_input_capture_publisher/module.mk @@ -0,0 +1 @@ +UDEFS += -DHAL_USE_EXT diff --git a/modules/timer_input_capture_publisher/timer_input_capture_publisher.c b/modules/timer_input_capture_publisher/timer_input_capture_publisher.c new file mode 100644 index 00000000..2e334956 --- /dev/null +++ b/modules/timer_input_capture_publisher/timer_input_capture_publisher.c @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include "timer_input_capture_publisher.h" + +#ifndef TIMER_INPUT_CAPTURE_PUBLISHER_QUEUE_DEPTH +#define TIMER_INPUT_CAPTURE_PUBLISHER_QUEUE_DEPTH 2 +#endif + +#ifndef TIMER_INPUT_CAPTURE_PUBLISHER_WORKER_THREAD +#error Please define TIMER_INPUT_CAPTURE_PUBLISHER_WORKER_THREAD in framework_conf.h. +#endif + +#define WT TIMER_INPUT_CAPTURE_PUBLISHER_WORKER_THREAD +WORKER_THREAD_DECLARE_EXTERN(WT) + + +struct timer_input_capture_publisher_topic_s { + expchannel_t channel; + bool mask_until_handled; + struct pubsub_topic_s* topic; + struct timer_input_capture_publisher_topic_s* next; +}; + +static struct worker_thread_publisher_task_s publisher_task; + +struct timer_input_capture_msg_s timer_input_capture_msg; +static ICUDriver icu_T1; +struct timer_input_capture_publisher_topic_s* irq_topic_T1 = NULL; + +RUN_ON(PUBSUB_TOPIC_INIT) { + worker_thread_add_publisher_task(&WT, &publisher_task, sizeof(struct timer_input_capture_msg_s), TIMER_INPUT_CAPTURE_PUBLISHER_QUEUE_DEPTH); +} + +MEMORYPOOL_DECL(timer_input_capture_publisher_topic_list_pool, sizeof(struct timer_input_capture_publisher_topic_s), chCoreAllocAlignedI); + +static bool timer_input_capture_publisher_enable_T1_with_mask_option(struct pubsub_topic_s* topic, bool mask_until_handled); + +void icuperiodcb(ICUDriver *icup) { +// __asm__("bkpt"); + if (irq_topic_T1 != NULL) { + chSysLockFromISR(); + if (irq_topic_T1->mask_until_handled) { + icu_lld_disable_notifications(&icu_T1); + } + timer_input_capture_msg.period = icuGetPeriodX(icup); + timer_input_capture_msg.width = icuGetWidthX(icup); + timer_input_capture_msg.timestamp = chVTGetSystemTimeX(); + worker_thread_publisher_task_publish_I(&publisher_task, irq_topic_T1->topic, sizeof(struct timer_input_capture_msg_s), pubsub_copy_writer_func, &timer_input_capture_msg); + chSysUnlockFromISR(); + } +} + +bool timer_input_capture_publisher_enable_T1(struct pubsub_topic_s* topic) { + bool mask_until_handled = false; + return timer_input_capture_publisher_enable_T1_with_mask_option(topic, mask_until_handled); +} + +bool timer_input_capture_publisher_enable_T1_oneshot(struct pubsub_topic_s* topic) { + bool mask_until_handled = true; + return timer_input_capture_publisher_enable_T1_with_mask_option(topic, mask_until_handled); +} + +static bool timer_input_capture_publisher_enable_T1_with_mask_option(struct pubsub_topic_s* topic, bool mask_until_handled) { + eventLog_debugEvent("ic_en"); + chSysLock(); +// __asm__("bkpt"); + if (!topic) { + chSysUnlock(); + return false; + } + + irq_topic_T1 = chPoolAllocI(&timer_input_capture_publisher_topic_list_pool); + if (!irq_topic_T1) { + chSysUnlock(); + return false; + } + + irq_topic_T1->channel = 1; + irq_topic_T1->topic = topic; + irq_topic_T1->mask_until_handled = mask_until_handled; + + const ICUConfig icucfg = { + ICU_INPUT_ACTIVE_HIGH, + 100000, /* 1MHz ICU clock frequency. */ + NULL, /* icuwidthcb */ + icuperiodcb, + NULL, + ICU_CHANNEL_1, + 0 + }; + + palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(6)); + __asm__("bkpt"); + icuStart(&ICUD1, &icucfg); +// icuStartCapture(&ICUD1); +// icuEnableNotifications(&ICUD1); + + chSysUnlock(); + eventLog_debugEvent("-ic_en"); + + return true; +} + +void timer_input_capture_publisher_disable_T1(void) { + icuDisableNotifications(&icu_T1); +} + +void timer_input_capture_publisher_unmask_T1(void) { + icuEnableNotifications(&icu_T1); +} diff --git a/modules/timer_input_capture_publisher/timer_input_capture_publisher.h b/modules/timer_input_capture_publisher/timer_input_capture_publisher.h new file mode 100644 index 00000000..b817b739 --- /dev/null +++ b/modules/timer_input_capture_publisher/timer_input_capture_publisher.h @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include "ch.h" +#include "hal.h" +#include +/* Timer TIM1 is the only one speifically designed for monitoring a PWM input. + * The STM32F302R8 and STM32F302K8 do not have TM2 but not TIM3 or TIM4 + * Timers 15, 16, & 17 are available for interval measurements. + * + * PA8 (5Volt tolerant) = TIM1_CH1 / AF6: for UQFN32 it is pin 18. For the LQFP64 it is pin 41. + * On the charger board this is CN9 pin 8 and labeled D7. + * On the charger board the nearest GND is CN7 pin 20 + * On the charger board the nearest +5V is CN7 pin 18 + * + */ + +struct timer_input_capture_msg_s { + systime_t timestamp; + icucnt_t width; + icucnt_t period; +}; + +bool timer_input_capture_publisher_enable_T1(struct pubsub_topic_s* topic); +bool timer_input_capture_publisher_enable_T1_oneshot(struct pubsub_topic_s* topic); // Masks the interrupt until a subscriber re-enables it. +void timer_input_capture_publisher_disable_T1(void); +void timer_input_capture_publisher_unmask_T1(void); 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);