Trinity Devboard PCB V1.0 Firmware. FreeRTOS is setup and the MCU reads IMU data over SPI fand Magnetometer data over I2C, each with a seperate task. Sensordata is then run though MadgwickAHRS and send over USB as serial packet data to use in trinity visualizer. Bare minimum functionality works and is replicated from the first prototype.
This commit is contained in:
@@ -0,0 +1,164 @@
|
||||
#include "main.h"
|
||||
#include <stdbool.h>
|
||||
#include "bmm350.h"
|
||||
#include "bmm350_defs.h"
|
||||
#include "bmm350_oor.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "cmsis_os2.h"
|
||||
|
||||
|
||||
// I2C Handle
|
||||
extern I2C_HandleTypeDef hi2c1;
|
||||
extern osThreadId_t defaultTaskHandle;
|
||||
extern osThreadId_t xIMU_Task;
|
||||
extern osThreadId_t xMagn_Task;
|
||||
//extern osThreadId_t xData_Task;
|
||||
|
||||
// ICM45686
|
||||
typedef struct {
|
||||
SPI_HandleTypeDef *hspi;
|
||||
GPIO_TypeDef *GPIO_Port;
|
||||
uint16_t GPIO_Pin;
|
||||
|
||||
uint8_t acc_fs;
|
||||
uint8_t gyro_fs;
|
||||
uint8_t odr;
|
||||
float acc_ssf;
|
||||
float gyro_ssf;
|
||||
|
||||
bool calibated;
|
||||
// float accel_bias[3];
|
||||
// float gyro_bias[3];
|
||||
float imu_bias[7];
|
||||
} ICM45686_HandleTypeDef;
|
||||
|
||||
typedef struct {
|
||||
float processed_imu_data[7];
|
||||
} ICM45686_Data;
|
||||
|
||||
// dataframe for sending quaternions over usb
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint8_t StartByte;
|
||||
uint8_t SensorAddress;
|
||||
float qw;
|
||||
float qx;
|
||||
float qy;
|
||||
float qz;
|
||||
uint8_t EndByte;
|
||||
} QuaternionData;
|
||||
|
||||
// enum to hold the packet dataframe data
|
||||
typedef enum {
|
||||
PACKET_START_BYTE = 0xDE,
|
||||
PACKET_END_BYTE = 0xAD
|
||||
} Packet_Bytes;
|
||||
|
||||
|
||||
typedef enum {
|
||||
ICM45686_WHO_AM_I = 0x72, // 0x72 = 01110010, dann den wert NIX(???)) einfach nur lesen
|
||||
ICM45686_ACCEL_CONFIG0 = 0x1B,
|
||||
ICM45686_GYRO_CONFIG0 = 0x1C,
|
||||
ICM45686_FIFO_CONFIG0 = 0x1D,
|
||||
ICM45686_PWR_MGMT0 = 0x10,
|
||||
ICM45686_ODR_DECIMATE_CONFIG = 0x40,
|
||||
ICM45686_ACCEL_DATA = 0x00,
|
||||
ICM45686_GYRO_DATA = 0x06,
|
||||
ICM45686_FIFO_COUNT = 0x12,
|
||||
ICM45686_FIFO_DATA = 0x14
|
||||
} ICM45686_registers;
|
||||
|
||||
typedef enum {
|
||||
ICM45686_ACC_FS_32G = 0x00,
|
||||
ICM45686_ACC_FS_16G = 0x10,
|
||||
ICM45686_ACC_FS_8G = 0x20,
|
||||
ICM45686_ACC_FS_4G = 0x30,
|
||||
ICM45686_ACC_FS_2G = 0x40,
|
||||
} ICM45686_ACC_CONF;
|
||||
|
||||
//typedef enum {
|
||||
// ICM45686_ACC_SSF_32G = 16384.0,
|
||||
// ICM45686_ACC_SSF_16G = 8192.0,
|
||||
// ICM45686_ACC_SSF_8G = 4096.0,
|
||||
// ICM45686_ACC_SSF_4G = 2048.0,
|
||||
// ICM45686_ACC_SSF_2G = 1024.0,
|
||||
//} ICM45686_ACC_FACTORS;
|
||||
|
||||
static const float ICM45686_ACC_SSF [5] = {
|
||||
1024.0f, 2048.0f, 4096.0f, 8192.0f, 16384.0f
|
||||
};
|
||||
|
||||
typedef enum {
|
||||
ICM45686_GYRO_FS_4000DPS = 0x00,
|
||||
ICM45686_GYRO_FS_2000DPS = 0x10,
|
||||
ICM45686_GYRO_FS_1000DPS = 0x20,
|
||||
ICM45686_GYRO_FS_500DPS = 0x30,
|
||||
ICM45686_GYRO_FS_250DPS = 0x40,
|
||||
ICM45686_GYRO_FS_125DPS = 0x50,
|
||||
ICM45686_GYRO_FS_62_5DPS = 0x60,
|
||||
ICM45686_GYRO_FS_31_25DPS = 0x70,
|
||||
ICM45686_GYRO_FS_15_625DPS = 0x80,
|
||||
} ICM45686_GYRO_CONF;
|
||||
|
||||
//typedef enum {
|
||||
// ICM45686_GYRO_SSF_4000DPS = 2097.2,
|
||||
// ICM45686_GYRO_SSF_2000DPS = 1048.6,
|
||||
// ICM45686_GYRO_SSF_1000DPS = 524.3,
|
||||
// ICM45686_GYRO_SSF_500DPS = 262.0,
|
||||
// ICM45686_GYRO_SSF_250DPS = 131.0,
|
||||
// ICM45686_GYRO_SSF_125DPS = 65.8,
|
||||
// ICM45686_GYRO_SSF_62_5DPS = 32.8,
|
||||
// ICM45686_GYRO_SSF_31_25DPS = 16.4,
|
||||
// ICM45686_GYRO_SSF_15_626DPS = 8.2,
|
||||
//} ICM45686_GYRO_FACTORS;
|
||||
|
||||
static const float ICM45686_GYRO_SSF [9] = {
|
||||
8.2f, 16.4f, 32.8f, 65.5f, 131.0f, 262.0f, 524.3f, 1048.6f, 2097.2f
|
||||
};
|
||||
|
||||
typedef enum {
|
||||
ICM45686_ODR_6_4kHz_LN = 0x03 ,
|
||||
ICM45686_ODR_3_2kHz_LN = 0x04,
|
||||
ICM45686_ODR_1_6kHz_LN = 0x05,
|
||||
ICM45686_ODR_800Hz_LN = 0x06,
|
||||
ICM45686_ODR_400Hz_LNLP = 0x07,
|
||||
ICM45686_ODR_200Hz_LNLP = 0x08,
|
||||
ICM45686_ODR_100Hz_LNLP = 0x09,
|
||||
ICM45686_ODR_50Hz_LNLP = 0x0A,
|
||||
ICM45686_ODR_25Hz_LNLP = 0x0B,
|
||||
ICM45686_ODR_12_5Hz_LNLP = 0x0C,
|
||||
ICM45686_ODR_6_26Hz_LP = 0x0D,
|
||||
ICM45686_ODR_3_125Hz_LP = 0x0E,
|
||||
ICM45686_ODR_1_5625Hz_LP = 0x0F
|
||||
} ICM45686_ODR;
|
||||
|
||||
|
||||
|
||||
|
||||
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi);
|
||||
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address);
|
||||
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address);
|
||||
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c);
|
||||
|
||||
// ICM45686
|
||||
void init_icm(ICM45686_HandleTypeDef *imu);
|
||||
HAL_StatusTypeDef read_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]);
|
||||
HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]);
|
||||
HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[]);
|
||||
void calibrate_icm (ICM45686_HandleTypeDef * imu, int samples);
|
||||
|
||||
// BMM350 - Declarations of read/write/delay functions for the BMM350 API
|
||||
void bmm_init_DWT(void);
|
||||
BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr);
|
||||
BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr);
|
||||
void bmm350_delay(uint32_t period_us, void *intf_ptr);
|
||||
int8_t init_bmm(struct bmm350_dev *dev);
|
||||
|
||||
// structs that hold the sensor data
|
||||
typedef struct {
|
||||
uint8_t raw_imu[14];
|
||||
uint8_t raw_magn [9];
|
||||
} raw_sensor_data;
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user