feature(LSM303DLHC Sensor):
Added: - Accelerometer Sensor Data - Magnetometer Sensor Data - Compass Direction
This commit is contained in:
parent
2b34f8c856
commit
32bad31564
|
|
@ -4,6 +4,7 @@ add_subdirectory(motor)
|
|||
add_subdirectory(line_sensor)
|
||||
add_subdirectory(car)
|
||||
add_subdirectory(ultrasonic_sensor)
|
||||
add_subdirectory(magnetometer)
|
||||
|
||||
add_executable(rtos_car rtos_car.c)
|
||||
|
||||
|
|
@ -27,6 +28,7 @@ target_link_libraries(rtos_car
|
|||
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
|
||||
hardware_adc
|
||||
hardware_pwm
|
||||
hardware_i2c
|
||||
)
|
||||
pico_enable_stdio_usb(rtos_car 1)
|
||||
pico_add_extra_outputs(rtos_car)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
#ifndef MAGNETOMETER_CONFIG_H
|
||||
#define MAGNETOMETER_CONFIG_H
|
||||
|
||||
#define I2C_PORT i2c0
|
||||
#define I2C_SDA ( 8 )
|
||||
#define I2C_SCL ( 9 )
|
||||
|
||||
#define MAGNETOMETER_READ_DELAY ( 1000 )
|
||||
#define ACCELEROMETER_READ_DELAY ( 1000 )
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
add_executable(
|
||||
magnetometer_test
|
||||
magnetometer_test.c
|
||||
)
|
||||
|
||||
target_link_libraries(
|
||||
magnetometer_test
|
||||
hardware_i2c
|
||||
pico_stdlib
|
||||
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
|
||||
)
|
||||
|
||||
target_include_directories(
|
||||
magnetometer_test
|
||||
PRIVATE ../config
|
||||
)
|
||||
|
||||
pico_enable_stdio_usb(magnetometer_test 1)
|
||||
pico_add_extra_outputs(magnetometer_test)
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
#ifndef MAGNETOMETER_REGISTER_H
|
||||
#define MAGNETOMETER_REGISTER_H
|
||||
|
||||
// Accelerometer registers
|
||||
#define LSM303_CTRL_REG1_A 0x20
|
||||
#define LSM303_CTRL_REG4_A 0x23
|
||||
#define LSM303_CTRL_REG5_A 0x24
|
||||
#define LSM303_OUT_X_L_A 0x28
|
||||
#define LSM303_OUT_X_H_A 0x29
|
||||
#define LSM303_OUT_Y_L_A 0x2A
|
||||
#define LSM303_OUT_Y_H_A 0x2B
|
||||
#define LSM303_OUT_Z_L_A 0x2C
|
||||
#define LSM303_OUT_Z_H_A 0x2D
|
||||
|
||||
// Magnetometer registers
|
||||
#define LSM303_CRA_REG_M 0x00
|
||||
#define LSM303_CRB_REG_M 0x01
|
||||
#define LSM303_MR_REG_M 0x02
|
||||
#define LSM303_OUT_X_H_M 0x03
|
||||
#define LSM303_OUT_X_L_M 0x04
|
||||
#define LSM303_OUT_Z_H_M 0x05
|
||||
#define LSM303_OUT_Z_L_M 0x06
|
||||
#define LSM303_OUT_Y_H_M 0x07
|
||||
#define LSM303_OUT_Y_L_M 0x08
|
||||
#define LSM303_SR_REG_M 0x09
|
||||
|
||||
#define ACCEL_ADDR 0x19
|
||||
#define MAG_ADDR 0x1E
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
#include "magnetometer_init.h"
|
||||
|
||||
static char
|
||||
*direction(int16_t acceleration[3], int16_t magnetometer[3]) {
|
||||
// Calculate the angle from accelerometer data
|
||||
float roll = atan2(acceleration[1], acceleration[2]) * (180.0 / M_PI);
|
||||
float pitch = atan2(- acceleration[0],
|
||||
sqrt(acceleration[1] * acceleration[1] +
|
||||
acceleration[2] * acceleration[2])) *
|
||||
(180.0 / M_PI);
|
||||
|
||||
// Calculate the heading from magnetometer data
|
||||
float heading = atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
|
||||
|
||||
// Adjust the heading for negative values
|
||||
if (heading < 0)
|
||||
{
|
||||
heading += 360.0;
|
||||
}
|
||||
|
||||
// Determine the direction based on the heading
|
||||
// TODO: Optimize this
|
||||
char *dir;
|
||||
|
||||
if (heading >= 315 || heading < 45)
|
||||
{
|
||||
dir = "North";
|
||||
}
|
||||
else
|
||||
{
|
||||
if (heading >= 45 && heading < 135)
|
||||
{
|
||||
dir = "East";
|
||||
}
|
||||
else
|
||||
{
|
||||
if (heading >= 135 && heading < 225)
|
||||
{
|
||||
dir = "South";
|
||||
}
|
||||
else
|
||||
{
|
||||
dir = "West";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading);
|
||||
|
||||
return dir;
|
||||
}
|
||||
|
||||
/**
|
||||
* FreeRTOS Tasks
|
||||
*/
|
||||
|
||||
void
|
||||
monitor_direction_task(__unused void *params) {
|
||||
for (;;)
|
||||
{
|
||||
if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
|
||||
{
|
||||
// Read from message buffer
|
||||
int16_t magnetometer[3];
|
||||
int16_t accelerometer[3];
|
||||
|
||||
read_magnetometer(magnetometer);
|
||||
read_accelerometer(accelerometer);
|
||||
|
||||
// Calculate the angle from accelerometer data
|
||||
float roll =
|
||||
atan2(accelerometer[1], accelerometer[2]) * (180.0 / M_PI);
|
||||
float pitch = atan2(- accelerometer[0],
|
||||
sqrt(accelerometer[1] * accelerometer[1] +
|
||||
accelerometer[2] * accelerometer[2])) *
|
||||
(180.0 / M_PI);
|
||||
|
||||
// Calculate the heading from magnetometer data
|
||||
float heading =
|
||||
atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI);
|
||||
|
||||
// Adjust the heading for negative values
|
||||
if (heading < 0)
|
||||
{
|
||||
heading += 360.0;
|
||||
}
|
||||
|
||||
// Determine the direction based on the heading
|
||||
char *dir;
|
||||
|
||||
if (heading >= 315 || heading < 45)
|
||||
{
|
||||
dir = "North";
|
||||
}
|
||||
else
|
||||
{
|
||||
if (heading >= 45 && heading < 135)
|
||||
{
|
||||
dir = "East";
|
||||
}
|
||||
else
|
||||
{
|
||||
if (heading >= 135 && heading < 225)
|
||||
{
|
||||
dir = "South";
|
||||
}
|
||||
else
|
||||
{
|
||||
dir = "West";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
printf("Roll: %f, Pitch: %f, Heading: %f\n", roll, pitch, heading);
|
||||
|
||||
printf("Direction: %s\n", dir);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,146 @@
|
|||
/**
|
||||
* @file magnetometer_init.h
|
||||
* @brief Initialise the magnetometer sensor and calculate the direction of the car
|
||||
* @details This file contains the function prototypes for the magnetometer sensor
|
||||
* and the function to calculate the direction of the car
|
||||
* based on the magnetometer sensor data
|
||||
* @author Woon Jun Wei
|
||||
*/
|
||||
|
||||
#ifndef MAGNETOMETER_INIT_H
|
||||
#define MAGNETOMETER_INIT_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/i2c.h"
|
||||
#include "pico/binary_info.h"
|
||||
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include "message_buffer.h"
|
||||
#include "semphr.h"
|
||||
|
||||
#include "magnetometer_config.h"
|
||||
#include "LSM303DLHC_register.h"
|
||||
|
||||
// Semaphores
|
||||
//SemaphoreHandle_t g_magnetometer_sem = NULL;
|
||||
//SemaphoreHandle_t g_accelerometer_sem = NULL;
|
||||
SemaphoreHandle_t g_direction_sem = NULL;
|
||||
|
||||
// Message Buffers
|
||||
//MessageBufferHandle_t g_magnetometer_buffer = NULL;
|
||||
//MessageBufferHandle_t g_accelerometer_buffer = NULL;
|
||||
|
||||
static void
|
||||
lsm303_reset() {
|
||||
/**
|
||||
* Accelerometer Setup
|
||||
*/
|
||||
|
||||
// 0x20 = CTRL_REG1_A
|
||||
// Normal power mode, all axes enabled, 10 Hz
|
||||
uint8_t buf[2] = {LSM303_CTRL_REG1_A, 0x27};
|
||||
i2c_write_blocking(i2c_default, ACCEL_ADDR, buf, 2, false);
|
||||
|
||||
// Reboot memory content (0x40 = CTRL_REG4_A)
|
||||
// Full Scale +-2g, continuous update (0x00 = 0b0000 0000)
|
||||
buf[0] = LSM303_CTRL_REG4_A;
|
||||
buf[1] = 0x00;
|
||||
i2c_write_blocking(i2c_default, ACCEL_ADDR, buf, 2, false);
|
||||
|
||||
/**
|
||||
* Magnetometer Setup
|
||||
*/
|
||||
|
||||
// MR_REG_M (0x02) - Continuous-conversion mode (0x00 -> 00000000)
|
||||
buf[0] = LSM303_MR_REG_M;
|
||||
buf[1] = 0x00;
|
||||
i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
|
||||
|
||||
// CRA_REG_M (0x00), 15 Hz (0x10 -> 00010000)
|
||||
// buf[0] = LSM303_CRA_REG_M;
|
||||
// buf[1] = 0x10;
|
||||
// i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
|
||||
|
||||
// CRB_REG_M (0x01) - Gain = +/- 1.3 (0x20 -> 00100000)
|
||||
buf[0] = LSM303_CRB_REG_M;
|
||||
buf[1] = 0x20;
|
||||
i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
|
||||
|
||||
// CRA_REG_M (0x00), 0x9C = 0b1001 1100
|
||||
buf[0] = LSM303_CRA_REG_M;
|
||||
buf[1] = 0x9C;
|
||||
i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
|
||||
}
|
||||
|
||||
void
|
||||
magnetometer_init()
|
||||
{
|
||||
i2c_init(I2C_PORT, 400 * 1000);
|
||||
|
||||
gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
|
||||
gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
|
||||
gpio_pull_up(I2C_SDA);
|
||||
gpio_pull_up(I2C_SCL);
|
||||
|
||||
lsm303_reset();
|
||||
|
||||
// Semaphore
|
||||
// g_magnetometer_sem = xSemaphoreCreateBinary();
|
||||
// g_accelerometer_sem = xSemaphoreCreateBinary();
|
||||
g_direction_sem = xSemaphoreCreateBinary();
|
||||
|
||||
// Message Buffers
|
||||
// g_magnetometer_buffer = xMessageBufferCreate(sizeof(int16_t) * 3);
|
||||
// g_accelerometer_buffer = xMessageBufferCreate(sizeof(int16_t) * 3);
|
||||
|
||||
printf("Magnetometer initialised\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Timer Interrupt Handler for the magnetometer
|
||||
* @param repeatingTimer The timer handler
|
||||
* @return True (To keep the timer running)
|
||||
*/
|
||||
//bool
|
||||
//h_magnetometer_timer_handler(repeating_timer_t *repeatingTimer) {
|
||||
//
|
||||
// BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
// xSemaphoreGiveFromISR(g_magnetometer_sem,
|
||||
// &xHigherPriorityTaskWoken);
|
||||
// portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
// return true;
|
||||
//}
|
||||
|
||||
/**
|
||||
* @brief Timer Interrupt Handler for the accelerometer
|
||||
* @param repeatingTimer The timer handler
|
||||
* @return True (To keep the timer running)
|
||||
*/
|
||||
//bool
|
||||
//h_accelerometer_timer_handler(repeating_timer_t *repeatingTimer) {
|
||||
//
|
||||
// BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
// xSemaphoreGiveFromISR(g_accelerometer_sem,
|
||||
// &xHigherPriorityTaskWoken);
|
||||
// portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
// return true;
|
||||
//}
|
||||
|
||||
/**
|
||||
* @brief Timer Interrupt Handler To calculate the direction of the car
|
||||
* @param repeatingTimer The timer handler
|
||||
* @return True (To keep the timer running)
|
||||
*/
|
||||
bool
|
||||
h_direction_timer_handler(repeating_timer_t *repeatingTimer) {
|
||||
|
||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
xSemaphoreGiveFromISR(g_direction_sem,
|
||||
&xHigherPriorityTaskWoken);
|
||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
#include "magnetometer_init.h"
|
||||
|
||||
static inline int
|
||||
read_data(uint8_t addr, uint8_t reg) {
|
||||
uint8_t data[1];
|
||||
|
||||
// Send the register address to read from
|
||||
i2c_write_blocking(i2c_default, addr, ®, 1, true);
|
||||
|
||||
// Read the data
|
||||
i2c_read_blocking(i2c_default, addr, data, 1, false);
|
||||
|
||||
return data[0];
|
||||
}
|
||||
|
||||
static inline void
|
||||
read_accelerometer(int16_t accelerometer[3]) {
|
||||
uint8_t buffer[6];
|
||||
|
||||
buffer[0] = read_data(ACCEL_ADDR, LSM303_OUT_X_L_A);
|
||||
buffer[1] = read_data(ACCEL_ADDR, LSM303_OUT_X_H_A);
|
||||
buffer[2] = read_data(ACCEL_ADDR, LSM303_OUT_Y_L_A);
|
||||
buffer[3] = read_data(ACCEL_ADDR, LSM303_OUT_Y_H_A);
|
||||
buffer[4] = read_data(ACCEL_ADDR, LSM303_OUT_Z_L_A);
|
||||
buffer[5] = read_data(ACCEL_ADDR, LSM303_OUT_Z_H_A);
|
||||
|
||||
// Combine high and low bytes
|
||||
|
||||
// xAcceleration
|
||||
accelerometer[0] = (int16_t) ((buffer[1] << 8) | buffer[0]);
|
||||
|
||||
// yAcceleration
|
||||
accelerometer[1] = (int16_t) ((buffer[3] << 8) | buffer[2]);
|
||||
|
||||
// zAcceleration
|
||||
accelerometer[2] = (int16_t) ((buffer[5] << 8) | buffer[4]);
|
||||
|
||||
}
|
||||
|
||||
static inline void
|
||||
read_magnetometer(int16_t magnetometer[3]) {
|
||||
uint8_t buffer[6];
|
||||
|
||||
buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
|
||||
buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M);
|
||||
buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M);
|
||||
buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M);
|
||||
buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M);
|
||||
buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M);
|
||||
|
||||
magnetometer[0] = (int16_t) (buffer[0] << 8 | buffer[1]); //xMag
|
||||
|
||||
magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag
|
||||
|
||||
magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
|
||||
}
|
||||
|
||||
/**
|
||||
* FreeRTOS Tasks
|
||||
*/
|
||||
|
||||
|
||||
//void
|
||||
//monitor_magnetometer_task(__unused void *params) {
|
||||
// for (;;)
|
||||
// {
|
||||
// if (xSemaphoreTake(g_magnetometer_sem, portMAX_DELAY) == pdTRUE)
|
||||
// {
|
||||
//// printf("Magnetometer Task");
|
||||
// int16_t magnetometer[3];
|
||||
// read_magnetometer(magnetometer);
|
||||
//
|
||||
// // Send to message buffer
|
||||
// xMessageBufferSend(g_magnetometer_buffer,
|
||||
// &magnetometer,
|
||||
// sizeof(magnetometer),
|
||||
// 0
|
||||
// );
|
||||
//
|
||||
// printf("Magnetometer: %d, %d, %d\n", magnetometer[0],
|
||||
// magnetometer[1], magnetometer[2]);
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
//void
|
||||
//monitor_accelerometer_task(__unused void *params) {
|
||||
// for (;;)
|
||||
// {
|
||||
// if (xSemaphoreTake(g_accelerometer_sem, portMAX_DELAY) == pdTRUE)
|
||||
// {
|
||||
// int16_t accelerometer[3];
|
||||
// read_accelerometer(accelerometer);
|
||||
//
|
||||
// // Send to message buffer
|
||||
// xMessageBufferSend(g_accelerometer_buffer,
|
||||
// &accelerometer,
|
||||
// sizeof(accelerometer),
|
||||
// 0
|
||||
// );
|
||||
//
|
||||
// printf("Accelerometer: %d, %d, %d\n", accelerometer[0],
|
||||
// accelerometer[1], accelerometer[2]);
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
|
||||
#include "magnetometer_init.h"
|
||||
#include "magnetometer_read.h"
|
||||
#include "magnetometer_direction.h"
|
||||
|
||||
//#define READ_MAGNETOMETER_PRIORITY (tskIDLE_PRIORITY + 2UL)
|
||||
//#define READ_ACCELEROMETER_PRIORITY (tskIDLE_PRIORITY + 3UL)
|
||||
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL)
|
||||
|
||||
void
|
||||
launch()
|
||||
{
|
||||
struct repeating_timer g_direction_timer;
|
||||
add_repeating_timer_ms(1000,
|
||||
h_direction_timer_handler,
|
||||
NULL,
|
||||
&g_direction_timer);
|
||||
|
||||
// struct repeating_timer g_magnetometer_timer;
|
||||
// add_repeating_timer_ms(MAGNETOMETER_READ_DELAY,
|
||||
// h_magnetometer_timer_handler,
|
||||
// NULL,
|
||||
// &g_magnetometer_timer);
|
||||
//
|
||||
// struct repeating_timer g_accelerometer_timer;
|
||||
// add_repeating_timer_ms(ACCELEROMETER_READ_DELAY,
|
||||
// h_accelerometer_timer_handler,
|
||||
// NULL,
|
||||
// &g_accelerometer_timer);
|
||||
|
||||
// TaskHandle_t h_monitor_magnetometer_task = NULL;
|
||||
// xTaskCreate(monitor_magnetometer_task,
|
||||
// "Monitor Magnetometer Task",
|
||||
// configMINIMAL_STACK_SIZE,
|
||||
// NULL,
|
||||
// READ_MAGNETOMETER_PRIORITY,
|
||||
// &h_monitor_magnetometer_task);
|
||||
|
||||
// TaskHandle_t h_monitor_accelerometer_task = NULL;
|
||||
// xTaskCreate(monitor_accelerometer_task,
|
||||
// "Monitor Accelerometer Task",
|
||||
// configMINIMAL_STACK_SIZE,
|
||||
// NULL,
|
||||
// READ_ACCELEROMETER_PRIORITY,
|
||||
// &h_monitor_accelerometer_task);
|
||||
|
||||
TaskHandle_t h_monitor_direction_task = NULL;
|
||||
xTaskCreate(monitor_direction_task,
|
||||
"Monitor Direction Task",
|
||||
configMINIMAL_STACK_SIZE,
|
||||
NULL,
|
||||
DIRECTION_TASK_PRIORITY,
|
||||
&h_monitor_direction_task);
|
||||
|
||||
vTaskStartScheduler();
|
||||
}
|
||||
|
||||
int
|
||||
main (void)
|
||||
{
|
||||
stdio_usb_init();
|
||||
|
||||
sleep_ms(2000);
|
||||
|
||||
printf("Test started!\n");
|
||||
magnetometer_init();
|
||||
|
||||
launch();
|
||||
|
||||
return(0);
|
||||
}
|
||||
Loading…
Reference in New Issue