Merge pull request #6 from Devoalda/main
This commit is contained in:
Richie Wang
GitHub
commit
1e34a8259f
|
|
@ -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,54 @@
|
|||
#ifndef MAGNETOMETER_CONFIG_H
|
||||
#define MAGNETOMETER_CONFIG_H
|
||||
|
||||
#define I2C_PORT i2c0
|
||||
#define I2C_SDA ( 8 )
|
||||
#define I2C_SCL ( 9 )
|
||||
|
||||
#define DIRECTION_READ_DELAY ( 100 )
|
||||
|
||||
#define ALPHA ( 0.01f ) // Complementary
|
||||
// Filter Constant
|
||||
|
||||
/**
|
||||
* @brief The orientation of the car
|
||||
*/
|
||||
|
||||
typedef enum {
|
||||
NORTH,
|
||||
NORTH_EAST,
|
||||
EAST,
|
||||
SOUTH_EAST,
|
||||
SOUTH,
|
||||
SOUTH_WEST,
|
||||
WEST,
|
||||
NORTH_WEST
|
||||
} compass_direction_t;
|
||||
|
||||
/**
|
||||
* Angle of the car
|
||||
*/
|
||||
typedef enum {
|
||||
UP = 0,
|
||||
DOWN = 1,
|
||||
LEFT = 2,
|
||||
RIGHT = 3
|
||||
} angle_t;
|
||||
|
||||
/**
|
||||
* @brief The direction of the car
|
||||
* roll = angle of the car (left or right)
|
||||
* pitch = angle of the car (up or down)
|
||||
* heading = direction of the car (north, east, south, west) in degrees
|
||||
* orientation = orientation of the car (north, east, south, west)
|
||||
*/
|
||||
typedef struct {
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
compass_direction_t orientation;
|
||||
angle_t roll_angle;
|
||||
angle_t pitch_angle;
|
||||
} direction_t;
|
||||
|
||||
#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,44 @@
|
|||
#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
|
||||
|
||||
// Temperature registers
|
||||
#define LSM303_TEMP_OUT_H_M 0x31
|
||||
#define LSM303_TEMP_OUT_L_M 0x32
|
||||
|
||||
// LSM303DLHC temperature compensation coefficients
|
||||
#define SCALE_Z 0.9 // Scale factor for Z-axis
|
||||
#define OFFSET_Z 5.0 // Offset for Z-axis
|
||||
|
||||
#define TEMPERATURE_OFFSET 25.0 // Reference temperature for calibration
|
||||
#define TEMPERATURE_COEFFICIENT_Z 0.33
|
||||
|
||||
#define OFFSET_Z 5.0
|
||||
#define SCALE_Z 0.9
|
||||
|
||||
#define ACCEL_ADDR 0x19
|
||||
#define MAG_ADDR 0x1E
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,286 @@
|
|||
/**
|
||||
* @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 data
|
||||
* The roll, pitch and yaw are combined to calculate the direction
|
||||
* of the car
|
||||
*
|
||||
* The direction of the car is calculated using the complementary
|
||||
* filter
|
||||
*
|
||||
* The complementary filter is used to combine the accelerometer
|
||||
* and magnetometer data 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"
|
||||
|
||||
/**
|
||||
* @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.0 / 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
|
||||
float delta_temp = (float) (temperature - TEMPERATURE_OFFSET);
|
||||
|
||||
// Apply temperature compensation to each axis using macros
|
||||
float compensated_yaw_mag =
|
||||
yaw_mag - (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) {
|
||||
return (yaw < 0) ? yaw + 360.0f : 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 from Complementary Filter
|
||||
* @return Compass Direction
|
||||
*/
|
||||
static inline compass_direction_t
|
||||
calculate_compass_direction(float yaw) {
|
||||
int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions
|
||||
switch (orientation)
|
||||
{
|
||||
case 0:
|
||||
return NORTH;
|
||||
case 1:
|
||||
return NORTH_EAST;
|
||||
case 2:
|
||||
return EAST;
|
||||
case 3:
|
||||
return SOUTH_EAST;
|
||||
case 4:
|
||||
return SOUTH;
|
||||
case 5:
|
||||
return SOUTH_WEST;
|
||||
case 6:
|
||||
return WEST;
|
||||
case 7:
|
||||
return NORTH_WEST;
|
||||
default:
|
||||
return NORTH;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @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) {
|
||||
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
|
||||
* @details Alpha is set to 0.98 to give more weight to the accelerometer data
|
||||
* @param acceleration Accelerometer Data
|
||||
* @param magnetometer Magnetometer Data
|
||||
*/
|
||||
static void
|
||||
read_direction(int16_t acceleration[3],
|
||||
int16_t magnetometer[3],
|
||||
int16_t temperature[1]) {
|
||||
|
||||
float roll = calculate_roll(acceleration);
|
||||
float pitch = calculate_pitch(acceleration);
|
||||
float yaw_mag = calculate_yaw_magnetometer(magnetometer);
|
||||
|
||||
// Apply temperature compensation to the magnetometer data
|
||||
float compensated_mag_yaw = compensate_magnetometer(yaw_mag,
|
||||
temperature[0]);
|
||||
|
||||
float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI);
|
||||
float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw);
|
||||
|
||||
yaw = adjust_yaw(yaw);
|
||||
|
||||
compass_direction_t compass_direction = calculate_compass_direction(yaw);
|
||||
|
||||
update_orientation_data(roll, pitch, yaw, compass_direction);
|
||||
}
|
||||
|
||||
/**
|
||||
* FreeRTOS Tasks
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Task to Monitor the Direction of the Car
|
||||
* @param params
|
||||
*/
|
||||
void print_orientation_data() {
|
||||
printf("Roll: %f, Pitch: %f, Yaw: %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 monitor_direction_task(__unused void *params) {
|
||||
for (;;)
|
||||
{
|
||||
if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
|
||||
{
|
||||
int16_t magnetometer[3];
|
||||
int16_t accelerometer[3];
|
||||
int16_t temperature[1];
|
||||
|
||||
read_magnetometer(magnetometer);
|
||||
read_accelerometer(accelerometer);
|
||||
read_temperature(temperature);
|
||||
|
||||
read_direction(accelerometer, magnetometer, temperature);
|
||||
|
||||
// Temperature in degrees Celsius
|
||||
printf("Temperature: %d\n", temperature[0]);
|
||||
|
||||
print_orientation_data();
|
||||
|
||||
printf("Direction: ");
|
||||
|
||||
print_direction(g_direction.orientation);
|
||||
|
||||
print_roll_and_pitch(g_direction.roll_angle,
|
||||
g_direction.pitch_angle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
/**
|
||||
* @file magnetometer_init.h
|
||||
* @author Woon Jun Wei
|
||||
* @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
|
||||
*/
|
||||
|
||||
#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_direction_sem = NULL;
|
||||
|
||||
direction_t g_direction = {
|
||||
.roll = 0,
|
||||
.pitch = 0,
|
||||
// .heading = 0,
|
||||
.yaw = 0,
|
||||
.orientation = NORTH,
|
||||
.roll_angle = LEFT,
|
||||
.pitch_angle = UP
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Initialise the LSM303DLHC sensor (Accelerometer and Magnetometer)
|
||||
* @details
|
||||
* Accelerometer - Normal power mode, all axes enabled, 10 Hz,
|
||||
* Full Scale +-2g, continuous update
|
||||
*
|
||||
* Magnetometer - Continuous-conversion mode, Gain = +/- 1.3,
|
||||
* Enable temperature sensor, 220 Hz
|
||||
*
|
||||
* @return None
|
||||
*/
|
||||
static void
|
||||
LSM303DLHC_init() {
|
||||
/**
|
||||
* 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);
|
||||
|
||||
// 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
|
||||
// Enable temperature sensor (0x80 -> 1000 0000)
|
||||
// 220 Hz (0x1C -> 0001 1100)
|
||||
buf[0] = LSM303_CRA_REG_M;
|
||||
buf[1] = 0x9C;
|
||||
i2c_write_blocking(i2c_default, MAG_ADDR, buf, 2, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialise the Magnetometer Sensor
|
||||
* @details Initialise the I2C Port, SDA and SCL Pins, and the LSM303DLHC Sensor
|
||||
*/
|
||||
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);
|
||||
|
||||
LSM303DLHC_init();
|
||||
|
||||
// Semaphore
|
||||
g_direction_sem = xSemaphoreCreateBinary();
|
||||
}
|
||||
|
||||
/**
|
||||
* @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,110 @@
|
|||
/**
|
||||
* @file magnetometer_read.h
|
||||
* @author Woon Jun Wei
|
||||
* @brief This file contains the functions to read the data
|
||||
* from the LSM303DLHC accelerometer and magnetometer sensor
|
||||
*/
|
||||
|
||||
#ifndef MAGNETOMETER_READ_H
|
||||
#define MAGNETOMETER_READ_H
|
||||
|
||||
#include "magnetometer_init.h"
|
||||
|
||||
/**
|
||||
* @brief Read Data with I2C, given the address and register
|
||||
* @param addr Address of the device
|
||||
* @param reg Register to read from
|
||||
* @return 1 piece of data read from the register
|
||||
*/
|
||||
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];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read Accelerometer Data
|
||||
* @param accelerometer Accelerometer Data
|
||||
*/
|
||||
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]);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read Magnetometer Data
|
||||
* @param magnetometer Magnetometer Data
|
||||
*/
|
||||
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
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read Temperature Data in Degrees Celsius
|
||||
* @param temperature Temperature Data in Degrees Celsius
|
||||
*/
|
||||
static inline void
|
||||
read_temperature(int16_t temperature[1]) {
|
||||
uint8_t buffer[2];
|
||||
|
||||
buffer[0] = read_data(MAG_ADDR, LSM303_TEMP_OUT_H_M);
|
||||
buffer[1] = read_data(MAG_ADDR, LSM303_TEMP_OUT_L_M);
|
||||
|
||||
/**
|
||||
* Normalize temperature; it is big-endian, fixed-point
|
||||
* 9 bits signed integer, 3 bits fractional part, 4 bits zeros
|
||||
* and is relative to 20 degrees Celsius
|
||||
* Source: https://electronics.stackexchange.com/a/356964
|
||||
*/
|
||||
|
||||
int16_t raw_temperature =
|
||||
(20 << 3) + (((int16_t) buffer[0] << 8 | buffer[1]) >> 4);
|
||||
|
||||
// Convert the raw temperature data to degrees Celsius
|
||||
float temperature_celsius = (float) raw_temperature / 8.0;
|
||||
|
||||
// Store the result in the temperature array
|
||||
temperature[0] = (int16_t) temperature_celsius;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
|
||||
#include "magnetometer_init.h"
|
||||
#include "magnetometer_read.h"
|
||||
#include "magnetometer_direction.h"
|
||||
|
||||
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL)
|
||||
|
||||
void
|
||||
launch()
|
||||
{
|
||||
struct repeating_timer g_direction_timer;
|
||||
add_repeating_timer_ms(DIRECTION_READ_DELAY,
|
||||
h_direction_timer_handler,
|
||||
NULL,
|
||||
&g_direction_timer);
|
||||
|
||||
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