Skip to content

Commit a64771f

Browse files
Vali0004Vali0004
committed
Add support for the QMC5883p, and general cleanup in drivers
1 parent 36f1361 commit a64771f

File tree

11 files changed

+338
-67
lines changed

11 files changed

+338
-67
lines changed

‎src/sensor/calibration.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -366,7 +366,7 @@ static void sensor_calibrate_imu()
366366
LOG_INF("Suspending sensor thread");
367367
main_imu_suspend();
368368
LOG_INF("Running BMI270 component retrimming");
369-
int err = bmi_crt(sensor_data); // will automatically reinitialize // TODO: this blocks sensor!
369+
int err = bmi270_crt(sensor_data); // will automatically reinitialize // TODO: this blocks sensor!
370370
LOG_INF("Resuming sensor thread");
371371
main_imu_resume();
372372
if (err)
@@ -564,7 +564,7 @@ static int check_sides(const float *a)
564564
}
565565

566566
static void magneto_reset(void)
567-
{
567+
{
568568
magneto_progress = 0; // reusing ata, so guarantee cleared mag progress
569569
last_magneto_progress = 0;
570570
magneto_progress_time = 0;

‎src/sensor/imu/BMI270.c‎

Lines changed: 26 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ static int asic_init(void);
2525
static int upload_config_file(void);
2626
static int factor_zx_read(void);
2727

28-
int bmi_init(float clock_rate, float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time)
28+
int bmi270_init(float clock_rate, float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time)
2929
{
3030
// setup interface for SPI
3131
sensor_interface_spi_configure(SENSOR_INTERFACE_DEV_IMU, MHZ(10), 1);
@@ -38,15 +38,15 @@ int bmi_init(float clock_rate, float accel_time, float gyro_time, float *accel_a
3838
last_gyro_odr = 0xff; // reset last odr
3939
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, BMI270_ACC_RANGE, accel_fs);
4040
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, BMI270_GYR_RANGE, gyro_fs);
41-
err |= bmi_update_odr(accel_time, gyro_time, accel_actual_time, gyro_actual_time);
41+
err |= bmi270_update_odr(accel_time, gyro_time, accel_actual_time, gyro_actual_time);
4242
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, BMI270_FIFO_CONFIG_0, 0x00); // do not return sensortime frame
4343
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, BMI270_FIFO_CONFIG_1, 0xC0); // enable a+g data in FIFO, don't store header
4444
if (err)
4545
LOG_ERR("Communication error");
4646
return (err < 0 ? err : 0);
4747
}
4848

49-
void bmi_shutdown(void) // this does not reset the device, to avoid clearing the config
49+
void bmi270_shutdown(void) // this does not reset the device, to avoid clearing the config
5050
{
5151
last_accel_odr = 0xff; // reset last odr
5252
last_gyro_odr = 0xff; // reset last odr
@@ -56,7 +56,7 @@ void bmi_shutdown(void) // this does not reset the device, to avoid clearing the
5656
LOG_ERR("Communication error");
5757
}
5858

