devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ultrasonic added Callibration

This commit is contained in:
xypoon 2023-10-28 14:04:07 +08:00
parent 67fbfe8a23
commit 460394720d
2 changed files with 58 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
frtos/ultrasonic_sensor/ultrasonic_sensor.c
View File

@ -3,89 +3,15 @@
#include "task.h"
#include <stdio.h>
#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;

69
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -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
//
}
}
}