306 lines
11 KiB
C
306 lines
11 KiB
C
|
#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;
|
||
|
|
}
|