diff --git a/mk/source.mk b/mk/source.mk index 3193f786e53..65286356953 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -365,6 +365,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/compass/compass_lis3mdl.c \ + drivers/compass/compass_ist8310.c \ drivers/display_ug2864hsweg01.c \ drivers/inverter.c \ drivers/light_ws2811strip.c \ diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 353241c8ace..99b1b367e2d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -157,7 +157,7 @@ const char * const lookupTableBaroHardware[] = { #if defined(USE_SENSOR_NAMES) || defined(USE_MAG) // sync with magSensor_e const char * const lookupTableMagHardware[] = { - "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883", "LIS3MDL", "MAG_MPU925X_AK8963" + "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883", "LIS3MDL", "MPU925X_AK8963", "IST8310" }; #endif #if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER) diff --git a/src/main/drivers/at32/bus_i2c_atbsp.c b/src/main/drivers/at32/bus_i2c_atbsp.c index 7b511585df6..26a94e409a5 100644 --- a/src/main/drivers/at32/bus_i2c_atbsp.c +++ b/src/main/drivers/at32/bus_i2c_atbsp.c @@ -219,7 +219,7 @@ bool i2cReadBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, u i2c_status_type status; - status = i2c_memory_read_int(pHandle,I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_,buf, len,I2C_TIMEOUT); + status = i2c_memory_read_int(pHandle, I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_, buf, len, I2C_TIMEOUT); if (status != I2C_OK) { i2c_wait_flag(pHandle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_TIMEOUT); diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c new file mode 100644 index 00000000000..9f716b9fbd8 --- /dev/null +++ b/src/main/drivers/compass/compass_ist8310.c @@ -0,0 +1,198 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight 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 this software. + * + * If not, see . + */ + +#include +#include + +#include + +#include "platform.h" + +#ifdef USE_MAG_IST8310 + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" + +#include "drivers/bus.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_busdev.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +#include "compass.h" +#include "compass_ist8310.h" + +//#define DEBUG_MAG_DATA_READY_INTERRUPT + +#define IST8310_MAG_I2C_ADDRESS 0x0C + +/* ist8310 Slave Address Select : default address 0x0C + * CAD1 | CAD0 | Address + * ------------------------------ + * VSS | VSS | 0CH + * VSS | VDD | 0DH + * VDD | VSS | 0EH + * VDD | VDD | 0FH + * if CAD1 and CAD0 are floating, I2C address will be 0EH + * + * + * CTRL_REGA: Control Register 1 + * Read Write + * Default value: 0x0A + * 7:4 0 Reserved. + * 3:0 DO2-DO0: Operating mode setting + * DO3 | DO2 | DO1 | DO0 | mode + * ------------------------------------------------------ + * 0 | 0 | 0 | 0 | Stand-By mode + * 0 | 0 | 0 | 1 | Single measurement mode + * Others: Reserved + * + * CTRL_REGB: Control Register 2 + * Read Write + * Default value: 0x0B + * 7:4 0 Reserved. + * 3 DREN : Data ready enable control: + * 0: disable + * 1: enable + * 2 DRP: DRDY pin polarity control + * 0: active low + * 1: active high + * 1 0 Reserved. + * 0 SRST: Soft reset, perform Power On Reset (POR) routine + * 0: no action + * 1: start immediately POR routine + * This bit will be set to zero after POR routine + */ + +#define IST8310_REG_DATA 0x03 +#define IST8310_REG_WAI 0x00 +#define IST8310_REG_WAI_VALID 0x10 + +#define IST8310_REG_STAT1 0x02 +#define IST8310_REG_STAT2 0x09 + +#define IST8310_DRDY_MASK 0x01 + +// I2C Control Register +#define IST8310_REG_CNTRL1 0x0A +#define IST8310_REG_CNTRL2 0x0B +#define IST8310_REG_AVERAGE 0x41 +#define IST8310_REG_PDCNTL 0x42 + +// Parameter +// ODR = Output Data Rate, we use single measure mode for getting more data. +#define IST8310_ODR_SINGLE 0x01 +#define IST8310_ODR_10_HZ 0x03 +#define IST8310_ODR_20_HZ 0x05 +#define IST8310_ODR_50_HZ 0x07 +#define IST8310_ODR_100_HZ 0x06 + +#define IST8310_AVG_16 0x24 +#define IST8310_PULSE_DURATION_NORMAL 0xC0 + +#define IST8310_CNTRL2_RESET 0x01 +#define IST8310_CNTRL2_DRPOL 0x04 +#define IST8310_CNTRL2_DRENA 0x08 + +static bool ist8310Init(magDev_t *magDev) +{ + extDevice_t *dev = &magDev->dev; + + busDeviceRegister(dev); + + // Init setting + bool ack = busWriteRegister(dev, IST8310_REG_AVERAGE, IST8310_AVG_16); + delay(6); + ack = ack && busWriteRegister(dev, IST8310_REG_PDCNTL, IST8310_PULSE_DURATION_NORMAL); + delay(6); + ack = ack && busWriteRegister(dev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE); + + return ack; +} + +static bool ist8310Read(magDev_t * magDev, int16_t *magData) +{ + extDevice_t *dev = &magDev->dev; + + static uint8_t buf[6]; + const int LSB2FSV = 3; // 3mG - 14 bit + + static enum { + STATE_REQUEST_DATA, + STATE_FETCH_DATA, + } state = STATE_REQUEST_DATA; + + switch (state) { + default: + case STATE_REQUEST_DATA: + busReadRegisterBufferStart(dev, IST8310_REG_DATA, buf, sizeof(buf)); + + state = STATE_FETCH_DATA; + + return false; + case STATE_FETCH_DATA: + // Looks like datasheet is incorrect and we need to invert Y axis to conform to right hand rule + magData[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV; + magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV; + magData[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV; + + // Force single measurement mode for next read + busWriteRegisterStart(dev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE); + + state = STATE_REQUEST_DATA; + + return true; + } + + // TODO: do cross axis compensation + + return false; +} + +static bool deviceDetect(magDev_t * magDev) +{ + uint8_t result = 0; + bool ack = busReadRegisterBuffer(&magDev->dev, IST8310_REG_WAI, &result, 1); + + return ack && result == IST8310_REG_WAI_VALID; +} + +bool ist8310Detect(magDev_t * magDev) +{ + extDevice_t *dev = &magDev->dev; + + if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) { + dev->busType_u.i2c.address = IST8310_MAG_I2C_ADDRESS; + } + + if (deviceDetect(magDev)) { + magDev->init = ist8310Init; + magDev->read = ist8310Read; + + return true; + } + + return false; +} + +#endif diff --git a/src/main/drivers/compass/compass_ist8310.h b/src/main/drivers/compass/compass_ist8310.h new file mode 100644 index 00000000000..452c0a38107 --- /dev/null +++ b/src/main/drivers/compass/compass_ist8310.h @@ -0,0 +1,26 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight 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 this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/io_types.h" + +bool ist8310Detect(magDev_t *mag); diff --git a/src/main/drivers/stm32/bus_i2c_hal.c b/src/main/drivers/stm32/bus_i2c_hal.c index 612181527ef..bdb3c5fd39c 100644 --- a/src/main/drivers/stm32/bus_i2c_hal.c +++ b/src/main/drivers/stm32/bus_i2c_hal.c @@ -196,7 +196,7 @@ bool i2cReadBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, u HAL_StatusTypeDef status; - status = HAL_I2C_Mem_Read_IT(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len); + status = HAL_I2C_Mem_Read_IT(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, buf, len); if (status == HAL_BUSY) { return false; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index bcdc94eee49..d49402a4c92 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -42,6 +42,7 @@ #include "drivers/compass/compass_lis3mdl.h" #include "drivers/compass/compass_mpu925x_ak8963.h" #include "drivers/compass/compass_qmc5883l.h" +#include "drivers/compass/compass_ist8310.h" #include "drivers/io.h" #include "drivers/light_led.h" @@ -92,7 +93,7 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig) compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN); compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; -#elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))) +#elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || defined(USE_MAG_IST8310) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))) compassConfig->mag_busType = BUS_TYPE_I2C; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE); compassConfig->mag_i2c_address = MAG_I2C_ADDRESS; @@ -271,6 +272,22 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment) #endif FALLTHROUGH; + case MAG_IST8310: +#ifdef USE_MAG_IST8310 + if (dev->bus->busType == BUS_TYPE_I2C) { + dev->busType_u.i2c.address = compassConfig()->mag_i2c_address; + } + + if (ist8310Detect(magDev)) { +#ifdef MAG_IST8310_ALIGN + *alignment = MAG_IST8310_ALIGN; +#endif + magHardware = MAG_IST8310; + break; + } +#endif + FALLTHROUGH; + case MAG_NONE: magHardware = MAG_NONE; break; @@ -279,7 +296,7 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment) // MAG_MPU925X_AK8963 is an MPU925x configured as I2C passthrough to the built-in AK8963 magnetometer // Passthrough mode disables the gyro/acc part of the MPU, so we only want to detect this sensor if mag_hardware was explicitly set to MAG_MPU925X_AK8963 #ifdef USE_MAG_MPU925X_AK8963 - if(compassConfig()->mag_hardware == MAG_MPU925X_AK8963){ + if (compassConfig()->mag_hardware == MAG_MPU925X_AK8963){ if (mpu925Xak8963CompassDetect(magDev)) { magHardware = MAG_MPU925X_AK8963; } else { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index f040c03b542..30e0dd79142 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -37,7 +37,8 @@ typedef enum { MAG_AK8963 = 4, MAG_QMC5883 = 5, MAG_LIS3MDL = 6, - MAG_MPU925X_AK8963 = 7 + MAG_MPU925X_AK8963 = 7, + MAG_IST8310 = 8 } magSensor_e; typedef struct mag_s { diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 248847bca6d..12666423f98 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -114,6 +114,9 @@ #ifndef USE_MAG_AK8975 #define USE_MAG_AK8975 #endif +#ifndef USE_MAG_IST8310 +#define USE_MAG_IST8310 +#endif #endif // END MAG HW defines