From 5c70e4d1b72b5d86b27da4ce0daa8254f2713685 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Thu, 26 Oct 2023 17:09:25 +0800 Subject: [PATCH] fix(Line sensor): Changed line sensor task --- frtos/line_sensor/line_sensor.h | 54 +++++++++++++++++---- frtos/line_sensor/line_sensor_test.c | 38 +++++++-------- frtos/magnetometer/magnetometer_direction.h | 10 ++-- 3 files changed, 67 insertions(+), 35 deletions(-) diff --git a/frtos/line_sensor/line_sensor.h b/frtos/line_sensor/line_sensor.h index 23c30e0..ff6191d 100644 --- a/frtos/line_sensor/line_sensor.h +++ b/frtos/line_sensor/line_sensor.h @@ -14,14 +14,35 @@ * * @param params */ +//void +//monitor_left_sensor_task(__unused void *params) { +// for (;;) +// { +// if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE) +// { +// if (left_sensor_triggered == pdTRUE) +// { +// printf("left sensor triggered\n"); +// // Get Current State +// state_t state = gpio_get(LEFT_SENSOR_PIN); +// +// xMessageBufferSend(left_sensor_msg_buffer, +// &state, +// sizeof(state_t), +// 0); +// // Reset the flag +// left_sensor_triggered = pdFALSE; +// } +// } +// } +//} + void monitor_left_sensor_task(__unused void *params) { for (;;) { if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE) { - if (left_sensor_triggered == pdTRUE) - { printf("left sensor triggered\n"); // Get Current State state_t state = gpio_get(LEFT_SENSOR_PIN); @@ -30,9 +51,6 @@ monitor_left_sensor_task(__unused void *params) { &state, sizeof(state_t), 0); - // Reset the flag - left_sensor_triggered = pdFALSE; - } } } } @@ -45,12 +63,32 @@ monitor_left_sensor_task(__unused void *params) { * * @param params */ +//void +//monitor_right_sensor_task(void *params) { +// for (;;) { +// if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) { +// // Check the flag or receive the message +// if (right_sensor_triggered == pdTRUE) { +// printf("right sensor triggered\n"); +// // Get Current State +// state_t state = gpio_get(RIGHT_SENSOR_PIN); +// +// xMessageBufferSend(right_sensor_msg_buffer, +// &state, +// sizeof(state_t), +// 0); +// // Reset the flag +// right_sensor_triggered = pdFALSE; +// } +// } +// } +//} + void monitor_right_sensor_task(void *params) { for (;;) { if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) { // Check the flag or receive the message - if (right_sensor_triggered == pdTRUE) { printf("right sensor triggered\n"); // Get Current State state_t state = gpio_get(RIGHT_SENSOR_PIN); @@ -59,14 +97,10 @@ monitor_right_sensor_task(void *params) { &state, sizeof(state_t), 0); - // Reset the flag - right_sensor_triggered = pdFALSE; - } } } } - /** * @brief Monitor the direction and Oritentation of the car * diff --git a/frtos/line_sensor/line_sensor_test.c b/frtos/line_sensor/line_sensor_test.c index 44750da..eff8ddf 100644 --- a/frtos/line_sensor/line_sensor_test.c +++ b/frtos/line_sensor/line_sensor_test.c @@ -10,26 +10,26 @@ void launch() { // isr to detect left line sensor - gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler); - - // isr to detect right line sensor - gpio_set_irq_enabled(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(RIGHT_SENSOR_PIN, h_line_sensor_handler); - - irq_set_enabled(IO_IRQ_BANK0, true); - -// struct repeating_timer g_left_sensor_timer; -// add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, -// h_left_sensor_timer_handler, -// NULL, -// &g_left_sensor_timer); +// gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); +// gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler); // -// struct repeating_timer g_right_sensor_timer; -// add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, -// h_right_sensor_timer_handler, -// NULL, -// &g_right_sensor_timer); +// // isr to detect right line sensor +// gpio_set_irq_enabled(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); +// gpio_add_raw_irq_handler(RIGHT_SENSOR_PIN, h_line_sensor_handler); +// +// irq_set_enabled(IO_IRQ_BANK0, true); + + struct repeating_timer g_left_sensor_timer; + add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, + h_left_sensor_timer_handler, + NULL, + &g_left_sensor_timer); + + struct repeating_timer g_right_sensor_timer; + add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, + h_right_sensor_timer_handler, + NULL, + &g_right_sensor_timer); TaskHandle_t h_monitor_left_sensor_task; xTaskCreate(monitor_left_sensor_task, diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index 441e0cb..c7d7189 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -6,15 +6,13 @@ * * @details The direction of the car is calculated using the roll, pitch and yaw * The roll and pitch are calculated using the accelerometer data - * The yaw is calculated using the magnetometer data + * The yaw is calculated using the magnetometer and accelerometer data * The roll, pitch and yaw are combined to calculate the direction - * of the car - * - * The direction of the car is calculated using the complementary - * filter + * of the car with a complementary filter and compensating for the + * temperature. * * The complementary filter is used to combine the accelerometer - * and magnetometer data to calculate the direction of the car + * and magnetometer data (yaw) to calculate the direction of the car * * Source: * https://www.nxp.com/docs/en/application-note/AN3461.pdf