83 lines
1.8 KiB
C
83 lines
1.8 KiB
C
/**
|
|
* @file ultrasonic_sensor.h
|
|
* @brief Monitor the distance between the car and the wall
|
|
* @author Poon Xiang Yuan
|
|
*/
|
|
|
|
#ifndef ULTRASONIC_SENSOR_H
|
|
#define ULTRASONIC_SENSOR_H
|
|
|
|
#include "ultrasonic_init.h"
|
|
|
|
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(1000);
|
|
|
|
gpio_put(TRIG_PIN, 1);
|
|
sleep_us(10);
|
|
gpio_put(TRIG_PIN, 0);
|
|
|
|
while (echo_rising)
|
|
{
|
|
tight_loop_contents();
|
|
}
|
|
|
|
// Calculate the distance (in centimeters)
|
|
uint32_t pulse_duration = end_time - start_time;
|
|
float distance = (pulse_duration * 0.034 / 2) * 1.125; // Speed of sound in cm/us
|
|
|
|
// printf("Distance: %.2f cm\n", distance);
|
|
printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
|
|
|
|
if (distance < 5)
|
|
{
|
|
printf("Collision Imminent!\n");
|
|
}
|
|
}
|
|
}
|
|
#endif /* ULTRASONIC_SENSOR_H */
|