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,305 @@
|
||||
#include "main.h"
|
||||
#include "sensors.h"
|
||||
//#include "stm32wbxx_hal_i2c.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#include "cmsis_os2.h"
|
||||
#include "task.h"
|
||||
|
||||
#define HAL_I2C_TIMEOUT 100
|
||||
//extern osThreadId_t defaultTaskHandle;
|
||||
//TaskHandle_t xStartDefaultTask = NULL;
|
||||
//extern osThreadId_t defaultTaskHandle
|
||||
osThreadId_t xIMU_Task = NULL;
|
||||
osThreadId_t xMagn_Task = NULL;
|
||||
//osThreadId_t xData_Task = NULL;
|
||||
|
||||
|
||||
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
|
||||
if (hspi->Instance == SPI1) {
|
||||
HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
|
||||
// notify the task that the data is ready
|
||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
|
||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
}
|
||||
}
|
||||
|
||||
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address) {
|
||||
// printf("HAL_I2C_MemRxCpltCallback");
|
||||
if (I2C_address->Instance == I2C1) {
|
||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
|
||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
}
|
||||
|
||||
}
|
||||
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address) {
|
||||
// printf("HAL_I2C_MemTxCpltCallback");
|
||||
if (I2C_address->Instance == I2C1) {
|
||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
|
||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
}
|
||||
}
|
||||
|
||||
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
|
||||
if (hi2c->Instance == I2C1) {
|
||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
printf("ErrorCallback: %ld", HAL_I2C_GetError(hi2c));
|
||||
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
|
||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
}
|
||||
}
|
||||
|
||||
// ICM 45686
|
||||
HAL_StatusTypeDef read_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]) {
|
||||
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
|
||||
HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
|
||||
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_SET);
|
||||
return status;
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]) {
|
||||
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
|
||||
HAL_StatusTypeDef status = HAL_SPI_TransmitReceive_DMA(imu->hspi, tx, rx, sizeof(rx));
|
||||
return status;
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[]) {
|
||||
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
|
||||
HAL_StatusTypeDef status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
|
||||
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_SET);
|
||||
return status;
|
||||
}
|
||||
|
||||
void init_icm(ICM45686_HandleTypeDef *imu) {
|
||||
// TODO: Add settings as enum to choose from
|
||||
// FIFO
|
||||
// Interrupt
|
||||
// APEX???
|
||||
// EDMP???
|
||||
|
||||
uint8_t tx[2], rx[2];
|
||||
HAL_StatusTypeDef status;
|
||||
|
||||
printf("Initializing ICM45686\n");
|
||||
// WHO AM
|
||||
tx[0] = ICM45686_WHO_AM_I | 0x80;
|
||||
// HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
|
||||
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
status = read_icm(imu, tx, rx);
|
||||
if (status != HAL_OK) {
|
||||
printf("Error in reading WHO AM I:%d\n", status);
|
||||
} else {
|
||||
printf("WHO AM I (0xE9): 0x%02X \n", rx[1]);
|
||||
}
|
||||
|
||||
// acc
|
||||
tx[0] = ICM45686_ACCEL_CONFIG0;
|
||||
tx[1] = imu->acc_fs | imu->odr;
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
|
||||
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
status = write_icm(imu, tx);
|
||||
if (status != HAL_OK) {
|
||||
printf("Error in setting ACCEL_CONFIG0:%d\n", status);
|
||||
} else {
|
||||
printf("ACCEL_CONFIG0 wrote: 0x%02X \n", tx[1]);
|
||||
}
|
||||
|
||||
tx[0] = ICM45686_ACCEL_CONFIG0 | 0x80;
|
||||
tx[1] = 0x00;
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
|
||||
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
status = read_icm(imu, tx, rx);
|
||||
if (status != HAL_OK) {
|
||||
printf("Error reading ACCEL_CONFIG0:%d\n", status);
|
||||
} else {
|
||||
printf("ACCEL_CONFIG0 read: 0x%02X \n", rx[1]);
|
||||
}
|
||||
|
||||
// gyro
|
||||
tx[0] = ICM45686_GYRO_CONFIG0;
|
||||
tx[1] = imu->gyro_fs | imu->odr;
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
|
||||
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
status = write_icm(imu, tx);
|
||||
if (status != HAL_OK) {
|
||||
printf("Error in setting GYRO_CONFIG0:%d\n", status);
|
||||
} else {
|
||||
printf("GYRO_CONFIG0 wrote: 0x%02X \n", tx[1]);
|
||||
}
|
||||
|
||||
tx[0] = ICM45686_GYRO_CONFIG0 | 0x80;
|
||||
tx[1] = 0x00;
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
|
||||
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
status = read_icm(imu, tx, rx);
|
||||
if (status != HAL_OK) {
|
||||
printf("Error reading GYRO_CONFIG0:%d\n", status);
|
||||
} else {
|
||||
printf("GYRO_CONFIG0 read: 0x%02X \n", rx[1]);
|
||||
}
|
||||
|
||||
// power settings
|
||||
tx[0] = ICM45686_PWR_MGMT0;
|
||||
tx[1] = 0x0F; //0x10 | 0x03;
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
|
||||
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
status = write_icm(imu, tx);
|
||||
if (status != HAL_OK) {
|
||||
printf("Error in setting PWR_MGMT0:%d\n", status);
|
||||
} else {
|
||||
printf("PWR_MGMT0 wrote: 0x%02X \n", tx[1]);
|
||||
}
|
||||
|
||||
tx[0] = ICM45686_PWR_MGMT0 | 0x80;
|
||||
tx[1] = 0x00;
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
|
||||
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
|
||||
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
|
||||
status = read_icm(imu, tx, rx);
|
||||
if (status != HAL_OK) {
|
||||
printf("Error reading PWR_MGMT0:%d\n", status);
|
||||
} else {
|
||||
printf("PWR_MGMT0 read: 0x%02X \n", rx[1]);
|
||||
}
|
||||
|
||||
osDelay(250);
|
||||
}
|
||||
|
||||
void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
|
||||
HAL_StatusTypeDef status;
|
||||
uint8_t raw_data[15];
|
||||
int16_t sensor_data[7] = {0};
|
||||
uint8_t tx[15] = {0x00};
|
||||
tx[0] = 0x00 | 0x80;
|
||||
|
||||
for (int i = 1; i <= samples; i++) {
|
||||
status = read_icm_dma(imu, tx, raw_data);
|
||||
if (status != HAL_OK) {
|
||||
printf("Calibration read ICM failed: %d\n", status);
|
||||
}
|
||||
sensor_data[0] += (int16_t)(raw_data[1] << 8 | raw_data[2]); // a_x
|
||||
sensor_data[1] += (int16_t)(raw_data[3] << 8 | raw_data[4]); // a_y
|
||||
sensor_data[2] += (int16_t)(raw_data[5] << 8 | raw_data[6]); // a_z
|
||||
sensor_data[3] += (int16_t)(raw_data[7] << 8 | raw_data[8]); // g_x
|
||||
sensor_data[4] += (int16_t)(raw_data[9] << 8 | raw_data[10]); // g_y
|
||||
sensor_data[5] += (int16_t)(raw_data[11] << 8 | raw_data[12]); // g_z
|
||||
sensor_data[6] += (int16_t)(raw_data[13] << 8 | raw_data[14]); // t
|
||||
}
|
||||
|
||||
imu->imu_bias[0] = (float)sensor_data[0] / (float)samples;
|
||||
imu->imu_bias[1] = (float)sensor_data[1] / (float)samples;
|
||||
imu->imu_bias[2] = (float)sensor_data[2] / (float)samples;
|
||||
imu->imu_bias[3] = (float)sensor_data[3] / (float)samples;
|
||||
imu->imu_bias[4] = (float)sensor_data[4] / (float)samples;
|
||||
imu->imu_bias[5] = (float)sensor_data[5] / (float)samples;
|
||||
imu->imu_bias[6] = (float)sensor_data[6] / (float)samples;
|
||||
|
||||
// remrove gravity
|
||||
imu->imu_bias[2] -= 1.0f;
|
||||
|
||||
imu->calibated = true;
|
||||
|
||||
printf("Calibration finished of ICM45686\n");
|
||||
printf("a_x: %.2f a_y: %.2f a_z: %.2f g_x: %.2f g_y: %.2f g_z: %.2f t: %.2f\n",
|
||||
imu->imu_bias[0], imu->imu_bias[1], imu->imu_bias[2],
|
||||
imu->imu_bias[3], imu->imu_bias[4], imu->imu_bias[5],
|
||||
imu->imu_bias[6]);
|
||||
|
||||
}
|
||||
|
||||
// BMM350
|
||||
void bmm_init_DWT(void){
|
||||
// Enable trace and debug block (TRCENA)
|
||||
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
|
||||
|
||||
// Reset cycle counter
|
||||
DWT->CYCCNT = 0;
|
||||
|
||||
// Enable the cycle counter
|
||||
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
|
||||
}
|
||||
|
||||
BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr){
|
||||
// Extract I2C device address from intf_ptr
|
||||
uint8_t device_addr = *(uint8_t*)intf_ptr;
|
||||
|
||||
// STM32 HAL: Write register address, then read data
|
||||
// Device address is left-shifted by 1 for STM32 HAL
|
||||
// if (HAL_I2C_Mem_Read(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
|
||||
if (HAL_I2C_Mem_Read_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length) == HAL_OK) {
|
||||
if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
|
||||
return BMM350_INTF_RET_SUCCESS; // 0
|
||||
}
|
||||
}
|
||||
return -1; // Communication failure
|
||||
|
||||
}
|
||||
|
||||
BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr){
|
||||
uint8_t device_addr = *(uint8_t*)intf_ptr;
|
||||
// uint8_t rslt = HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length);
|
||||
// printf("rslt write: %d\n", rslt);
|
||||
// // STM32 HAL: Write register address + data
|
||||
if (HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length) == HAL_OK) {
|
||||
// if (HAL_I2C_Mem_Write(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
|
||||
// if (rslt == HAL_OK) {
|
||||
if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
|
||||
return BMM350_INTF_RET_SUCCESS; // 0
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void bmm350_delay(uint32_t period_us, void *intf_ptr){
|
||||
(void)intf_ptr; // Unused
|
||||
|
||||
// For STM32, use DWT cycle counter for accurate microsecond delays
|
||||
uint32_t start = DWT->CYCCNT;
|
||||
uint32_t cycles = period_us * (SystemCoreClock / 1000000);
|
||||
|
||||
while ((DWT->CYCCNT - start) < cycles);
|
||||
}
|
||||
|
||||
// Initialization function
|
||||
int8_t init_bmm(struct bmm350_dev *dev) {
|
||||
int8_t rslt;
|
||||
|
||||
// Static variable to hold I2C device address
|
||||
// Must be static or global because dev.intf_ptr will point to it
|
||||
static uint8_t dev_addr = BMM350_I2C_ADSEL_SET_LOW;
|
||||
// Assign function pointers
|
||||
dev->read = bmm350_i2c_read;
|
||||
dev->write = bmm350_i2c_write;
|
||||
dev->delay_us = bmm350_delay;
|
||||
dev->intf_ptr = &dev_addr;
|
||||
|
||||
printf("Starting up and configuring BMM350\n");
|
||||
|
||||
// Initialize BMM350 driver
|
||||
rslt = bmm350_init(dev);
|
||||
if (rslt != BMM350_OK)
|
||||
{
|
||||
// Handle error: chip ID mismatch, communication failure, etc.
|
||||
return rslt;
|
||||
}
|
||||
printf("Initialization: %d\n", rslt);
|
||||
printf("CHIP_ID (should be 0x33) - Read Register: 0x00 BMM350 Chip ID: 0x%X\n", dev->chip_id);
|
||||
|
||||
// Set powermode, odr and avg
|
||||
rslt = bmm350_set_powermode(BMM350_NORMAL_MODE, dev);
|
||||
printf("Powermode (%d): %d\n", BMM350_NORMAL_MODE, rslt);
|
||||
rslt = bmm350_set_odr_performance(BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, dev);
|
||||
printf("ODR(%d) and AVG(%d): %d\n", BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, rslt);
|
||||
|
||||
return BMM350_OK;
|
||||
}
|
||||
Reference in New Issue
Block a user