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

Updated Magnetometer Calibration Struct

This commit is contained in:
Devoalda 2023-11-10 18:37:50 +08:00
parent c4908eb6f2
commit fa8e561490
3 changed files with 78 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
frtos/config/magnetometer_config.h
View File

@ -7,10 +7,11 @@
#define DIRECTION_READ_DELAY (200) #define DIRECTION_READ_DELAY (200)
#define NUM_READINGS ( 10 ) // Number of readings to #define NUM_READINGS \
// take before (10) // Number of readings to
// calculating // take before
// direction // calculating
// direction
// #define ALPHA ( 0.1f ) // Low Pass Filter // #define ALPHA ( 0.1f ) // Low Pass Filter
// Coefficient // Coefficient
@ -56,6 +57,12 @@ typedef enum
RIGHT = 3 RIGHT = 3
} angle_t; } angle_t;
typedef struct s_calibration_data
{
int16_t accelerometerBias[3];
int16_t magnetometerBias[3];
} calibration_data_t;
/** /**
* @brief The direction of the car * @brief The direction of the car
* roll = angle of the car (left or right) * roll = angle of the car (left or right)
@ -71,6 +78,7 @@ typedef struct
compass_direction_t orientation; compass_direction_t orientation;
angle_t roll_angle; angle_t roll_angle;
angle_t pitch_angle; angle_t pitch_angle;
calibration_data_t *calibration_data;
} direction_t; } direction_t;
#endif #endif

79
frtos/magnetometer/magnetometer_direction.h
View File

@ -310,54 +310,45 @@ updateDirection(volatile direction_t * g_direction)
static int cur_x = 0; static int cur_x = 0;
static int cur_y = 0; static int cur_y = 0;
read_magnetometer(magnetometer); read_magnetometer(magnetometer, g_direction);
read_accelerometer(accelerometer); read_accelerometer(accelerometer, g_direction);
read_temperature(temperature); read_temperature(temperature);
read_direction(accelerometer, magnetometer, g_direction); read_direction(accelerometer, magnetometer, g_direction);
print_orientation_data(*g_direction); // print_orientation_data(*g_direction);
//
// Temperature in degrees Celsius // switch (g_direction->orientation)
// printf("Temperature: %d\n", temperature[0]); // {
// case NORTH:
// print_orientation_data(); // cur_y++;
// break;
// printf("Direction: "); // case EAST:
// cur_x++;
// print_direction(g_direction.orientation); // break;
// case SOUTH:
switch (g_direction->orientation) // cur_y--;
{ // break;
case NORTH: // case WEST:
cur_y++; // cur_x--;
break; // break;
case EAST: // case NORTH_EAST:
cur_x++; // cur_x++;
break; // cur_y++;
case SOUTH: // break;
cur_y--; // case SOUTH_EAST:
break; // cur_x++;
case WEST: // cur_y--;
cur_x--; // break;
break; // case SOUTH_WEST:
case NORTH_EAST: // cur_x--;
cur_x++; // cur_y--;
cur_y++; // break;
break; // case NORTH_WEST:
case SOUTH_EAST: // cur_x--;
cur_x++; // cur_y++;
cur_y--; // break;
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 the map based on the direction of the car (N, E, S, W)
// update_map(g_direction.orientation, cur_x, cur_y); // update_map(g_direction.orientation, cur_x, cur_y);

72
frtos/magnetometer/magnetometer_init.h
View File

@ -29,24 +29,6 @@
// Semaphores // Semaphores
SemaphoreHandle_t g_direction_sem = NULL; SemaphoreHandle_t g_direction_sem = NULL;
// direction_t g_direction = {
// .roll = 0,
// .pitch = 0,
// .yaw = 0,
// .orientation = NORTH,
// .roll_angle = LEFT,
// .pitch_angle = UP
// };
struct s_calibration_data
{
int16_t accelerometerBias[3];
int16_t magnetometerBias[3];
};
struct s_calibration_data g_calibration_data
= { .accelerometerBias = { 0, 0, 0 }, .magnetometerBias = { 0, 0, 0 } };
/** /**
* @brief Read Data with I2C, given the address and register * @brief Read Data with I2C, given the address and register
* @param addr Address of the device * @param addr Address of the device
@ -72,7 +54,7 @@ read_data(uint8_t addr, uint8_t reg)
* @param accelerometer Accelerometer Data * @param accelerometer Accelerometer Data
*/ */
static inline void static inline void
read_accelerometer(int16_t accelerometer[3]) read_accelerometer(int16_t accelerometer[3], volatile direction_t *p_direction)
{ {
uint8_t buffer[6]; uint8_t buffer[6];
@ -95,9 +77,9 @@ read_accelerometer(int16_t accelerometer[3])
accelerometer[2] = (int16_t)((buffer[5] << 8) | buffer[4]); accelerometer[2] = (int16_t)((buffer[5] << 8) | buffer[4]);
// Apply the calibration data // Apply the calibration data
accelerometer[0] -= g_calibration_data.accelerometerBias[0]; accelerometer[0] -= p_direction->calibration_data->accelerometerBias[0];
accelerometer[1] -= g_calibration_data.accelerometerBias[1]; accelerometer[1] -= p_direction->calibration_data->accelerometerBias[1];
accelerometer[2] -= g_calibration_data.accelerometerBias[2]; accelerometer[2] -= p_direction->calibration_data->accelerometerBias[2];
} }
/** /**
@ -105,7 +87,7 @@ read_accelerometer(int16_t accelerometer[3])
* @param magnetometer Magnetometer Data * @param magnetometer Magnetometer Data
*/ */
static inline void static inline void
read_magnetometer(int16_t magnetometer[3]) read_magnetometer(int16_t magnetometer[3], volatile direction_t *p_direction)
{ {
uint8_t buffer[6]; uint8_t buffer[6];
int32_t xMagFiltered = 0; int32_t xMagFiltered = 0;
@ -133,9 +115,9 @@ read_magnetometer(int16_t magnetometer[3])
magnetometer[2] = zMagFiltered / NUM_READINGS; magnetometer[2] = zMagFiltered / NUM_READINGS;
// Apply the calibration data // Apply the calibration data
magnetometer[0] -= g_calibration_data.magnetometerBias[0]; magnetometer[0] -= p_direction->calibration_data->magnetometerBias[0];
magnetometer[1] -= g_calibration_data.magnetometerBias[1]; magnetometer[1] -= p_direction->calibration_data->magnetometerBias[1];
magnetometer[2] -= g_calibration_data.magnetometerBias[2]; magnetometer[2] -= p_direction->calibration_data->magnetometerBias[2];
} }
/** /**
@ -167,8 +149,13 @@ read_temperature(int16_t temperature[1])
temperature[0] = (int16_t)temperature_celsius; temperature[0] = (int16_t)temperature_celsius;
} }
/**
* @brief Calibrate the Magnetometer Sensor via bias values
*
* @return None
*/
void void
initial_calibration() initial_calibration(direction_t *p_direction)
{ {
int16_t accelerometer[3]; int16_t accelerometer[3];
int16_t magnetometer[3]; int16_t magnetometer[3];
@ -184,8 +171,8 @@ initial_calibration()
{ {
printf("Calibrating... %d\n", i); printf("Calibrating... %d\n", i);
read_accelerometer(accelerometer); read_accelerometer(accelerometer, p_direction);
read_magnetometer(magnetometer); read_magnetometer(magnetometer, p_direction);
for (int j = 0; j < 3; j++) for (int j = 0; j < 3; j++)
{ {
@ -209,29 +196,29 @@ initial_calibration()
sleep_ms(10); sleep_ms(10);
} }
g_calibration_data.accelerometerBias[0] p_direction->calibration_data->accelerometerBias[0]
= (accelerometerMax[0] + accelerometerMin[0]) / 2; = (accelerometerMax[0] + accelerometerMin[0]) / 2;
g_calibration_data.accelerometerBias[1] p_direction->calibration_data->accelerometerBias[1]
= (accelerometerMax[1] + accelerometerMin[1]) / 2; = (accelerometerMax[1] + accelerometerMin[1]) / 2;
g_calibration_data.accelerometerBias[2] p_direction->calibration_data->accelerometerBias[2]
= (accelerometerMax[2] + accelerometerMin[2]) / 2; = (accelerometerMax[2] + accelerometerMin[2]) / 2;
g_calibration_data.magnetometerBias[0] p_direction->calibration_data->magnetometerBias[0]
= (magnetometerMax[0] + magnetometerMin[0]) / 2; = (magnetometerMax[0] + magnetometerMin[0]) / 2;
g_calibration_data.magnetometerBias[1] p_direction->calibration_data->magnetometerBias[1]
= (magnetometerMax[1] + magnetometerMin[1]) / 2; = (magnetometerMax[1] + magnetometerMin[1]) / 2;
g_calibration_data.magnetometerBias[2] p_direction->calibration_data->magnetometerBias[2]
= (magnetometerMax[2] + magnetometerMin[2]) / 2; = (magnetometerMax[2] + magnetometerMin[2]) / 2;
printf("Accelerometer Bias: %d, %d, %d\n", printf("Accelerometer Bias: %d, %d, %d\n",
g_calibration_data.accelerometerBias[0], p_direction->calibration_data->accelerometerBias[0],
g_calibration_data.accelerometerBias[1], p_direction->calibration_data->accelerometerBias[1],
g_calibration_data.accelerometerBias[2]); p_direction->calibration_data->accelerometerBias[2]);
printf("Magnetometer Bias: %d, %d, %d\n", printf("Magnetometer Bias: %d, %d, %d\n",
g_calibration_data.magnetometerBias[0], p_direction->calibration_data->magnetometerBias[0],
g_calibration_data.magnetometerBias[1], p_direction->calibration_data->magnetometerBias[1],
g_calibration_data.magnetometerBias[2]); p_direction->calibration_data->magnetometerBias[2]);
} }
/** /**
@ -298,6 +285,9 @@ magnetometer_init(car_struct_t *p_car_struct)
p_car_struct->p_direction->orientation = NORTH; p_car_struct->p_direction->orientation = NORTH;
p_car_struct->p_direction->roll_angle = LEFT; p_car_struct->p_direction->roll_angle = LEFT;
p_car_struct->p_direction->pitch_angle = UP; p_car_struct->p_direction->pitch_angle = UP;
calibration_data_t g_calibration_data
= { .accelerometerBias = { 0, 0, 0 }, .magnetometerBias = { 0, 0, 0 } };
p_car_struct->p_direction->calibration_data = &g_calibration_data;
printf("Magnetometer Initialising\n"); printf("Magnetometer Initialising\n");
i2c_init(I2C_PORT, 400 * 1000); i2c_init(I2C_PORT, 400 * 1000);