From 4862fdc23a57705160f487c5e11dc767156cc8f6 Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Fri, 13 Oct 2023 19:19:49 +0800
Subject: [PATCH] fixed interrupt issue

---
 frtos/car/rtos_car.c | 20 ++++++++--------
 frtos/car/wheel.h    | 57 +++++++++++++++++++++++++++++++-------------
 2 files changed, 51 insertions(+), 26 deletions(-)

diff --git a/frtos/car/rtos_car.c b/frtos/car/rtos_car.c
index c46b6e5..70aa7e9 100644
--- a/frtos/car/rtos_car.c
+++ b/frtos/car/rtos_car.c
@@ -25,17 +25,17 @@
 void
 launch()
 {
-    // isr to detect left wheel slot
-    gpio_set_irq_enabled_with_callback(SPEED_PIN_RIGHT,
-                                       GPIO_IRQ_EDGE_FALL,
-                                       true,
-                                       &right_wheel_sensor_isr);
-
     // isr to detect right wheel slot
-    gpio_set_irq_enabled_with_callback(SPEED_PIN_LEFT,
-                                       GPIO_IRQ_EDGE_FALL,
-                                       true,
-                                       &left_wheel_sensor_isr);
+    gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
+    gpio_add_raw_irq_handler(SPEED_PIN_RIGHT,
+                             h_right_wheel_sensor_isr_handler);
+
+    // isr to detect left wheel slot
+    gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
+    gpio_add_raw_irq_handler(SPEED_PIN_LEFT,
+                             h_left_wheel_sensor_isr_handler);
+
+    irq_set_enabled(IO_IRQ_BANK0, true);
 
     TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
     xTaskCreate(monitor_left_wheel_speed_task,
diff --git a/frtos/car/wheel.h b/frtos/car/wheel.h
index b9932cc..ef50f58 100644
--- a/frtos/car/wheel.h
+++ b/frtos/car/wheel.h
@@ -13,8 +13,8 @@
 #define DIRECTION_LEFT_FORWARD    (1U << DIRECTION_PIN_LEFT_IN4)
 #define DIRECTION_LEFT_BACKWARD   (1U << DIRECTION_PIN_LEFT_IN3)
 
-#define SPEED_PIN_RIGHT 27U // ADC0
-#define SPEED_PIN_LEFT  26U // ADC1
+#define SPEED_PIN_RIGHT 15U
+#define SPEED_PIN_LEFT  16U
 
 #define PWM_CLK_DIV 250.f
 #define PWM_WRAP    5000U
@@ -125,19 +125,31 @@ set_wheel_speed (float speed, uint8_t side)
 }
 
 void
-left_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events)
+h_left_wheel_sensor_isr_handler (void)
 {
-    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-    xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken);
-    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
+    {
+        gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
+
+        printf("left wheel sensor isr\n");
+        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+        xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken);
+        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    }
 }
 
 void
-right_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events)
+h_right_wheel_sensor_isr_handler (void)
 {
-    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-    xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken);
-    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
+    {
+        gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
+
+        printf("right wheel sensor isr\n");
+        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+        xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken);
+        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    }
 }
 
 void
@@ -152,14 +164,22 @@ monitor_left_wheel_speed_task (__unused void *pvParameters)
 
 
             static uint64_t prev_time_left    = 0u;
-            static uint64_t elapsed_time_left = 0u;
+            static uint64_t elapsed_time_left = 1u; // to avoid division by 0
 
             elapsed_time_left = curr_time_left - prev_time_left;
 
-            printf("time elapsed: %llu\n", elapsed_time_left);
-
             prev_time_left = curr_time_left;
 
+            static float speed_left = 0.f;
+            // speed in cm/s; speed = distance / time
+            // distance = circumference / 20
+            // circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
+            // distance = 20.4203522483 cm / 20 = 1.02101761242 cm
+            speed_left = (float)
+                (1.02101761242f / (elapsed_time_left / 1000000.f));
+
+            printf("left speed: %f cm/s\n", speed_left);
+
         }
 
     }
@@ -177,13 +197,18 @@ monitor_right_wheel_speed_task (__unused void *pvParameters)
                             curr_time_right  = time_us_64();
 
             static uint64_t prev_time_right    = 0u;
-            static uint64_t elapsed_time_right = 0u;
+            static uint64_t elapsed_time_right = 1u; // to avoid division by 0
 
             elapsed_time_right = curr_time_right - prev_time_right;
 
-            printf("time elapsed: %llu\n", elapsed_time_right);
-
             prev_time_right = curr_time_right;
+
+            static float speed_right = 0.f;
+
+            speed_right = (float)
+                (1.02101761242f / (elapsed_time_right / 1000000.f));
+
+            printf("right speed: %f cm/s\n", speed_right);
         }
 
     }