From 6de702a1444bc3641ba80856097fbf3c75b9faaf Mon Sep 17 00:00:00 2001 From: Florian Sansou Date: Mon, 25 Nov 2024 22:53:33 +0100 Subject: [PATCH] Driver for MPU6500, HMC59xx and BMP280 (#3406) * Enables MPU6500 with MPU60x0 spi driver, adds hmc59xx and enables BMP280 SPI and I2C --------- Co-authored-by: Florian Sansou --- conf/modules/airspeed_adc.xml | 3 +- conf/modules/airspeed_ads1114.xml | 4 +- conf/modules/baro_bmp280.xml | 51 +++ conf/modules/baro_bmp280_i2c.xml | 30 -- conf/modules/baro_bmp3.xml | 3 +- conf/modules/board_matek_f405_wing.xml | 5 +- conf/modules/board_matek_f765_car.xml | 4 +- conf/modules/board_matek_f765_wing.xml | 4 +- conf/modules/mag_hmc58xx.xml | 6 + conf/modules/mag_hmc59xx.xml | 18 + .../{baro_bmp280_i2c.c => baro_bmp280.c} | 39 +- .../{baro_bmp280_i2c.h => baro_bmp280.h} | 15 +- sw/airborne/peripherals/bmp280.c | 399 ++++++++++++++++++ sw/airborne/peripherals/bmp280.h | 118 ++++++ sw/airborne/peripherals/bmp280_i2c.c | 248 ----------- sw/airborne/peripherals/bmp280_i2c.h | 53 --- sw/airborne/peripherals/bmp280_regs.h | 60 +-- sw/airborne/peripherals/hmc58xx.c | 12 +- sw/airborne/peripherals/hmc58xx.h | 12 +- sw/airborne/peripherals/hmc58xx_regs.h | 2 +- sw/airborne/peripherals/mpu60x0_regs.h | 1 + sw/airborne/peripherals/mpu60x0_spi.c | 5 +- 22 files changed, 678 insertions(+), 414 deletions(-) create mode 100644 conf/modules/baro_bmp280.xml delete mode 100644 conf/modules/baro_bmp280_i2c.xml create mode 100644 conf/modules/mag_hmc59xx.xml rename sw/airborne/modules/sensors/{baro_bmp280_i2c.c => baro_bmp280.c} (77%) rename sw/airborne/modules/sensors/{baro_bmp280_i2c.h => baro_bmp280.h} (79%) create mode 100644 sw/airborne/peripherals/bmp280.c create mode 100644 sw/airborne/peripherals/bmp280.h delete mode 100644 sw/airborne/peripherals/bmp280_i2c.c delete mode 100644 sw/airborne/peripherals/bmp280_i2c.h diff --git a/conf/modules/airspeed_adc.xml b/conf/modules/airspeed_adc.xml index 66577e3459..08b97bdf3b 100644 --- a/conf/modules/airspeed_adc.xml +++ b/conf/modules/airspeed_adc.xml @@ -31,7 +31,7 @@ - + @@ -43,4 +43,3 @@ - diff --git a/conf/modules/airspeed_ads1114.xml b/conf/modules/airspeed_ads1114.xml index ac9303d95a..9921a33ae0 100644 --- a/conf/modules/airspeed_ads1114.xml +++ b/conf/modules/airspeed_ads1114.xml @@ -20,11 +20,9 @@ - + - - diff --git a/conf/modules/baro_bmp280.xml b/conf/modules/baro_bmp280.xml new file mode 100644 index 0000000000..19a9f779b9 --- /dev/null +++ b/conf/modules/baro_bmp280.xml @@ -0,0 +1,51 @@ + + + + + + Bosch-Sensortech BMP280xx pressure sensor (I2C or SPI) + + + + + + + + + i2c|spi_master + baro + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/baro_bmp280_i2c.xml b/conf/modules/baro_bmp280_i2c.xml deleted file mode 100644 index 2d850bd3a8..0000000000 --- a/conf/modules/baro_bmp280_i2c.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - Bosch-Sensortech BMP280xx pressure sensor - - - - - - - i2c - baro - -
- -
- - - - - - - - - - - - diff --git a/conf/modules/baro_bmp3.xml b/conf/modules/baro_bmp3.xml index 1b5e95e327..c69b2a9667 100644 --- a/conf/modules/baro_bmp3.xml +++ b/conf/modules/baro_bmp3.xml @@ -26,10 +26,9 @@ - + - diff --git a/conf/modules/board_matek_f405_wing.xml b/conf/modules/board_matek_f405_wing.xml index 5c3dc2874f..ec6ddb22f9 100644 --- a/conf/modules/board_matek_f405_wing.xml +++ b/conf/modules/board_matek_f405_wing.xml @@ -13,7 +13,7 @@ - spi_master,osd_max7456,baro_bmp280_i2c,imu_mpu6000 + spi_master,osd_max7456,baro_bmp280,imu_mpu6000 @@ -21,8 +21,7 @@ - - + diff --git a/conf/modules/board_matek_f765_car.xml b/conf/modules/board_matek_f765_car.xml index 7f95034103..472fee04c6 100644 --- a/conf/modules/board_matek_f765_car.xml +++ b/conf/modules/board_matek_f765_car.xml @@ -14,7 +14,7 @@ - spi_master,baro_bmp280_i2c,current_sensor,imu_mpu6000 + spi_master,baro_bmp280,current_sensor,imu_mpu6000 @@ -27,7 +27,7 @@ - + diff --git a/conf/modules/board_matek_f765_wing.xml b/conf/modules/board_matek_f765_wing.xml index 8325b73de1..daf5678d89 100644 --- a/conf/modules/board_matek_f765_wing.xml +++ b/conf/modules/board_matek_f765_wing.xml @@ -14,7 +14,7 @@ - spi_master,osd_max7456,baro_bmp280_i2c,current_sensor,imu_mpu6000 + spi_master,osd_max7456,baro_bmp280,current_sensor,imu_mpu6000 @@ -27,7 +27,7 @@ - + diff --git a/conf/modules/mag_hmc58xx.xml b/conf/modules/mag_hmc58xx.xml index ba33838afe..a2e79785a0 100644 --- a/conf/modules/mag_hmc58xx.xml +++ b/conf/modules/mag_hmc58xx.xml @@ -21,6 +21,8 @@ + + i2c,@imu @@ -44,6 +46,10 @@ + + + + diff --git a/conf/modules/mag_hmc59xx.xml b/conf/modules/mag_hmc59xx.xml new file mode 100644 index 0000000000..1e295c06c4 --- /dev/null +++ b/conf/modules/mag_hmc59xx.xml @@ -0,0 +1,18 @@ + + + + + + HMC59xx magnetometer with HMC58xx magnetometer driver. + + + + mag_hmc58xx + mag + + + + + + + diff --git a/sw/airborne/modules/sensors/baro_bmp280_i2c.c b/sw/airborne/modules/sensors/baro_bmp280.c similarity index 77% rename from sw/airborne/modules/sensors/baro_bmp280_i2c.c rename to sw/airborne/modules/sensors/baro_bmp280.c index 6b58bda45e..2155c2dee8 100644 --- a/sw/airborne/modules/sensors/baro_bmp280_i2c.c +++ b/sw/airborne/modules/sensors/baro_bmp280.c @@ -1,5 +1,6 @@ /* * Chris Efstathiou hendrixgr@gmail.com + * Florian Sansou florian.sansou@enac.fr * This file is part of paparazzi. * * paparazzi is free software; you can redistribute it and/or modify @@ -19,14 +20,14 @@ */ /** - * @file modules/sensors/baro_bmp280_i2c.c - * Bosch BMP280 I2C sensor interface. + * @file modules/sensors/baro_bmp280.c + * Bosch BMP280 sensor interface. * - * This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C. + * This reads the values for pressure and temperature from the Bosch BMP280 sensor. */ -#include "baro_bmp280_i2c.h" +#include "baro_bmp280.h" #include "modules/core/abi.h" #include "mcu_periph/uart.h" @@ -37,19 +38,31 @@ #if DOWNLINK && !defined(BMP280_SYNC_SEND) #include "modules/datalink/telemetry.h" #endif +PRINT_CONFIG_VAR(BMP280_SYNC_SEND) +#ifndef BMP280_USE_SPI +#define BMP280_USE_SPI FALSE +#endif +PRINT_CONFIG_VAR(BMP280_USE_SPI) +PRINT_CONFIG_VAR(BMP280_DEV) /** default slave address */ + #ifndef BMP280_SLAVE_ADDR #define BMP280_SLAVE_ADDR BMP280_I2C_ADDR #endif +#ifndef BMP280_SLAVE_IDX +#define BMP280_SLAVE_IDX SPI_SLAVE0 +#endif + + float baro_alt = 0; float baro_temp = 0; float baro_press = 0; bool baro_alt_valid = 0; -struct Bmp280_I2c baro_bmp280; +struct bmp280_t baro_bmp280; #if DOWNLINK && !defined(BMP280_SYNC_SEND) static void send_baro_bmp_data(struct transport_tx *trans, struct link_device *dev) @@ -67,8 +80,16 @@ static void send_baro_bmp_data(struct transport_tx *trans, struct link_device *d void baro_bmp280_init(void) { - - bmp280_i2c_init(&baro_bmp280, &BMP280_I2C_DEV, BMP280_SLAVE_ADDR); + #if BMP280_USE_SPI + baro_bmp280.bus = BMP280_SPI; + baro_bmp280.spi.p = &BMP280_DEV; + baro_bmp280.spi.slave_idx = BMP280_SLAVE_IDX; + #else + baro_bmp280.bus = BMP280_I2C; + baro_bmp280.i2c.p = &BMP280_DEV; + baro_bmp280.i2c.slave_addr = BMP280_SLAVE_ADDR; + #endif + bmp280_init(&baro_bmp280); #if DOWNLINK && !defined(BMP280_SYNC_SEND) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BMP_STATUS, send_baro_bmp_data); #endif @@ -76,12 +97,12 @@ void baro_bmp280_init(void) void baro_bmp280_periodic(void) { - bmp280_i2c_periodic(&baro_bmp280); + bmp280_periodic(&baro_bmp280); } void baro_bmp280_event(void) { - bmp280_i2c_event(&baro_bmp280); + bmp280_event(&baro_bmp280); if (baro_bmp280.data_available) { uint32_t now_ts = get_sys_time_usec(); diff --git a/sw/airborne/modules/sensors/baro_bmp280_i2c.h b/sw/airborne/modules/sensors/baro_bmp280.h similarity index 79% rename from sw/airborne/modules/sensors/baro_bmp280_i2c.h rename to sw/airborne/modules/sensors/baro_bmp280.h index 0dfb5c66c7..ef59f29101 100644 --- a/sw/airborne/modules/sensors/baro_bmp280_i2c.h +++ b/sw/airborne/modules/sensors/baro_bmp280.h @@ -1,5 +1,6 @@ /* * Chris Efstathiou hendrixgr@gmail.com + * Florian Sansou florian.sansou@enac.fr * * This file is part of paparazzi. * @@ -20,18 +21,18 @@ */ /** - * @file modules/sensors/baro_bmp280_i2c.h - * Bosch BMP280 I2C sensor interface. + * @file modules/sensors/baro_bmp280.h + * Bosch BMP280 sensor interface. * - * This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C. + * This reads the values for pressure and temperature from the Bosch BMP280 sensor. */ -#ifndef BARO_BMP280_I2C_H -#define BARO_BMP280_I2C_H +#ifndef BARO_BMP280_H +#define BARO_BMP280_H -#include "peripherals/bmp280_i2c.h" +#include "peripherals/bmp280.h" -extern struct Bmp280_I2c baro_bmp280; +extern struct bmp280_t baro_bmp280; extern float baro_alt; extern bool baro_alt_valid; diff --git a/sw/airborne/peripherals/bmp280.c b/sw/airborne/peripherals/bmp280.c new file mode 100644 index 0000000000..0e4ad72857 --- /dev/null +++ b/sw/airborne/peripherals/bmp280.c @@ -0,0 +1,399 @@ +/* + * Chris Efstathiou hendrixgr@gmail.com + * Florian Sansou florian.sansou@enac.fr + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file peripherals/bmp280.c + * @brief Sensor driver for BMP280 sensor + * + * + */ + +#include "peripherals/bmp280.h" + +/** local function to extract raw data from i2c buffer + * and compute compensation with selected precision + */ +static void parse_sensor_data(struct bmp280_t *bmp, uint8_t *data); +static void parse_calib_data(struct bmp280_t *bmp, uint8_t *data); +PRINT_CONFIG_MSG("BMP280 uses double precision compensation") +static double compensate_pressure(struct bmp280_t *bmp); +static double compensate_temperature(struct bmp280_t *bmp); +static bool bmp280_config(struct bmp280_t *bmp); +static void bmp280_register_write(struct bmp280_t *bmp, uint8_t reg, uint8_t value); +static void bmp280_register_read(struct bmp280_t *bmp, uint8_t reg, uint16_t size); + + +/** + * @brief Initialize the bmp280 sensor instance + * + * @param bmp The structure containing the configuration of the bmp280 instance + */ +void bmp280_init(struct bmp280_t *bmp) +{ + + bmp->data_available = false; + bmp->initialized = false; + bmp->status = BMP280_STATUS_UNINIT; + + bmp->config_idx = 0; + + /* SPI setup */ + if(bmp->bus == BMP280_SPI) { + bmp->spi.trans.cpol = SPICpolIdleHigh; + bmp->spi.trans.cpha = SPICphaEdge2; + bmp->spi.trans.dss = SPIDss8bit; + bmp->spi.trans.bitorder = SPIMSBFirst; + bmp->spi.trans.cdiv = SPIDiv16; + + bmp->spi.trans.select = SPISelectUnselect; + bmp->spi.trans.slave_idx = bmp->spi.slave_idx; + bmp->spi.trans.output_length = 0; + bmp->spi.trans.input_length = 0; + bmp->spi.trans.before_cb = NULL; + bmp->spi.trans.after_cb = NULL; + bmp->spi.trans.input_buf = bmp->spi.rx_buf; + bmp->spi.trans.output_buf = bmp->spi.tx_buf; + bmp->spi.trans.status = SPITransDone; + + // in SPI read, the first byte is garbage because writing the register address + bmp->rx_buffer = &bmp->spi.rx_buf[1]; + bmp->tx_buffer = bmp->spi.tx_buf; + bmp->rx_length = &bmp->spi.trans.input_length; + } + /* I2C setup */ + else { + + bmp->i2c.trans.slave_addr = bmp->i2c.slave_addr; // slave address + bmp->i2c.trans.status = I2CTransDone; // set initial status: Done + + bmp->rx_buffer = (uint8_t *)bmp->i2c.trans.buf; + bmp->tx_buffer = (uint8_t *)bmp->i2c.trans.buf; + bmp->rx_length = &bmp->i2c.trans.len_r; + + } +} + +/** + * @brief Should be called periodically to request sensor readings + * - First detects the sensor using WHO_AM_I reading + * - Configures the sensor according the users requested configuration + * - Requests a sensor reading + * + * @param bmp The bmp280 instance + */ +void bmp280_periodic(struct bmp280_t *bmp) +{ + + /* Idle */ + if((bmp->bus == BMP280_SPI && bmp->spi.trans.status == SPITransDone) || + (bmp->bus == BMP280_I2C && bmp->i2c.trans.status == I2CTransDone)) { + + switch (bmp->status) { + case BMP280_STATUS_UNINIT: + bmp->data_available = false; + bmp->initialized = false; + bmp->status = BMP280_STATUS_IDLE; + break; + + case BMP280_STATUS_IDLE: + /* Request WHO_AM_I */ + bmp280_register_read(bmp, BMP280_CHIP_ID_REG_ADDR, 1); + break; + + case BMP280_STATUS_GET_CALIB: + // request calibration data + bmp280_register_read(bmp, BMP280_CALIB_LSB_DATA_ADDR, BMP280_CALIB_DATA_LEN); + //process in bmp280_event() + break; + + case BMP280_STATUS_CONFIGURE: + /* Start configuring */ + if(bmp280_config(bmp)) { + bmp->status = BMP280_STATUS_READ_STATUS_REG; + bmp->initialized = true; + } + break; + + case BMP280_STATUS_READ_STATUS_REG: + // READ THE STATUS BYTE + bmp280_register_read(bmp, BMP280_STATUS_REG_ADDR, 1); + break; + + case BMP280_STATUS_READ_DATA_REGS: + // READ ALL 6 DATA REGISTERS + bmp280_register_read(bmp, BMP280_DATA_START_REG_ADDR, BMP280_P_T_DATA_LEN); + break; + + default: + break; + } + } +} + +/** + * @brief Should be called in the event thread + * - Configures the sensor and reads the responses + * - Parse and request the sensor data + * + * @param bmp The bmp280 instance + */ +void bmp280_event(struct bmp280_t *bmp) +{ + /* Successful transfer */ + if((bmp->bus == BMP280_SPI && bmp->spi.trans.status == SPITransSuccess) || + (bmp->bus == BMP280_I2C && bmp->i2c.trans.status == I2CTransSuccess)) { + switch (bmp->status) { + + case BMP280_STATUS_IDLE: + /* WHO_AM_I */ + if(bmp->rx_buffer[0] == BMP280_ID_NB) { + bmp->device = BMP_280; + bmp->status = BMP280_STATUS_GET_CALIB; + } else { + bmp->status = BMP280_STATUS_IDLE; + } + break; + + case BMP280_STATUS_GET_CALIB: + // compute calib + parse_calib_data(bmp, &bmp->rx_buffer[0]); + bmp->status = BMP280_STATUS_CONFIGURE; + break; + + case BMP280_STATUS_CONFIGURE: + if(bmp280_config(bmp)) { + bmp->status = BMP280_STATUS_READ_STATUS_REG; + bmp->initialized = true; + } + break; + + case BMP280_STATUS_READ_STATUS_REG: + // check status byte + if ((bmp->rx_buffer[0] & (BMP280_EOC_BIT | BMP280_NVRAM_COPY_BIT)) == 0) { + bmp->status = BMP280_STATUS_READ_DATA_REGS; + } + break; + + case BMP280_STATUS_READ_DATA_REGS: + // parse sensor data, compensate temperature first, then pressure + parse_sensor_data(bmp, &bmp->rx_buffer[0]); + compensate_temperature(bmp); + compensate_pressure(bmp); + bmp->data_available = true; + bmp->status = BMP280_STATUS_READ_STATUS_REG; + break; + + default: + bmp->status = BMP280_STATUS_GET_CALIB; // just to avoid the compiler's warning message + break; + } + if(bmp->bus == BMP280_I2C){ + bmp->i2c.trans.status = I2CTransDone; + } + else{ + bmp->spi.trans.status = SPITransDone; + } + + } else if ((bmp->bus == BMP280_SPI && bmp->spi.trans.status == SPITransFailed) || + (bmp->bus == BMP280_I2C && bmp->i2c.trans.status == I2CTransFailed)) { + /* try again */ + if (!bmp->initialized) { + bmp->status = BMP280_STATUS_UNINIT; + } + if(bmp->bus == BMP280_I2C){ + bmp->i2c.trans.status = I2CTransDone; + } + else{ + bmp->spi.trans.status = SPITransDone; + } + } + + return; +} + +static void parse_sensor_data(struct bmp280_t *bmp, uint8_t *data) +{ + /* Temporary variables to store the sensor data */ + uint32_t data_xlsb; + uint32_t data_lsb; + uint32_t data_msb; + + // BMP280 HAS THE 6 DATA REGISTERS START AT F7 AND GOING UP TO FC MSB FIRST THEN LSB AND LAST THE XLSB BYTE. + // THE FIRST THREE BYTES ARE THE PRESSURE AND THE NEXT 3 THE TEMPERATURE. + /* Store the parsed register values for pressure data */ + data_msb = (uint32_t)data[0] << 16; + data_lsb = (uint32_t)data[1] << 8; + data_xlsb = (uint32_t)data[2]; + bmp->raw_pressure = (int32_t)((data_msb | data_lsb | data_xlsb) >> 4); + + /* Store the parsed register values for temperature data */ + data_msb = (uint32_t)data[3] << 16; + data_lsb = (uint32_t)data[4] << 8; + data_xlsb = (uint32_t)data[5]; + bmp->raw_temperature = (int32_t)((data_msb | data_lsb | data_xlsb) >> 4); +} + + +/** + * @brief This internal API is used to parse the calibration data, compensates + * it and store it in device structure (float version) + */ +static void parse_calib_data(struct bmp280_t *bmp, uint8_t *data) +{ + bmp->calib.dig_t1 = BMP280_CONCAT_BYTES(data[1], data[0]); + bmp->calib.dig_t2 = (int16_t)BMP280_CONCAT_BYTES(data[3], data[2]); + bmp->calib.dig_t3 = (int16_t)BMP280_CONCAT_BYTES(data[5], data[4]); + + bmp->calib.dig_p1 = BMP280_CONCAT_BYTES(data[7], data[6]); + bmp->calib.dig_p2 = (int16_t)BMP280_CONCAT_BYTES(data[9], data[8]); + bmp->calib.dig_p3 = (int16_t)BMP280_CONCAT_BYTES(data[11], data[10]); + bmp->calib.dig_p4 = (int16_t)BMP280_CONCAT_BYTES(data[13], data[12]); + bmp->calib.dig_p5 = (int16_t)BMP280_CONCAT_BYTES(data[15], data[14]); + bmp->calib.dig_p6 = (int16_t)BMP280_CONCAT_BYTES(data[17], data[16]); + bmp->calib.dig_p7 = (int16_t)BMP280_CONCAT_BYTES(data[19], data[18]); + bmp->calib.dig_p8 = (int16_t)BMP280_CONCAT_BYTES(data[21], data[20]); + bmp->calib.dig_p9 = (int16_t)BMP280_CONCAT_BYTES(data[23], data[22]); + + return; +} + +/** + * @brief This internal API is used to compensate the raw temperature data and + * return the compensated temperature data in float data type. + */ +static double compensate_temperature(struct bmp280_t *bmp) +{ + double var1 = (((double)bmp->raw_temperature / 16384.0) - ((double)bmp->calib.dig_t1 / 1024.0)) * ((double)bmp->calib.dig_t2); + double var2 = ((double)bmp->raw_temperature / 131072.0) - ((double)bmp->calib.dig_t1 / 8192.0); + var2 = (var2 * var2) * (double)bmp->calib.dig_t3; + int64_t t_fine = (int64_t)(var1 + var2); + + /* Store t_lin in dev. structure for pressure calculation */ + bmp->calib.t_fine = t_fine; + /* Store compensated temperature in float in structure */ + bmp->temperature = (((var1 + var2) / 5120.f) * 100); + + return (double)bmp->temperature; +} + +/** + * @brief This internal API is used to compensate the raw pressure data and + * return the compensated pressure data in integer data type. + */ +static double compensate_pressure(struct bmp280_t *bmp) +{ + double var1; + double var2; + double p; + + var1 = ((double)bmp->calib.t_fine / 2) - 64000.0; + var2 = (var1 * var1 * (double)bmp->calib.dig_p5) / 32768.0; + var2 = var2 + (var1 * (double)bmp->calib.dig_p5 * 2.0); + var2 = (var2 / 4.0) + ((double)bmp->calib.dig_p4 * 65536.0); + var1 = (((double)bmp->calib.dig_p3 * var1 * (var1 / 524288.0)) + ((double)bmp->calib.dig_p2 * var1)) / 524288.0; + var1 = (1 + (var1 / 32768.0)) * (double)bmp->calib.dig_p1; + p = 1048576.0 - (double)bmp->raw_pressure; + p = (p - (var2 / 4096.0)) * (6250.0 / var1); + var1 = ((double)bmp->calib.dig_p9 * p) * (p / 2147483648.0); + var2 = (p * ((double)bmp->calib.dig_p8)) / 32768.0; + p = p + ((var1 + var2 + (double)bmp->calib.dig_p7) / 16.0); + bmp->pressure = p; + + return (p); +} + +/** + * @brief Configure the BMP280 device register by register + * + * @param bmp The bmp280 instance + * @return true When the configuration is completed + * @return false Still busy configuring + */ +static bool bmp280_config(struct bmp280_t *bmp) { + // Only one transaction can be made per call to the periodic function + switch(bmp->config_idx) { + case 0: + // From datasheet, recommended config for drone usecase: + // osrs_p = 16, osrs_t = 2 + bmp280_register_write(bmp, BMP280_CTRL_MEAS_REG_ADDR, (BMP280_OVERSAMPLING_2X_T | BMP280_OVERSAMPLING_16X_P | BMP280_POWER_NORMAL_MODE)); + bmp->config_idx++; + break; + + case 1: + // IIR filter = 16 + bmp280_register_write(bmp, BMP280_CONFIG_REG_ADDR, (BMP280_INACTIVITY_HALF_MS | BMP280_IIR_FILTER_COEFF_16)); + bmp->config_idx++; + break; + + default: + return true; + } + return false; +} + + +/** + * @brief Write a register with a value + * + * @param bmp The bmp280 instance + * @param reg The register address + * @param value The value to write to the register + */ +static void bmp280_register_write(struct bmp280_t *bmp, uint8_t reg, uint8_t value) { + + bmp->tx_buffer[1] = value; + + /* SPI transaction */ + if(bmp->bus == BMP280_SPI) { + bmp->tx_buffer[0] = (reg & 0x7F); //write command (bit 7 = RW = '0') + bmp->spi.trans.output_length = 2; + bmp->spi.trans.input_length = 0; + spi_submit(bmp->spi.p, &(bmp->spi.trans)); + } + /* I2C transaction */ + else { + bmp->tx_buffer[0] = reg; + i2c_transmit(bmp->i2c.p, &(bmp->i2c.trans), bmp->i2c.slave_addr, 2); + } +} + +/** + * @brief Read a register + * + * @param bmp The bmp280 instance + * @param reg The register address + * @param size The size to read (already 1 is added for the transmission of the register to read) + */ +static void bmp280_register_read(struct bmp280_t *bmp, uint8_t reg, uint16_t size) { + + bmp->tx_buffer[0] = reg | BMP280_READ_FLAG; + /* SPI transaction */ + if(bmp->bus == BMP280_SPI) { + bmp->spi.trans.output_length = 2; + bmp->spi.trans.input_length = size+1; + bmp->tx_buffer[1] = 0; + spi_submit(bmp->spi.p, &(bmp->spi.trans)); + } + /* I2C transaction */ + else { + i2c_transceive(bmp->i2c.p, &(bmp->i2c.trans), bmp->i2c.slave_addr, 1, size); + } +} + diff --git a/sw/airborne/peripherals/bmp280.h b/sw/airborne/peripherals/bmp280.h new file mode 100644 index 0000000000..9f65d5fe83 --- /dev/null +++ b/sw/airborne/peripherals/bmp280.h @@ -0,0 +1,118 @@ +/* + * Chris Efstathiou hendrixgr@gmail.com + * Florian Sansou florian.sansou@enac.fr + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file peripherals/bmp280.h + * @brief Sensor driver for BMP280 sensor + * + * + */ + +#ifndef BMP280_H +#define BMP280_H + +/* Header includes */ +#include "peripherals/bmp280_regs.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/spi.h" + +struct bmp280_i2c_t { + struct i2c_periph *p; ///< Peripheral device for communication + struct i2c_transaction trans; ///< Transaction used during configuration and measurements + uint8_t slave_addr; ///< The I2C slave address on the bus +}; + +struct bmp280_spi_t { + struct spi_periph *p; ///< Peripheral device for communication + uint8_t slave_idx; ///< Slave index used for Slave Select + struct spi_transaction trans; ///< Transaction used during configuration and measurements + + uint8_t tx_buf[50]; ///< Transmit buffer + uint8_t rx_buf[50]; //< Receive buffer +}; + +/* Possible communication busses for the bmp280 driver */ +enum bmp280_bus_t { + BMP280_SPI, + BMP280_I2C +}; + +enum bmp_device_t{ + BMP_UNKOWN, + BMP_280 +}; + +/* Different states the bmp driver can be in */ +enum bmp280_status_t { + BMP280_STATUS_UNINIT, + BMP280_STATUS_IDLE, + BMP280_STATUS_GET_CALIB, + BMP280_STATUS_CONFIGURE, + BMP280_STATUS_READ_STATUS_REG, + BMP280_STATUS_READ_DATA_REGS +}; + +// brief Register Trim Variables +struct bmp280_reg_calib_data_t { + uint16_t dig_t1; + int16_t dig_t2; + int16_t dig_t3; + uint16_t dig_p1; + int16_t dig_p2; + int16_t dig_p3; + int16_t dig_p4; + int16_t dig_p5; + int16_t dig_p6; + int16_t dig_p7; + int16_t dig_p8; + int16_t dig_p9; + int64_t t_fine; +}; + + +struct bmp280_t { + enum bmp280_status_t status; ///< state machine status + enum bmp_device_t device; ///< The device type detected + bool initialized; ///< config done flag + volatile bool data_available; ///< data ready flag + struct bmp280_reg_calib_data_t calib; ///< calibration data + uint32_t raw_pressure; ///< uncompensated pressure + uint32_t raw_temperature; ///< uncompensated temperature + float pressure; ///< pressure in Pascal + float temperature; ///< temperature in deg Celcius + enum bmp280_bus_t bus; ///< The communication bus used to connect the device SPI/I2C + union { + struct bmp280_spi_t spi; ///< SPI specific configuration + struct bmp280_i2c_t i2c; ///< I2C specific configuration + }; + uint8_t* rx_buffer; + uint8_t* tx_buffer; + uint16_t* rx_length; + + uint8_t config_idx; ///< The current configuration index +}; + + +extern void bmp280_read_eeprom_calib(struct bmp280_t *bmp); +extern void bmp280_init(struct bmp280_t *bmp); +extern void bmp280_periodic(struct bmp280_t *bmp); +extern void bmp280_event(struct bmp280_t *bmp); + +#endif /* BMP280_H */ diff --git a/sw/airborne/peripherals/bmp280_i2c.c b/sw/airborne/peripherals/bmp280_i2c.c deleted file mode 100644 index bda0f0e907..0000000000 --- a/sw/airborne/peripherals/bmp280_i2c.c +++ /dev/null @@ -1,248 +0,0 @@ -/* - * Chris Efstathiou hendrixgr@gmail.com - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file peripherals/bmp280_i2c.c - * @brief Sensor driver for BMP280 sensor via I2C - * - * - */ - -#include "peripherals/bmp280_i2c.h" - -/** local function to extract raw data from i2c buffer - * and compute compensation with selected precision - */ -static void parse_sensor_data(struct Bmp280_I2c *bmp); -static void parse_calib_data(struct Bmp280_I2c *bmp); -PRINT_CONFIG_MSG("BMP280 uses double precision compensation") -static double compensate_pressure(struct Bmp280_I2c *bmp); -static double compensate_temperature(struct Bmp280_I2c *bmp); - -int64_t t_fine; - -// init function -void bmp280_i2c_init(struct Bmp280_I2c *bmp, struct i2c_periph *i2c_p, uint8_t addr) -{ - // set i2c_peripheral - bmp->i2c_p = i2c_p; - - // slave address - bmp->i2c_trans.slave_addr = addr; - // set initial status: Done - bmp->i2c_trans.status = I2CTransDone; - - bmp->data_available = false; - bmp->initialized = false; - bmp->status = BMP280_STATUS_UNINIT; -} - -// Start new measurement if sensor ready -void bmp280_i2c_periodic(struct Bmp280_I2c *bmp) -{ - if (bmp->i2c_trans.status != I2CTransDone) { - return; // transaction not finished - } - - switch (bmp->status) { - case BMP280_STATUS_UNINIT: - bmp->data_available = false; - bmp->initialized = false; - bmp->status = BMP280_STATUS_GET_CALIB; - break; - - case BMP280_STATUS_GET_CALIB: - // request calibration data - bmp->i2c_trans.buf[0] = BMP280_CALIB_LSB_DATA_ADDR; - i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, BMP280_CALIB_DATA_LEN); - break; - - case BMP280_STATUS_CONFIGURE: - // From datasheet, recommended config for drone usecase: - // osrs_p = 8, osrs_t = 1 - // IIR filter = 2 (note: this one doesn't exist...) - bmp->i2c_trans.buf[0] = BMP280_CTRL_MEAS_REG_ADDR; - bmp->i2c_trans.buf[1] = (BMP280_OVERSAMPLING_1X_T | BMP280_OVERSAMPLING_8X_P | BMP280_POWER_NORMAL_MODE); - bmp->i2c_trans.buf[2] = BMP280_CONFIG_REG_ADDR; - bmp->i2c_trans.buf[3] = (BMP280_INACTIVITY_62_5_MS | BMP280_IIR_FILTER_COEFF_2); - i2c_transmit(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, (BMP280_CONFIG_LEN * 2)); - break; - - case BMP280_STATUS_READ_STATUS_REG: - // READ THE STATUS BYTE - bmp->i2c_trans.buf[0] = BMP280_STATUS_REG_ADDR; - i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, 1); - break; - - case BMP280_STATUS_READ_DATA_REGS: - // READ ALL 6 DATA REGISTERS - bmp->i2c_trans.buf[0] = BMP280_DATA_START_REG_ADDR; - i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, BMP280_P_T_DATA_LEN); - break; - - default: - break; - } -} - -void bmp280_i2c_event(struct Bmp280_I2c *bmp) -{ - if (bmp->i2c_trans.status == I2CTransSuccess) { - switch (bmp->status) { - - case BMP280_STATUS_GET_CALIB: - // compute calib - parse_calib_data(bmp); - bmp->status = BMP280_STATUS_CONFIGURE; - break; - - case BMP280_STATUS_CONFIGURE: - // nothing else to do, start reading - bmp->status = BMP280_STATUS_READ_STATUS_REG; - bmp->initialized = true; - break; - - case BMP280_STATUS_READ_STATUS_REG: - // check status byte - if ((bmp->i2c_trans.buf[0] & (BMP280_EOC_BIT | BMP280_NVRAM_COPY_BIT)) == 0) { - bmp->status = BMP280_STATUS_READ_DATA_REGS; - } - break; - - case BMP280_STATUS_READ_DATA_REGS: - // parse sensor data, compensate temperature first, then pressure - parse_sensor_data(bmp); - compensate_temperature(bmp); - compensate_pressure(bmp); - bmp->data_available = true; - bmp->status = BMP280_STATUS_READ_STATUS_REG; - break; - - default: - bmp->status = BMP280_STATUS_GET_CALIB; // just to avoid the compiler's warning message - break; - } - bmp->i2c_trans.status = I2CTransDone; - } else if (bmp->i2c_trans.status == I2CTransFailed) { - /* try again */ - if (!bmp->initialized) { - bmp->status = BMP280_STATUS_UNINIT; - } - bmp->i2c_trans.status = I2CTransDone; - } - - return; -} - -static void parse_sensor_data(struct Bmp280_I2c *bmp) -{ - /* Temporary variables to store the sensor data */ - uint32_t data_xlsb; - uint32_t data_lsb; - uint32_t data_msb; - - // BMP280 HAS THE 6 DATA REGISTERS START AT F7 AND GOING UP TO FC MSB FIRST THEN LSB AND LAST THE XLSB BYTE. - // THE FIRST THREE BYTES ARE THE PRESSURE AND THE NEXT 3 THE TEMPERATURE. - /* Store the parsed register values for pressure data */ - data_msb = (uint32_t)bmp->i2c_trans.buf[0] << 16; - data_lsb = (uint32_t)bmp->i2c_trans.buf[1] << 8; - data_xlsb = (uint32_t)bmp->i2c_trans.buf[2]; - bmp->raw_pressure = (int32_t)((data_msb | data_lsb | data_xlsb) >> 4); - - /* Store the parsed register values for temperature data */ - data_msb = (uint32_t)bmp->i2c_trans.buf[3] << 16; - data_lsb = (uint32_t)bmp->i2c_trans.buf[4] << 8; - data_xlsb = (uint32_t)bmp->i2c_trans.buf[5]; - bmp->raw_temperature = (int32_t)((data_msb | data_lsb | data_xlsb) >> 4); -} - - -/** - * @brief This internal API is used to parse the calibration data, compensates - * it and store it in device structure (float version) - */ -static void parse_calib_data(struct Bmp280_I2c *bmp) -{ - uint8_t *data = (uint8_t *)bmp->i2c_trans.buf; // we know that this buffer will not be modified during this call - - bmp->calib.dig_t1 = BMP280_CONCAT_BYTES(data[1], data[0]); - bmp->calib.dig_t2 = (int16_t)BMP280_CONCAT_BYTES(data[3], data[2]); - bmp->calib.dig_t3 = (int16_t)BMP280_CONCAT_BYTES(data[5], data[4]); - - bmp->calib.dig_p1 = BMP280_CONCAT_BYTES(data[7], data[6]); - bmp->calib.dig_p2 = (int16_t)BMP280_CONCAT_BYTES(data[9], data[8]); - bmp->calib.dig_p3 = (int16_t)BMP280_CONCAT_BYTES(data[11], data[10]); - bmp->calib.dig_p4 = (int16_t)BMP280_CONCAT_BYTES(data[13], data[12]); - bmp->calib.dig_p5 = (int16_t)BMP280_CONCAT_BYTES(data[15], data[14]); - bmp->calib.dig_p6 = (int16_t)BMP280_CONCAT_BYTES(data[17], data[16]); - bmp->calib.dig_p7 = (int16_t)BMP280_CONCAT_BYTES(data[19], data[18]); - bmp->calib.dig_p8 = (int16_t)BMP280_CONCAT_BYTES(data[21], data[20]); - bmp->calib.dig_p9 = (int16_t)BMP280_CONCAT_BYTES(data[23], data[22]); - - return; -} - -/** - * @brief This internal API is used to compensate the raw temperature data and - * return the compensated temperature data in float data type. - */ -static double compensate_temperature(struct Bmp280_I2c *bmp) -{ - - double var1; - double var2; - - var1 = (((double)bmp->raw_temperature / 16384.0) - ((double)bmp->calib.dig_t1 / 1024.0)) * ((double)bmp->calib.dig_t2); - var2 = ((double)bmp->raw_temperature / 131072.0) - ((double)bmp->calib.dig_t1 / 8192.0); - var2 = (var2 * var2) * (double)bmp->calib.dig_t3; - t_fine = (int64_t)(var1 + var2); - - /* Store t_lin in dev. structure for pressure calculation */ - bmp->calib.t_fine = t_fine; - /* Store compensated temperature in float in structure */ - bmp->temperature = (((var1 + var2) / 5120.f) * 100); - - return (double)bmp->temperature; -} - -/** - * @brief This internal API is used to compensate the raw pressure data and - * return the compensated pressure data in integer data type. - */ -static double compensate_pressure(struct Bmp280_I2c *bmp) -{ - double var1; - double var2; - double p; - - var1 = ((double)t_fine / 2) - 64000.0; - var2 = (var1 * var1 * (double)bmp->calib.dig_p5) / 32768.0; - var2 = var2 + (var1 * (double)bmp->calib.dig_p5 * 2.0); - var2 = (var2 / 4.0) + ((double)bmp->calib.dig_p4 * 65536.0); - var1 = (((double)bmp->calib.dig_p3 * var1 * (var1 / 524288.0)) + ((double)bmp->calib.dig_p2 * var1)) / 524288.0; - var1 = (1 + (var1 / 32768.0)) * (double)bmp->calib.dig_p1; - p = 1048576.0 - (double)bmp->raw_pressure; - p = (p - (var2 / 4096.0)) * (6250.0 / var1); - var1 = ((double)bmp->calib.dig_p9 * p) * (p / 2147483648.0); - var2 = (p * ((double)bmp->calib.dig_p8)) / 32768.0; - p = p + ((var1 + var2 + (double)bmp->calib.dig_p7) / 16.0); - bmp->pressure = p; - - return (p); -} diff --git a/sw/airborne/peripherals/bmp280_i2c.h b/sw/airborne/peripherals/bmp280_i2c.h deleted file mode 100644 index 3fc45e00db..0000000000 --- a/sw/airborne/peripherals/bmp280_i2c.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Chris Efstathiou hendrixgr@gmail.com - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file peripherals/bmp280_i2c.h - * @brief Sensor driver for BMP280 sensor via I2C - * - * - */ - -#ifndef BMP280_I2C_H -#define BMP280_I2C_H - -/* Header includes */ -#include "peripherals/bmp280_regs.h" -#include "mcu_periph/i2c.h" - - -struct Bmp280_I2c { - struct i2c_periph *i2c_p; - struct i2c_transaction i2c_trans; - enum Bmp280Status status; ///< state machine status - bool initialized; ///< config done flag - volatile bool data_available; ///< data ready flag - struct bmp280_reg_calib_data calib; ///< calibration data - uint32_t raw_pressure; ///< uncompensated pressure - uint32_t raw_temperature; ///< uncompensated temperature - float pressure; ///< pressure in Pascal - float temperature; ///< temperature in deg Celcius -}; - -extern void bmp280_i2c_read_eeprom_calib(struct Bmp280_I2c *bmp); -extern void bmp280_i2c_init(struct Bmp280_I2c *bmp, struct i2c_periph *i2c_p, uint8_t addr); -extern void bmp280_i2c_periodic(struct Bmp280_I2c *bmp); -extern void bmp280_i2c_event(struct Bmp280_I2c *bmp); - -#endif /* BMP280_I2C_H */ diff --git a/sw/airborne/peripherals/bmp280_regs.h b/sw/airborne/peripherals/bmp280_regs.h index 9cd2871b16..a21ef10ade 100644 --- a/sw/airborne/peripherals/bmp280_regs.h +++ b/sw/airborne/peripherals/bmp280_regs.h @@ -33,33 +33,33 @@ #include "std.h" +#define BMP280_READ_FLAG 0x80 + // I2C addresses (8 bits) #define BMP280_I2C_ADDR 0xEC #define BMP280_I2C_ADDR_ALT 0xEE -/**\name BMP280 chip identifier */ -#define BMP280_CHIP_ID 0x50 // CALIBRATION REGISTER ADDRESSES #define BMP280_CALIB_LSB_DATA_ADDR 0x88 #define BMP280_CALIB_DATA_LEN 24 #define BMP280_DIG_T1_UINT 0x88 -#define BMP280_DIG_T2_INT 0x8A -#define BMP280_DIG_T3_INT 0x8C +#define BMP280_DIG_T2_INT 0x8A +#define BMP280_DIG_T3_INT 0x8C #define BMP280_DIG_P1_UINT 0x8E #define BMP280_DIG_P2_INT 0x90 -#define BMP280_DIG_P3_INT 0x92 -#define BMP280_DIG_P4_INT 0x94 +#define BMP280_DIG_P3_INT 0x92 +#define BMP280_DIG_P4_INT 0x94 #define BMP280_DIG_P5_INT 0x96 #define BMP280_DIG_P6_INT 0x98 -#define BMP280_DIG_P7_INT 0x9A -#define BMP280_DIG_P8_INT 0x9C +#define BMP280_DIG_P7_INT 0x9A +#define BMP280_DIG_P8_INT 0x9C #define BMP280_DIG_P9_INT 0x9E // CONTROL AND VALUES REGISTER ADDRESSES -#define BMP280_CONFIG_ADDR 0xF4 -#define BMP280_CONFIG_LEN 0x02 +#define BMP280_CONFIG_ADDR 0xF4 +#define BMP280_CONFIG_LEN 0x02 #define BMP280_DATA_START_REG_ADDR 0xF7 #define BMP280_P_T_DATA_LEN 6 #define BMP280_P_DATA_LEN 3 @@ -77,13 +77,14 @@ #define BMP280_T_LSB_REG_ADDR 0xFB #define BMP280_T_XLSB_REG_ADDR 0xFC -// BMP280 ID -#define BMP280_ID_NB 0x58 -//BMP280 RESET COMMAND VALUE -#define BMP280_RESET_VAL 0xB6 -#define BMP280_EOC_BIT (1<<3) -#define BMP280_NVRAM_COPY_BIT (1<<0) +// BMP280 ID +#define BMP280_ID_NB 0x58 +//BMP280 RESET COMMAND VALUE +#define BMP280_RESET_VAL 0xB6 + +#define BMP280_EOC_BIT (1<<3) +#define BMP280_NVRAM_COPY_BIT (1<<0) // BMP280 CONTROL MEASUREMENT REGISTER BIT VALUES. #define BMP280_NO_OVERSAMPLING_T (0x00<<5) @@ -142,33 +143,6 @@ #define BMP280_COMPENSATION BMP280_DOUBLE_PRECISION_COMPENSATION #endif -/** - * @brief Status enum - */ -enum Bmp280Status { - BMP280_STATUS_UNINIT, - BMP280_STATUS_GET_CALIB, - BMP280_STATUS_CONFIGURE, - BMP280_STATUS_READ_STATUS_REG, - BMP280_STATUS_READ_DATA_REGS -}; - -// brief Register Trim Variables -struct bmp280_reg_calib_data { - uint16_t dig_t1; - int16_t dig_t2; - int16_t dig_t3; - uint16_t dig_p1; - int16_t dig_p2; - int16_t dig_p3; - int16_t dig_p4; - int16_t dig_p5; - int16_t dig_p6; - int16_t dig_p7; - int16_t dig_p8; - int16_t dig_p9; - int32_t t_fine; -}; #endif /* BMP280_REGS_H */ diff --git a/sw/airborne/peripherals/hmc58xx.c b/sw/airborne/peripherals/hmc58xx.c index 3db8446110..6a7ff03d24 100644 --- a/sw/airborne/peripherals/hmc58xx.c +++ b/sw/airborne/peripherals/hmc58xx.c @@ -23,7 +23,7 @@ /** * @file peripherals/hmc58xx.c * - * Driver for Honeywell HMC5843 and HMC5883 magnetometers. + * Driver for Honeywell HMC5843, HMC5883 and HMC5983 magnetometers. * @todo DRDY/IRQ handling */ @@ -45,6 +45,12 @@ #ifndef HMC58XX_DEFAULT_MD #define HMC58XX_DEFAULT_MD 0x0 // Continious measurement mode #endif +#ifndef HMC58XX_DEFAULT_SA +#define HMC58XX_DEFAULT_SA 0x0 // Number of samples averaged +#endif +#ifndef HMC58XX_DEFAULT_TC +#define HMC58XX_DEFAULT_TC 0x0 // Automatic compensation of sensitivity over temperature +#endif /** HMC58XX startup delay * @@ -64,6 +70,8 @@ static void hmc58xx_set_default_config(struct Hmc58xxConfig *c) c->meas = HMC58XX_DEFAULT_MS; c->gain = HMC58XX_DEFAULT_GN; c->mode = HMC58XX_DEFAULT_MD; + c->samples_averaged = HMC58XX_DEFAULT_SA; + c->temp_comp = HMC58XX_DEFAULT_TC; } /** @@ -102,7 +110,7 @@ static void hmc58xx_send_config(struct Hmc58xx *hmc) { switch (hmc->init_status) { case HMC_CONF_CRA: - hmc58xx_i2c_tx_reg(hmc, HMC58XX_REG_CFGA, (hmc->config.rate << 2) | (hmc->config.meas)); + hmc58xx_i2c_tx_reg(hmc, HMC58XX_REG_CFGA, (hmc->config.temp_comp << 7) | (hmc->config.samples_averaged << 5) | (hmc->config.rate << 2) | (hmc->config.meas)); hmc->init_status++; break; case HMC_CONF_CRB: diff --git a/sw/airborne/peripherals/hmc58xx.h b/sw/airborne/peripherals/hmc58xx.h index c91cd9f004..d3716d2ba0 100644 --- a/sw/airborne/peripherals/hmc58xx.h +++ b/sw/airborne/peripherals/hmc58xx.h @@ -23,7 +23,7 @@ /** * @file peripherals/hmc58xx.c * - * Driver for Honeywell HMC5843 and HMC5883 magnetometers. + * Driver for Honeywell HMC5843, HMC5883 and HMC5983 magnetometers. */ #ifndef HMC58XX_H @@ -37,10 +37,12 @@ #include "peripherals/hmc58xx_regs.h" struct Hmc58xxConfig { - uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883) - uint8_t meas; ///< Measurement configuration - uint8_t gain; ///< Gain configuration (1 -> +- 1 Gauss) - uint8_t mode; ///< Measurement mode + uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883) + uint8_t meas; ///< Measurement configuration + uint8_t gain; ///< Gain configuration (1 -> +- 1 Gauss) + uint8_t mode; ///< Measurement mode + uint8_t samples_averaged; ///< Number of samples averaged per measurement output. + uint8_t temp_comp; ///< Enable temperature sensor (only on HMC5983) }; /** config status states */ diff --git a/sw/airborne/peripherals/hmc58xx_regs.h b/sw/airborne/peripherals/hmc58xx_regs.h index 7eb9a1aa2f..e3cb21ab98 100644 --- a/sw/airborne/peripherals/hmc58xx_regs.h +++ b/sw/airborne/peripherals/hmc58xx_regs.h @@ -21,7 +21,7 @@ /** * @file peripherals/hmc58xx_regs.h - * Register defs for Honeywell HMC5843 and HMC5883 magnetometers. + * Register defs for Honeywell HMC5843, HMC5883 and HMC5983 magnetometers. */ #ifndef HMC58XX_REGS_H diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h index f382bcc0c2..97a88c0dee 100644 --- a/sw/airborne/peripherals/mpu60x0_regs.h +++ b/sw/airborne/peripherals/mpu60x0_regs.h @@ -117,6 +117,7 @@ // Different sensor WHOAMI replies #define MPU60X0_REG_WHO_AM_I 0x75 #define MPU60X0_WHOAMI_REPLY 0x68 +#define MPU6500_WHOAMI_REPLY 0x70 #define ICM20600_WHOAMI_REPLY 0x11 #define ICM20608_WHOAMI_REPLY 0xAF #define ICM20602_WHOAMI_REPLY 0x12 diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c index e1dfda4e9e..b05b3375bd 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.c +++ b/sw/airborne/peripherals/mpu60x0_spi.c @@ -84,9 +84,10 @@ void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu) mpu->rx_buf[1] == ICM20600_WHOAMI_REPLY || mpu->rx_buf[1] == ICM20608_WHOAMI_REPLY || mpu->rx_buf[1] == ICM20602_WHOAMI_REPLY || - mpu->rx_buf[1] == ICM20689_WHOAMI_REPLY)) { + mpu->rx_buf[1] == ICM20689_WHOAMI_REPLY || + mpu->rx_buf[1] == MPU6500_WHOAMI_REPLY)) { - if (mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY) { + if (mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY || mpu->rx_buf[1] == MPU6500_WHOAMI_REPLY) { mpu->config.type = MPU60X0; } else if (mpu->rx_buf[1] == ICM20600_WHOAMI_REPLY) { mpu->config.type = ICM20600;