59-
void bmi_update_fs(float accel_range, float gyro_range, float *accel_actual_range, float *gyro_actual_range)
59+
void bmi270_update_fs(float accel_range, float gyro_range, float *accel_actual_range, float *gyro_actual_range)
6060
{
6161
if (accel_range > 8)
6262
{
@@ -107,7 +107,7 @@ void bmi_update_fs(float accel_range, float gyro_range, float *accel_actual_rang
107107
*gyro_actual_range = gyro_range;
108108
}
109109

110-
int bmi_update_odr(float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time)
110+
int bmi270_update_odr(float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time)
111111
{
112112
int ODR;
113113
uint8_t acc_odr;
@@ -260,7 +260,7 @@ int bmi_update_odr(float accel_time, float gyro_time, float *accel_actual_time,
260260
}
261261

262262
// TODO: gyro rotation data is delayed for some reason, accelerometer still responds instantly
263-
uint16_t bmi_fifo_read(uint8_t *data, uint16_t len)
263+
uint16_t bmi270_fifo_read(uint8_t *data, uint16_t len)
264264
{
265265
int err = 0;
266266
uint16_t total = 0;
@@ -292,7 +292,7 @@ static const uint8_t overread[2] = {0x00, 0x80};
292292
static const uint8_t invalid_accel[6] = {0x01, 0x7F, 0x00, 0x80, 0x00, 0x80};
293293
static const uint8_t invalid_gyro[6] = {0x02, 0x7F, 0x00, 0x80, 0x00, 0x80};
294294

295-
int bmi_fifo_process(uint16_t index, uint8_t *data, float a[3], float g[3])
295+
int bmi270_fifo_process(uint16_t index, uint8_t *data, float a[3], float g[3])
296296
{
297297
index *= PACKET_SIZE;
298298
if (!memcmp(&data[index], overread, sizeof(overread)))
@@ -323,7 +323,7 @@ int bmi_fifo_process(uint16_t index, uint8_t *data, float a[3], float g[3])
323323
return 0;
324324
}
325325

326-
void bmi_accel_read(float a[3])
326+
void bmi270_accel_read(float a[3])
327327
{
328328
uint8_t rawAccel[6];
329329
int err = ssi_burst_read(SENSOR_INTERFACE_DEV_IMU, BMI270_DATA_8, &rawAccel[0], 6);
@@ -340,7 +340,7 @@ void bmi_accel_read(float a[3])
340340
a[2] = a_bmi[2];
341341
}
342342

343-
void bmi_gyro_read(float g[3])
343+
void bmi270_gyro_read(float g[3])
344344
{
345345
uint8_t rawGyro[6];
346346
int err = ssi_burst_read(SENSOR_INTERFACE_DEV_IMU, BMI270_DATA_14, &rawGyro[0], 6);
@@ -359,7 +359,7 @@ void bmi_gyro_read(float g[3])
359359
g[2] = g_bmi[2];
360360
}
361361

362-
float bmi_temp_read(void)
362+
float bmi270_temp_read(void)
363363
{
364364
uint8_t rawTemp[2];
365365
int err = ssi_burst_read(SENSOR_INTERFACE_DEV_IMU, BMI270_TEMPERATURE_0, &rawTemp[0], 2);
@@ -375,7 +375,7 @@ float bmi_temp_read(void)
375375
return temp;
376376
}
377377

378-
uint8_t bmi_setup_DRDY(uint16_t threshold)
378+
uint8_t bmi270_setup_DRDY(uint16_t threshold)
379379
{
380380
uint8_t buf[2];
381381
buf[0] = threshold & 0xFF;
@@ -389,7 +389,7 @@ uint8_t bmi_setup_DRDY(uint16_t threshold)
389389
return NRF_GPIO_PIN_PULLUP << 4 | NRF_GPIO_PIN_SENSE_LOW; // active low
390390
}
391391

392-
uint8_t bmi_setup_WOM(void) // TODO: seems too sensitive? try to match icm at least // TODO: half working.
392+
uint8_t bmi270_setup_WOM(void) // TODO: seems too sensitive? try to match icm at least // TODO: half working.
393393
{
394394
uint8_t config[4] = {0};
395395
uint16_t *ptr = (uint16_t *)config; // bmi is little endian
@@ -480,7 +480,7 @@ static int factor_zx_read(void)
480480
}
481481

482482
// from https://github.com/SlimeVR/SlimeVR-Tracker-ESP/blob/main/src/sensors/softfusion/drivers/bmi270.h
483-
int bmi_crt(uint8_t *data)
483+
int bmi270_crt(uint8_t *data)
484484
{
485485
uint8_t status;
486486
uint8_t acc_odr = last_accel_odr; // store last odr
@@ -537,7 +537,7 @@ int bmi_crt(uint8_t *data)
537537
// in case of SPI, where CS pin must trigger rising edge for BMI to enable interface
538538
err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_IMU, 0x00, &tmp);
539539
k_usleep(200);
540-
bmi_init(0, 0, 0, 0, 0);
540+
bmi270_init(0, 0, 0, 0, 0);
541541
if (acc_odr != 0)
542542
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, BMI270_ACC_CONF, 0xA0 | acc_odr);
543543
if (gyr_odr != 0)
@@ -554,7 +554,7 @@ int bmi_crt(uint8_t *data)
554554
return 0;
555555
}
556556

557-
void bmi_gain_apply(uint8_t *data)
557+
void bmi270_gain_apply(uint8_t *data)
558558
{
559559
if (data[0] == 1) // flag for valid gain
560560
{
@@ -564,20 +564,20 @@ void bmi_gain_apply(uint8_t *data)
564564
}
565565

566566
const sensor_imu_t sensor_imu_bmi270 = {
567-
*bmi_init,
568-
*bmi_shutdown,
567+
*bmi270_init,
568+
*bmi270_shutdown,
569569

570-
*bmi_update_fs,
571-
*bmi_update_odr,
570+
*bmi270_update_fs,
571+
*bmi270_update_odr,
572572

573-
*bmi_fifo_read,
574-
*bmi_fifo_process,
575-
*bmi_accel_read,
576-
*bmi_gyro_read,
577-
*bmi_temp_read,
573+
*bmi270_fifo_read,
574+
*bmi270_fifo_process,
575+
*bmi270_accel_read,
576+
*bmi270_gyro_read,
577+
*bmi270_temp_read,
578578

579-
*bmi_setup_DRDY,
580-
*bmi_setup_WOM,
579+
*bmi270_setup_DRDY,
580+
*bmi270_setup_WOM,
581581

582582
*imu_none_ext_setup,
583583
*imu_none_ext_passthrough

‎src/sensor/imu/BMI270.h‎

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -75,23 +75,23 @@
7575
#define RANGE_250 0x03
7676
#define RANGE_125 0x04
7777

78-
int bmi_init(float clock_rate, float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time);
79-
void bmi_shutdown(void);
78+
int bmi270_init(float clock_rate, float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time);
79+
void bmi270_shutdown(void);
8080

81-
void bmi_update_fs(float accel_range, float gyro_range, float *accel_actual_range, float *gyro_actual_range);
82-
int bmi_update_odr(float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time);
81+
void bmi270_update_fs(float accel_range, float gyro_range, float *accel_actual_range, float *gyro_actual_range);
82+
int bmi270_update_odr(float accel_time, float gyro_time, float *accel_actual_time, float *gyro_actual_time);
8383

84-
uint16_t bmi_fifo_read(uint8_t *data, uint16_t len);
85-
int bmi_fifo_process(uint16_t index, uint8_t *data, float a[3], float g[3]);
86-
void bmi_accel_read(float a[3]);
87-
void bmi_gyro_read(float g[3]);
88-
float bmi_temp_read(void);
84+
uint16_t bmi270_fifo_read(uint8_t *data, uint16_t len);
85+
int bmi270_fifo_process(uint16_t index, uint8_t *data, float a[3], float g[3]);
86+
void bmi270_accel_read(float a[3]);
87+
void bmi270_gyro_read(float g[3]);
88+
float bmi270_temp_read(void);
8989

90-
uint8_t bmi_setup_DRDY(uint16_t threshold);
91-
uint8_t bmi_setup_WOM(void);
90+
uint8_t bmi270_setup_DRDY(uint16_t threshold);
91+
uint8_t bmi270_setup_WOM(void);
9292

93-
int bmi_crt(uint8_t *data);
94-
void bmi_gain_apply(uint8_t *data);
93+
int bmi270_crt(uint8_t *data);
94+
void bmi270_gain_apply(uint8_t *data);
9595

9696
extern const sensor_imu_t sensor_imu_bmi270;
9797

0 commit comments

Comments
 (0)