86 lines
2.8 KiB
C
86 lines
2.8 KiB
C
#include "icm20948.h"
|
||
|
||
extern I2C_HandleTypeDef hi2c1;
|
||
#define ICM20948_ADDR (0x69 << 1)
|
||
|
||
uint8_t read_register(uint8_t reg) {
|
||
uint8_t val;
|
||
HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, reg, 1, &val, 1, HAL_MAX_DELAY);
|
||
return val;
|
||
}
|
||
|
||
void write_register(uint8_t reg, uint8_t val) {
|
||
HAL_I2C_Mem_Write(&hi2c1, ICM20948_ADDR, reg, 1, &val, 1, HAL_MAX_DELAY);
|
||
}
|
||
|
||
void icm20948_select_bank(uint8_t bank) {
|
||
write_register(0x7F, bank << 4); // REG_BANK_SEL (0x7F)
|
||
}
|
||
|
||
void icm20948_init(void) {
|
||
icm20948_select_bank(0); // BANK 0
|
||
write_register(0x06, 0x01); // PWR_MGMT_1: reset
|
||
HAL_Delay(10);
|
||
write_register(0x06, 0x01); // Clock source auto
|
||
|
||
icm20948_select_bank(2); // BANK 2
|
||
// write_register(0x14, 0x00); // ACCEL_CONFIG: ±2g
|
||
write_register(0x14, 0x04); // ACCEL_CONFIG : ±8g (bits 2:1 = 10)
|
||
|
||
icm20948_select_bank(0); // Revenir en BANK 0 pour lecture
|
||
}
|
||
|
||
void icm20948_read_accel(float *ax, float *ay, float *az) {
|
||
uint8_t data[6];
|
||
HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x2D, 1, data, 6, HAL_MAX_DELAY);
|
||
int16_t x = (data[0] << 8) | data[1];
|
||
int16_t y = (data[2] << 8) | data[3];
|
||
int16_t z = (data[4] << 8) | data[5];
|
||
//*ax = (float)x / 16384.0f;
|
||
//*ay = (float)y / 16384.0f;
|
||
//*az = (float)z / 16384.0f;
|
||
*ax = (float)x / 4096.0f;
|
||
*ay = (float)y / 4096.0f;
|
||
*az = (float)z / 4096.0f;
|
||
|
||
}
|
||
|
||
void icm20948_read_gyro(float *gx, float *gy, float *gz) {
|
||
uint8_t data[6];
|
||
icm20948_select_bank(0);
|
||
HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x33, 1, data, 6, HAL_MAX_DELAY);
|
||
int16_t x = (data[0] << 8) | data[1];
|
||
int16_t y = (data[2] << 8) | data[3];
|
||
int16_t z = (data[4] << 8) | data[5];
|
||
*gx = (float)x / 131.0f; // sensibilité typique ±250 dps
|
||
*gy = (float)y / 131.0f;
|
||
*gz = (float)z / 131.0f;
|
||
}
|
||
|
||
|
||
|
||
void icm20948_mag_init(void) {
|
||
icm20948_select_bank(3);
|
||
// Configurer le bus maître I2C (ex : 400 kHz)
|
||
write_register(0x01, 0x07); // I2C_MST_CTRL, 400 kHz
|
||
// Configure la lecture du magnéto via I2C Master
|
||
// Exemples de setup (registres slave 0, adresse, etc.)
|
||
// ...
|
||
icm20948_select_bank(0);
|
||
}
|
||
|
||
void icm20948_read_mag(float *mx, float *my, float *mz) {
|
||
uint8_t data[6];
|
||
// Lecture des registres magnéto via I2C Master
|
||
// Par défaut les données du magnétomètre sont relayées en registres USER_MAG_DATA_X/Y/Z
|
||
// ou on doit lire via I2C Master
|
||
// Exemple simplifié :
|
||
HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x31, 1, data, 6, HAL_MAX_DELAY);
|
||
int16_t x = (data[1] << 8) | data[0]; // Attention à l’ordre des octets pour AK09916
|
||
int16_t y = (data[3] << 8) | data[2];
|
||
int16_t z = (data[5] << 8) | data[4];
|
||
*mx = (float)x * 0.15f; // conversion en µT (selon datasheet)
|
||
*my = (float)y * 0.15f;
|
||
*mz = (float)z * 0.15f;
|
||
}
|