396 lines
10 KiB
C
396 lines
10 KiB
C
/**
|
|
* @file magnetometer_direction.h
|
|
* @author Woon Jun Wei
|
|
* @brief This file contains the functions to calculate the direction of
|
|
* the car using the accelerometer and magnetometer data
|
|
*
|
|
* @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 and accelerometer data
|
|
* The roll, pitch and yaw are combined to calculate the direction
|
|
* of the car with a complementary filter and compensating for the
|
|
* temperature.
|
|
*
|
|
* The complementary filter is used to combine the accelerometer
|
|
* and magnetometer data (yaw) to calculate the direction of the car
|
|
*
|
|
* Source:
|
|
* https://www.nxp.com/docs/en/application-note/AN3461.pdf
|
|
* https://ahrs.readthedocs.io/en/latest/filters/complementary.html
|
|
*
|
|
*/
|
|
|
|
#ifndef MAGNETOMETER_DIRECTION_H
|
|
#define MAGNETOMETER_DIRECTION_H
|
|
|
|
#include "magnetometer_init.h"
|
|
#include "map.h"
|
|
|
|
/**
|
|
* @brief Roll Calculation with Accelerometer Data
|
|
* @param acceleration Accelerometer Data
|
|
* @return
|
|
*/
|
|
static inline float
|
|
calculate_roll(int16_t acceleration[3])
|
|
{
|
|
return atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
|
|
}
|
|
|
|
/**
|
|
* @brief Pitch Calculation with Accelerometer Data
|
|
* @param acceleration Accelerometer Data
|
|
* @return
|
|
*/
|
|
static inline float
|
|
calculate_pitch(int16_t acceleration[3])
|
|
{
|
|
return atan2(-acceleration[0],
|
|
sqrt((acceleration[1] * acceleration[1])
|
|
+ (acceleration[2] * acceleration[2])))
|
|
* (180.0 / M_PI);
|
|
}
|
|
|
|
/**
|
|
* @brief Yaw Calculation with Magnetometer Data
|
|
* @param magnetometer Magnetometer Data
|
|
* @return
|
|
*/
|
|
static inline float
|
|
calculate_yaw_magnetometer(int16_t magnetometer[3])
|
|
{
|
|
return atan2(magnetometer[1], magnetometer[0]) * (180.0f / M_PI);
|
|
}
|
|
|
|
/**
|
|
* @brief Complementary Filter for Yaw
|
|
* @param yaw_acc Yaw calculated from Accelerometer Data
|
|
* @param yaw_mag Yaw calculated from Magnetometer Data
|
|
* @return yaw Yaw calculated from Complementary Filter
|
|
*/
|
|
// static inline float
|
|
// calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
|
|
// return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
|
|
// }
|
|
|
|
/**
|
|
* @brief Compensate the magnetometer readings for temperature
|
|
* @param yaw_mag Yaw calculated from Magnetometer Data
|
|
* @param temperature Temperature in degrees Celsius
|
|
* @return Compensated Yaw
|
|
*/
|
|
float
|
|
compensate_magnetometer(float yaw_mag, int16_t temperature) {
|
|
// Calculate temperature difference from the reference temperature
|
|
uint delta_temp = temperature - TEMPERATURE_OFFSET;
|
|
|
|
// Apply temperature compensation to each axis using macros
|
|
float compensated_yaw_mag
|
|
= yaw_mag - ((float)delta_temp * TEMPERATURE_COEFFICIENT_Z);
|
|
|
|
// Apply scale and offset corrections using macros
|
|
compensated_yaw_mag = (compensated_yaw_mag - OFFSET_Z) * SCALE_Z;
|
|
|
|
return compensated_yaw_mag;
|
|
}
|
|
|
|
/**
|
|
* @brief Adjust Yaw to be between 0 and 360 degrees
|
|
* @param yaw Yaw calculated from Complementary Filter
|
|
* @return yaw Yaw adjusted to be between 0 and 360 degrees
|
|
*/
|
|
static inline float
|
|
adjust_yaw(float yaw)
|
|
{
|
|
if (yaw < 0)
|
|
{
|
|
yaw += 360;
|
|
}
|
|
|
|
if (yaw > 360)
|
|
{
|
|
yaw -= 360;
|
|
}
|
|
|
|
return yaw;
|
|
}
|
|
|
|
/**
|
|
* @brief Calculate the Compass Direction (N, NE, E, SE, S, SW, W, NW)
|
|
* 22.5 = 360 / 16, used to calculate the compass direction from
|
|
* the compass direction enum
|
|
* 45.0 = 360 / 8, used to calculate the compass direction from
|
|
* the orientation (0 - 7)
|
|
* @param yaw Yaw calculated
|
|
* @return Compass Direction
|
|
*/
|
|
static inline compass_direction_t
|
|
calculate_compass_direction(float yaw)
|
|
{
|
|
if (yaw >= 337.5 || yaw < 22.5)
|
|
{
|
|
return NORTH;
|
|
}
|
|
else
|
|
{
|
|
if (yaw >= 22.5 && yaw < 67.5)
|
|
{
|
|
return NORTH_EAST;
|
|
}
|
|
else
|
|
{
|
|
if (yaw >= 67.5 && yaw < 112.5)
|
|
{
|
|
return EAST;
|
|
}
|
|
else
|
|
{
|
|
if (yaw >= 112.5 && yaw < 157.5)
|
|
{
|
|
return SOUTH_EAST;
|
|
}
|
|
else
|
|
{
|
|
if (yaw >= 157.5 && yaw < 202.5)
|
|
{
|
|
return SOUTH;
|
|
}
|
|
else
|
|
{
|
|
if (yaw >= 202.5 && yaw < 247.5)
|
|
{
|
|
return SOUTH_WEST;
|
|
}
|
|
else
|
|
{
|
|
if (yaw >= 247.5 && yaw < 292.5)
|
|
{
|
|
return WEST;
|
|
}
|
|
else
|
|
{
|
|
if (yaw >= 292.5 && yaw < 337.5)
|
|
{
|
|
return NORTH_WEST;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Update the Orientation Data
|
|
* @param roll Roll calculated from Accelerometer Data
|
|
* @param pitch Pitch calculated from Accelerometer Data
|
|
* @param yaw Yaw calculated from Complementary Filter
|
|
* @param compass_direction Compass Direction
|
|
*/
|
|
static inline void
|
|
update_orientation_data(float roll,
|
|
float pitch,
|
|
float yaw,
|
|
compass_direction_t compass_direction,
|
|
volatile direction_t *g_direction)
|
|
{
|
|
g_direction->roll = roll;
|
|
g_direction->roll_angle = (roll > 0) ? LEFT : RIGHT;
|
|
g_direction->pitch = pitch;
|
|
g_direction->pitch_angle = (pitch > 0) ? UP : DOWN;
|
|
g_direction->yaw = yaw;
|
|
g_direction->orientation = compass_direction;
|
|
}
|
|
|
|
/**
|
|
* @brief Read the Accelerometer and Magnetometer Data and
|
|
* Calculate the Direction of the Car
|
|
* @param acceleration Accelerometer Data
|
|
* @param magnetometer Magnetometer Data
|
|
*/
|
|
static void
|
|
read_direction(int16_t acceleration[3],
|
|
int16_t magnetometer[3],
|
|
volatile direction_t *g_direction)
|
|
{
|
|
|
|
float roll = calculate_roll(acceleration);
|
|
float pitch = calculate_pitch(acceleration);
|
|
float yaw_mag = calculate_yaw_magnetometer(magnetometer);
|
|
|
|
yaw_mag = adjust_yaw(yaw_mag);
|
|
|
|
compass_direction_t compass_direction
|
|
= calculate_compass_direction(yaw_mag);
|
|
|
|
update_orientation_data(
|
|
roll, pitch, yaw_mag, compass_direction, g_direction);
|
|
}
|
|
|
|
/**
|
|
* FreeRTOS Tasks
|
|
*/
|
|
|
|
/**
|
|
* @brief Task to Monitor the Direction of the Car
|
|
* @param params
|
|
*/
|
|
void
|
|
print_orientation_data(volatile direction_t g_direction)
|
|
{
|
|
// printf("Roll: %f, Pitch: %f, Yaw: %f\n",
|
|
printf("%f %f %f\n", g_direction.roll, g_direction.pitch, g_direction.yaw);
|
|
}
|
|
|
|
void
|
|
print_direction(compass_direction_t direction)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case NORTH:
|
|
printf("North\n");
|
|
break;
|
|
case NORTH_EAST:
|
|
printf("North East\n");
|
|
break;
|
|
case EAST:
|
|
printf("East\n");
|
|
break;
|
|
case SOUTH_EAST:
|
|
printf("South East\n");
|
|
break;
|
|
case SOUTH:
|
|
printf("South\n");
|
|
break;
|
|
case SOUTH_WEST:
|
|
printf("South West\n");
|
|
break;
|
|
case WEST:
|
|
printf("West\n");
|
|
break;
|
|
case NORTH_WEST:
|
|
printf("North West\n");
|
|
break;
|
|
}
|
|
}
|
|
|
|
void
|
|
print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle)
|
|
{
|
|
switch (roll_angle)
|
|
{
|
|
case LEFT:
|
|
printf("Your left wheel is in the air!\n");
|
|
break;
|
|
case RIGHT:
|
|
printf("Your right wheel is in the air!\n");
|
|
break;
|
|
}
|
|
|
|
switch (pitch_angle)
|
|
{
|
|
case UP:
|
|
printf("You're Flying!\n");
|
|
break;
|
|
case DOWN:
|
|
printf("You're Plunging!\n");
|
|
break;
|
|
}
|
|
}
|
|
|
|
void
|
|
updateDirection(volatile direction_t * g_direction)
|
|
{
|
|
int16_t magnetometer[3];
|
|
int16_t accelerometer[3];
|
|
int16_t temperature[1];
|
|
|
|
static int cur_x = 0;
|
|
static int cur_y = 0;
|
|
|
|
read_magnetometer(magnetometer);
|
|
read_accelerometer(accelerometer);
|
|
read_temperature(temperature);
|
|
|
|
read_direction(accelerometer, magnetometer, g_direction);
|
|
|
|
print_orientation_data(*g_direction);
|
|
|
|
// Temperature in degrees Celsius
|
|
// printf("Temperature: %d\n", temperature[0]);
|
|
|
|
// print_orientation_data();
|
|
|
|
// printf("Direction: ");
|
|
|
|
// print_direction(g_direction.orientation);
|
|
|
|
switch (g_direction->orientation)
|
|
{
|
|
case NORTH:
|
|
cur_y++;
|
|
break;
|
|
case EAST:
|
|
cur_x++;
|
|
break;
|
|
case SOUTH:
|
|
cur_y--;
|
|
break;
|
|
case WEST:
|
|
cur_x--;
|
|
break;
|
|
case NORTH_EAST:
|
|
cur_x++;
|
|
cur_y++;
|
|
break;
|
|
case SOUTH_EAST:
|
|
cur_x++;
|
|
cur_y--;
|
|
break;
|
|
case SOUTH_WEST:
|
|
cur_x--;
|
|
cur_y--;
|
|
break;
|
|
case NORTH_WEST:
|
|
cur_x--;
|
|
cur_y++;
|
|
break;
|
|
}
|
|
|
|
// Update the map based on the direction of the car (N, E, S, W)
|
|
// update_map(g_direction.orientation, cur_x, cur_y);
|
|
|
|
// printf("Current Position: (%d, %d)\n", cur_x, cur_y);
|
|
// print_map();
|
|
|
|
// print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle);
|
|
}
|
|
|
|
void
|
|
monitor_direction_task(void *pvParameters)
|
|
{
|
|
volatile direction_t *p_direction = NULL;
|
|
p_direction = (direction_t *) pvParameters;
|
|
|
|
for (;;)
|
|
{
|
|
updateDirection(p_direction);
|
|
vTaskDelay(pdMS_TO_TICKS(DIRECTION_READ_DELAY));
|
|
}
|
|
}
|
|
|
|
void
|
|
magnetometer_tasks_init(car_struct_t *car_struct)
|
|
{
|
|
TaskHandle_t h_direction_task = NULL;
|
|
xTaskCreate(monitor_direction_task,
|
|
"Direction Task",
|
|
configMINIMAL_STACK_SIZE,
|
|
car_struct,
|
|
PRIO,
|
|
&h_direction_task);
|
|
}
|
|
|
|
#endif |