I am building an IMU as a school project. This required integrating the Fusion(Madgwick) library to get roll, pitch and yaw from acceleration and gyroscope values from MPU6050. These values are first saved in an array, then transferred to the sd card using no-OS-FatFS-SD-SDIO-SPI-RPi-Pico library.
The .c code
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include "hardware/spi.h"
#include "hardware/timer.h"
#include "hardware/sync.h"
#include "no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src/ff15/source/ff.h"
#include "no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src/sd_driver/sd_card.h"
#include "no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src/ff15/source/diskio.h"
#include "Fusion/Fusion/Fusion.h"
#include "Fusion/Fusion/FusionAhrs.h"
// **I2C and SPI Configuration**
#define I2C_PORT i2c0
#define SDA_PIN 20
#define SCL_PIN 21
#define SPI_PORT spi1
#define SD_MISO 16
#define SD_MOSI 15
#define SD_SCK 14
#define SD_CS 13
#define MPU6050_ADDR 0x68
#define BMP180_ADDR 0x77
#define QMC5883L_ADDR 0x0D
// **Fusion AHRS Instance**
FusionAhrs ahrs;
// **FatFs SD Card Variables**
FATFS fs;
FIL file;
FRESULT fr;
// **Scaling Factors**
float accel_scale,gyro_scale;
// **Data Storage Structure**
typedef struct {
uint32_t timestamp;
float ax, ay, az;
float gx, gy, gz;
float mx, my, mz;
float roll, pitch, yaw;
} SensorData;
// **Buffer for 500 Data Points**
#define BUFFER_SIZE 500
SensorData data_buffer[BUFFER_SIZE];
uint16_t data_index = 0;
// **Initialize I2C Communication**
void init_i2c() {
i2c_init(I2C_PORT, 400000); // Set I2C speed to 400 kHz (Fast Mode)
gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);
gpio_pull_up(SDA_PIN);
gpio_pull_up(SCL_PIN);
printf("I2C Initialized on SDA: %d, SCL: %d\n", SDA_PIN, SCL_PIN);
}
absolute_time_t start_time;
// **Get Time Since Boot (ms)**
uint32_t get_time_ms() {
return to_ms_since_boot(get_absolute_time());
}
// **Write to I2C Register**
void write_register(uint8_t addr, uint8_t reg, uint8_t value) {
uint8_t buf[2] = {reg, value};
i2c_write_blocking(I2C_PORT, addr, buf, 2, false);
}
// **Initialize MPU6050 (Set Ranges)**
void init_mpu6050(uint8_t accel_range, uint8_t gyro_range) {
write_register(MPU6050_ADDR, 0x6B, 0x00); // Wake up sensor
write_register(MPU6050_ADDR, 0x1C, accel_range); // Set accel range
write_register(MPU6050_ADDR, 0x1B, gyro_range); // Set gyro range
switch (accel_range) {
case 0x00: accel_scale = 16384.0; break;
case 0x08: accel_scale = 8192.0; break;
case 0x10: accel_scale = 4096.0; break;
case 0x18: accel_scale = 2048.0; break;
}
switch (gyro_range) {
case 0x00: gyro_scale = 131.0; break;
case 0x08: gyro_scale = 65.5; break;
case 0x10: gyro_scale = 32.8; break;
case 0x18: gyro_scale = 16.4; break;
}
printf("MPU6050 Initialized (Accel Scale: %.2f, Gyro Scale: %.2f)\n", accel_scale, gyro_scale);
}
// **Read MPU6050 Sensor Data**
void read_mpu6050(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) {
uint8_t reg = 0x3B;
uint8_t data[14];
int16_t raw_ax, raw_ay, raw_az, raw_gx, raw_gy, raw_gz;
i2c_write_blocking(I2C_PORT, MPU6050_ADDR, ®, 1, true);
i2c_read_blocking(I2C_PORT, MPU6050_ADDR, data, 14, false);
raw_ax = (data[0] << 8) | data[1];
raw_ay = (data[2] << 8) | data[3];
raw_az = (data[4] << 8) | data[5];
raw_gx = (data[8] << 8) | data[9];
raw_gy = (data[10] << 8) | data[11];
raw_gz = (data[12] << 8) | data[13];
*ax = raw_ax / accel_scale;
*ay = raw_ay / accel_scale;
*az = raw_az / accel_scale;
*gx = raw_gx / gyro_scale;
*gy = raw_gy / gyro_scale;
*gz = raw_gz / gyro_scale;
}
// **Initialize QMC5883L**
void init_qmc5883l() {
uint8_t buf[2] = {0x09, 0x1D};
i2c_write_blocking(I2C_PORT, QMC5883L_ADDR, buf, 2, false);
printf("QMC5883L Initialized\n");
}
// **Read QMC5883L Magnetometer**
void read_qmc5883l(float *mx, float *my, float *mz) {
uint8_t reg = 0x00;
uint8_t data[6];
int16_t raw_mag[3];
i2c_write_blocking(I2C_PORT, QMC5883L_ADDR, ®, 1, true);
i2c_read_blocking(I2C_PORT, QMC5883L_ADDR, data, 6, false);
raw_mag[0] = (data[1] << 8) | data[0];
raw_mag[1] = (data[3] << 8) | data[2];
raw_mag[2] = (data[5] << 8) | data[4];
*mx = raw_mag[0] * 0.1f;
*my = raw_mag[1] * 0.1f;
*mz = raw_mag[2] * 0.1f;
}
void init_fusion() {
FusionAhrsInitialise(&ahrs);
}
// **Calculate Orientation**
void calculate_orientation(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float *roll, float *pitch, float *yaw) {
FusionAhrsUpdate(&ahrs, (FusionVector){gx, gy, gz}, (FusionVector){ax, ay, az}, (FusionVector){mx, my, mz}, 0.01f);
FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
*roll = euler.angle.roll;
*pitch = euler.angle.pitch;
*yaw = euler.angle.yaw;
}
//**Initialize SD Card**
void init_sd_card() {
spi_init(SPI_PORT, 1000000);
gpio_set_function(SD_MISO, GPIO_FUNC_SPI);
gpio_set_function(SD_MOSI, GPIO_FUNC_SPI);
gpio_set_function(SD_SCK, GPIO_FUNC_SPI);
gpio_init(SD_CS);
gpio_set_dir(SD_CS, GPIO_OUT);
gpio_put(SD_CS, 1);
FRESULT fr = f_mount(&fs, "", 1);
if (fr != FR_OK) {
printf("SD Card Mount Failed!\n");
} else {
printf("SD Card Mounted Successfully.\n");
}
}
// **Store Data**
void store_data(uint32_t timestamp, float roll, float pitch, float yaw, float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) {
if (data_index < BUFFER_SIZE) {
data_buffer[data_index++] = (SensorData){timestamp, ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, yaw};
}
}
// **Flush Data to SD**
void flush_to_sd() {
if (data_index == BUFFER_SIZE) {
FRESULT fr = f_open(&file, "data_log.txt", FA_WRITE | FA_OPEN_APPEND);
if (fr == FR_OK) {
for (int i = 0; i < BUFFER_SIZE; i++) {
char buffer[100];
snprintf(buffer, sizeof(buffer), "%06d,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
data_buffer[i].timestamp, data_buffer[i].roll, data_buffer[i].pitch, data_buffer[i].yaw,
data_buffer[i].ax, data_buffer[i].ay, data_buffer[i].az,
data_buffer[i].gx, data_buffer[i].gy, data_buffer[i].gz,
data_buffer[i].mx, data_buffer[i].my, data_buffer[i].mz);
f_write(&file, buffer, strlen(buffer), NULL);
}
f_close(&file);
}
data_index = 0;
}
}
//Main Loop
int main() {
stdio_init_all();
init_mpu6050(0x00, 0x00);
init_qmc5883l();
init_sd_card();
init_fusion();
start_time = get_absolute_time();
while (true) {
uint32_t timestamp = absolute_time_diff_us(start_time, get_absolute_time()) / 1000000;
float ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, yaw;
read_mpu6050(&ax, &ay, &az, &gx, &gy, &gz);
read_qmc5883l(&mx, &my, &mz);
calculate_orientation(gx, gy, gz, ax, ay, az, mx, my, mz, &roll, &pitch, &yaw);
store_data(timestamp, roll, pitch, yaw, ax, ay, az, gx, gy, gz, mx, my, mz);
flush_to_sd();
}
return 0;
}
CMakeLists:
if(WIN32)
set(USERHOME $ENV{USERPROFILE})
else()
set(USERHOME $ENV{HOME})
endif()
set(sdkVersion 2.1.0)
set(toolchainVersion 13_3_Rel1)
set(picotoolVersion 2.1.0)
set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
if (EXISTS ${picoVscode})
include(${picoVscode})
endif()
# ====================================================================================
set(PICO_BOARD pico CACHE STRING "Board type")
cmake_minimum_required(VERSION 3.13)
include(pico_sdk_import.cmake)
project(Crusader C CXX ASM)
# Initialise the Raspberry Pi Pico SDK
pico_sdk_init()
# Add executable
add_executable(Crusader Crusader.c )
pico_set_program_name(Crusader "Crusader")
pico_set_program_version(Crusader "0.1")
# Generate PIO header
pico_generate_pio_header(Crusader ${CMAKE_CURRENT_LIST_DIR}/blink.pio)
# Enable standard I/O (UART and USB)
pico_enable_stdio_uart(Crusader 1)
pico_enable_stdio_usb(Crusader 1)
add_subdirectory("Fusion/Fusion")
add_subdirectory("no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src")
# Include directories
target_include_directories(Crusader PRIVATE
${CMAKE_CURRENT_LIST_DIR}
${CMAKE_CURRENT_LIST_DIR}/no-OS-FatFS-SD-SDIO-SPI-RPi-Pico
${CMAKE_CURRENT_LIST_DIR}/no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src
)
# Add standard library and required hardware libraries
target_link_libraries(Crusader
pico_stdlib
hardware_spi
hardware_i2c
hardware_pio
hardware_dma
hardware_gpio
hardware_clocks
)
# Add FatFs library sources
# add_library(no-OS-FatFS-SD-SDIO-SPI-RPi-Pico STATIC
# ${CMAKE_CURRENT_LIST_DIR}/no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src/ff15/source/ff.c
# ${CMAKE_CURRENT_LIST_DIR}/no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src/ff15/source/ffsystem.c
# ${CMAKE_CURRENT_LIST_DIR}/no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/src/ff15/source/ffunicode.c
# )
# Link FatFs library to the project
# target_link_libraries(Crusader no-OS-FatFS-SD-SDIO-SPI-RPi-Pico)
# Add extra outputs
pico_add_extra_outputs(Crusader)
I am unsure that I have linked the files properly in the CMakeLists.txt
Error:
FAILED: Crusader.elf
C:/Users/aadit/.pico-sdk/toolchain/13_3_Rel1/bin/../lib/gcc/arm-none-eabi/13.3.1/../../../../arm-none-eabi/bin/ld.exe: CMakeFiles/Crusader.dir/Crusader.c.obj: in function `flush_to_sd':
D:/Aaditya Sonawane/Personal/Phoenixsky Aerospace/Crusader/Program Files/Raspberry Pi Pico/Crusader/Crusader.c:191:(.text.flush_to_sd.part.0+0x10): undefined reference to `f_open'
C:/Users/aadit/.pico-sdk/toolchain/13_3_Rel1/bin/../lib/gcc/arm-none-eabi/13.3.1/../../../../arm-none-eabi/bin/ld.exe: D:/Aaditya Sonawane/Personal/Phoenixsky Aerospace/Crusader/Program Files/Raspberry Pi Pico/Crusader/Crusader.c:200:(.text.flush_to_sd.part.0+0xb8): undefined reference to `f_write'
C:/Users/aadit/.pico-sdk/toolchain/13_3_Rel1/bin/../lib/gcc/arm-none-eabi/13.3.1/../../../../arm-none-eabi/bin/ld.exe: D:/Aaditya Sonawane/Personal/Phoenixsky Aerospace/Crusader/Program Files/Raspberry Pi Pico/Crusader/Crusader.c:202:(.text.flush_to_sd.part.0+0xc2): undefined reference to `f_close'
C:/Users/aadit/.pico-sdk/toolchain/13_3_Rel1/bin/../lib/gcc/arm-none-eabi/13.3.1/../../../../arm-none-eabi/bin/ld.exe: CMakeFiles/Crusader.dir/Crusader.c.obj: in function `calculate_orientation':
D:/Aaditya Sonawane/Personal/Phoenixsky Aerospace/Crusader/Program Files/Raspberry Pi Pico/Crusader/Crusader.c:155:(.text.calculate_orientation+0x46): undefined reference to `FusionAhrsUpdate'
C:/Users/aadit/.pico-sdk/toolchain/13_3_Rel1/bin/../lib/gcc/arm-none-eabi/13.3.1/../../../../arm-none-eabi/bin/ld.exe: D:/Aaditya Sonawane/Personal/Phoenixsky Aerospace/Crusader/Program Files/Raspberry Pi Pico/Crusader/Crusader.c:156:(.text.calculate_orientation+0x4e): undefined reference to `FusionAhrsGetQuaternion'
C:/Users/aadit/.pico-sdk/toolchain/13_3_Rel1/bin/../lib/gcc/arm-none-eabi/13.3.1/../../../../arm-none-eabi/bin/ld.exe: CMakeFiles/Crusader.dir/Crusader.c.obj: in function `init_sd_card':
D:/Aaditya Sonawane/Personal/Phoenixsky Aerospace/Crusader/Program Files/Raspberry Pi Pico/Crusader/Crusader.c:173:(.text.init_sd_card+0x3a): undefined reference to `f_mount'
C:/Users/aadit/.pico-sdk/toolchain/13_3_Rel1/bin/../lib/gcc/arm-none-eabi/13.3.1/../../../../arm-none-eabi/bin/ld.exe: CMakeFiles/Crusader.dir/Crusader.c.obj: in function `init_fusion':
D:/Aaditya Sonawane/Personal/Phoenixsky Aerospace/Crusader/Program Files/Raspberry Pi Pico/Crusader/Crusader.c:150:(.text.startup.main+0x24): undefined reference to `FusionAhrsInitialise'
collect2.exe: error: ld returned 1 exit status
ninja: build stopped: subcommand failed.
This is the folder structure for reference:
Crusader/
├── CMakeLists.txt
├── Crusader.c
├── Crusader.h
├── pico_sdk_import.cmake
├── build/ (Generated build directory)
│ ├── CMakeCache.txt
│ ├── CMakeFiles/
│ ├── Makefile
│ ├── cmake_install.cmake
│ ├── Crusader.elf
│ ├── Crusader.map
│ ├── Crusader.bin
│ ├── Crusader.uf2
│ ├── libFatFs.a (Missing file issue)
│ ├── libFusion.a
│ ├── ninja.build
│ ├── ...
├── no-OS-FatFS-SD-SDIO-SPI-RPi-Pico/
│ ├── CMakeLists.txt
│ ├── include/
│ │ ├── FatFsSd.h
│ │ ├── FatFsSd_C.h
│ │ ├── FsLib/
│ │ │ ├── ff.h
│ │ │ ├── ffconf.h
│ │ │ ├── ffsystem.c
│ │ │ ├── ffunicode.c
│ │ │ ├── diskio.c
│ │ │ ├── diskio.h
│ │ │ ├── integer.h
│ │ ├── iostream/
│ │ │ ├── iostream.h
│ ├── src/
│ │ ├── FatFsSd.cpp
│ │ ├── f_util.c
│ │ ├── ff_stdio.c
│ │ ├── glue.c
│ │ ├── diskio.c
│ │ ├── ...
│ ├── examples/
│ │ ├── example_fatfs.c
│ ├── sd_driver/
│ │ ├── SPI/
│ │ │ ├── my_spi.c
│ │ │ ├── my_spi.h
│ │ ├── SDIO/
│ │ │ ├── sdio_driver.c
│ │ │ ├── sdio_driver.h
│ ├── README.md
├── Fusion/
│ ├── FusionAhrs.c
│ ├── FusionOffset.c
│ ├── FusionCompass.c
│ ├── Fusion.h
│ ├── CMakeLists.txt
│ ├── libFusion.a