From 3282bc640392bda1a730c5a2c8dc98efb2226ca1 Mon Sep 17 00:00:00 2001 From: xypoon <2837264P@student.gla.ac.uk> Date: Mon, 23 Oct 2023 11:05:16 +0800 Subject: [PATCH 1/2] Add Init Ultrasonic Sensor --- frtos/rtos_car.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 8084001..f7989b3 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -94,6 +94,8 @@ main (void) line_sensor_setup(); + init_ultrasonic(); + initialize_car_state(); // TODO: Could be common functionality, To confirm // during Integration launch(); From 460394720d8d8e5eabbd4c80ada2d689e066339f Mon Sep 17 00:00:00 2001 From: xypoon <2837264P@student.gla.ac.uk> Date: Sat, 28 Oct 2023 14:04:07 +0800 Subject: [PATCH 2/2] Ultrasonic added Callibration --- frtos/ultrasonic_sensor/ultrasonic_sensor.c | 90 ++------------------- frtos/ultrasonic_sensor/ultrasonic_sensor.h | 69 +++++++++++----- 2 files changed, 58 insertions(+), 101 deletions(-) diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.c b/frtos/ultrasonic_sensor/ultrasonic_sensor.c index 1c89e06..c5e7f6f 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.c +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.c @@ -3,89 +3,15 @@ #include "task.h" #include -#define TRIG_PIN 0 -#define ECHO_PIN 1 - -int16_t counter = 0; - -void -init_ultrasonic(void) -{ - // Set up the echo pin - gpio_init(ECHO_PIN); - gpio_set_dir(ECHO_PIN, GPIO_IN); - - // Set up the trigger pin - gpio_init(TRIG_PIN); - gpio_set_dir(TRIG_PIN, GPIO_OUT); -} - -float -KalmanFilter(float U) -{ - static float R = 10; // noise convariance can be 10, higher better smooth - static float H = 1; // Measurement Map scalar - static float Q = 10; // initial estimated convariance - static float P = 0; // initial error covariance - static float U_hat = 0; // initial estimated state - static float K = 0; // initial Kalman gain - - // Predict - // - K = P * H / (H * P * H + R); // Update Kalman gain - U_hat = U_hat + K * (U - H * U_hat); // Update estimated state - - // Update error covariance - // - P = (1 - K * H) * P + Q; - - return U_hat; -} - -void -distance_task(__unused void *params) -{ - while (true) - { - vTaskDelay(1000); - - // Trigger the ultrasonic sensor - gpio_put(TRIG_PIN, 1); - sleep_us(10); // Keep the trigger on for 10 microseconds - gpio_put(TRIG_PIN, 0); - - // Wait for the echo pulse to start - while (gpio_get(ECHO_PIN) == 0) - tight_loop_contents(); - - // Measure the pulse width (time taken for the echo to return) - uint32_t start_time = time_us_32(); - while (gpio_get(ECHO_PIN) == 1) - tight_loop_contents(); - uint32_t end_time = time_us_32(); - - // Calculate the distance (in centimeters) - uint32_t pulse_duration = end_time - start_time; - float distance = pulse_duration * 0.017; // Speed of sound at ~343 m/s - - // printf("Distance: %.2f cm\n", distance); - // printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance)); - printf("%d,%.2f,%.2f\n", counter++, distance, KalmanFilter(distance)); - - // If gonna bang wall - // - if (distance < 5) - { - printf("Collision Imminent!\n"); - // Proc stop - // - } - } -} +#include "ultrasonic_sensor.h" void vLaunch(void) -{ +{ + gpio_set_irq_enabled_with_callback(ECHO_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, echo_handler); + + irq_set_enabled(IO_IRQ_BANK0, true); + TaskHandle_t disttask; xTaskCreate(distance_task, "TestDistThread", @@ -94,8 +20,6 @@ vLaunch(void) 1, &disttask); - // Start the tasks and timer running. - // vTaskStartScheduler(); } @@ -104,7 +28,7 @@ main(void) { stdio_init_all(); init_ultrasonic(); - sleep_ms(3000); + sleep_ms(1000); vLaunch(); return 0; diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.h b/frtos/ultrasonic_sensor/ultrasonic_sensor.h index b61fc76..f5f1899 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.h +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.h @@ -9,40 +9,73 @@ #include "ultrasonic_init.h" -void distance_task(__unused void *params) +volatile uint32_t start_time; +volatile uint32_t end_time; +volatile bool echo_rising = false; + +float +KalmanFilter(float U) +{ + static float R = 10; // noise convariance can be 10, higher better smooth + static float H = 1; // Measurement Map scalar + static float Q = 10; // initial estimated convariance + static float P = 0; // initial error covariance + static float U_hat = 0; // initial estimated state + static float K = 0; // initial Kalman gain + + // Predict + // + K = P * H / (H * P * H + R); // Update Kalman gain + U_hat = U_hat + K * (U - H * U_hat); // Update estimated state + + // Update error covariance + // + P = (1 - K * H) * P + Q; + + return U_hat; +} + +void +echo_handler() +{ + if (gpio_get(ECHO_PIN)) + { + start_time = time_us_32(); + echo_rising = true; + } + else + { + end_time = time_us_32(); + echo_rising = false; + } +} + +void +distance_task(__unused void *params) { while (true) { - vTaskDelay(500); + vTaskDelay(1000); - // Trigger the ultrasonic sensor gpio_put(TRIG_PIN, 1); - sleep_us(10); // Keep the trigger on for 10 microseconds + sleep_us(10); gpio_put(TRIG_PIN, 0); - // Wait for the echo pulse to start - while (gpio_get(ECHO_PIN) == 0) + while (echo_rising) + { tight_loop_contents(); - - // Measure the pulse width (time taken for the echo to return) - uint32_t start_time = time_us_32(); - while (gpio_get(ECHO_PIN) == 1) - tight_loop_contents(); - uint32_t end_time = time_us_32(); + } // Calculate the distance (in centimeters) uint32_t pulse_duration = end_time - start_time; - float distance = pulse_duration * 0.017; // Speed of sound at ~343 m/s + float distance = (pulse_duration * 0.034 / 2) * 1.125; // Speed of sound in cm/us - printf("Distance: %.2f cm\n", distance); + // printf("Distance: %.2f cm\n", distance); + printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance)); - // If gonna bang wall - // if (distance < 5) { printf("Collision Imminent!\n"); - // Proc stop - // } } }