Pin change publisher oneshot

.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

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);
+}

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);

modules/timer_input_capture_publisher/module.mk

@@ -0,0 +1 @@
+UDEFS += -DHAL_USE_EXT

modules/timer_input_capture_publisher/timer_input_capture_publisher.c

@@ -0,0 +1,112 @@
+#include <common/ctor.h>
+#include <common/helpers.h>
+#include <platform.h>
+#include <modules/worker_thread/worker_thread.h>
+#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);
+}

modules/timer_input_capture_publisher/timer_input_capture_publisher.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "ch.h"
+#include "hal.h"
+#include <modules/pubsub/pubsub.h>
+/* 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);

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;

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);
 

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,28 +29,65 @@ 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; i<uavcan_get_num_instances(); i++) {
-        if (uavcan_get_node_id(i) != 0) {
-            continue;
-        }
+    allocation_init();
+}
 
+static void allocation_init(void) {
+    for (uint8_t i=0; i<uavcan_get_num_instances(); i++) {
         struct allocatee_instance_s* instance = chCoreAlloc(sizeof(struct allocatee_instance_s));
 
         chDbgCheck(instance != NULL);
         if (!instance) {
             continue;
         }
 
-        struct pubsub_topic_s* allocation_topic = uavcan_get_message_topic(i, &uavcan_protocol_dynamic_node_id_Allocation_descriptor);
-
+        memset(instance, 0, sizeof(struct allocatee_instance_s));
         instance->uavcan_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_idx<allocation_get_num_instances(); allocatee_idx++) {
+        struct allocatee_instance_s* instance = allocation_get_instance(allocatee_idx);
+        uint8_t nodeId_uavcan_instance = uavcan_get_node_id(instance->uavcan_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);
+        }
     }
 }
 

modules/uavcan_allocatee/uavcan_allocatee.h

@@ -0,0 +1,3 @@
+#pragma once
+
+void allocation_forget_nodeid(void);