[IMU Driver]: Add support for ASM330LHH IMU (#35)
* add support for asm330lhh imu * disable ghpf
This commit is contained in:
parent
ed99dbcd26
commit
2178d47965
|
@ -232,6 +232,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
common/typeconversion.c \
|
||||
drivers/accgyro/accgyro_mpu.c \
|
||||
drivers/accgyro/accgyro_mpu3050.c \
|
||||
drivers/accgyro/accgyro_spi_asm330lhh.c \
|
||||
drivers/accgyro/accgyro_spi_bmi160.c \
|
||||
drivers/accgyro/accgyro_spi_bmi270.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6ds3.c \
|
||||
|
@ -375,6 +376,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
|
|||
drivers/accgyro/accgyro_spi_mpu9250.c \
|
||||
drivers/accgyro/accgyro_spi_icm20689.c \
|
||||
drivers/accgyro/accgyro_spi_icm426xx.c \
|
||||
drivers/accgyro/accgyro_spi_asm330lhh_init.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6ds3_init.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl_init.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
|
||||
|
|
|
@ -136,14 +136,14 @@
|
|||
const char * const lookupTableAccHardware[] = {
|
||||
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
|
||||
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
|
||||
"BMI160", "BMI270", "LSM6DS3", "LSM6DSL", "LSM6DSO", "QMI8658", "SH3001", "FAKE"
|
||||
"BMI160", "BMI270", "ASM330LHH", "LSM6DS3", "LSM6DSL", "LSM6DSO", "QMI8658", "SH3001", "FAKE"
|
||||
};
|
||||
|
||||
// sync with gyroHardware_e
|
||||
const char * const lookupTableGyroHardware[] = {
|
||||
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
|
||||
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
|
||||
"BMI160", "BMI270", "LSM6DS3", "LSM6DSL", "LSM6DSO", "QMI8658", "SH3001", "FAKE"
|
||||
"BMI160", "BMI270", "ASM330LHH", "LSM6DS3", "LSM6DSL", "LSM6DSO", "QMI8658", "SH3001", "FAKE"
|
||||
};
|
||||
|
||||
#if defined(USE_SENSOR_NAMES) || defined(USE_BARO)
|
||||
|
|
|
@ -59,6 +59,7 @@ typedef enum {
|
|||
GYRO_ICM42688P,
|
||||
GYRO_BMI160,
|
||||
GYRO_BMI270,
|
||||
GYRO_ASM330LHH,
|
||||
GYRO_LSM6DS3,
|
||||
GYRO_LSM6DSL,
|
||||
GYRO_LSM6DSO,
|
||||
|
|
|
@ -51,6 +51,7 @@
|
|||
#include "drivers/accgyro/accgyro_spi_icm20649.h"
|
||||
#include "drivers/accgyro/accgyro_spi_icm20689.h"
|
||||
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
|
||||
#include "drivers/accgyro/accgyro_spi_asm330lhh.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6dsl.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
|
||||
|
@ -359,6 +360,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
|
|||
#ifdef USE_GYRO_SPI_ICM20689
|
||||
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
|
||||
#endif
|
||||
#ifdef USE_ACCGYRO_ASM330LHH
|
||||
asm330lhhDetect,
|
||||
#endif
|
||||
#ifdef USE_ACCGYRO_LSM6DS3
|
||||
lsm6ds3Detect,
|
||||
#endif
|
||||
|
|
|
@ -202,6 +202,7 @@ typedef enum {
|
|||
ICM_42688P_SPI,
|
||||
BMI_160_SPI,
|
||||
BMI_270_SPI,
|
||||
ASM330LHH_SPI,
|
||||
LSM6DS3_SPI,
|
||||
LSM6DSL_SPI,
|
||||
LSM6DSO_SPI,
|
||||
|
|
|
@ -0,0 +1,206 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and ATBetaflight (forked by flightng).
|
||||
*
|
||||
* Cleanflight and ATBetaflight (forked by flightng) are 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.
|
||||
*
|
||||
* Cleanflight and ATBetaflight (forked by flightng) are distributed in the hope that they
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_ACCGYRO_ASM330LHH
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/accgyro/accgyro_spi_asm330lhh.h"
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/io_impl.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
|
||||
#define GYRO_EXTI_DETECT_THRESHOLD 1000
|
||||
|
||||
#ifdef USE_GYRO_EXTI
|
||||
// Called in ISR context
|
||||
// Gyro read has just completed
|
||||
busStatus_e asm330lhhIntcallback(uint32_t arg)
|
||||
{
|
||||
gyroDev_t *gyro = (gyroDev_t *)arg;
|
||||
int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
|
||||
|
||||
if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
|
||||
gyro->gyroDmaMaxDuration = gyroDmaDuration;
|
||||
}
|
||||
|
||||
gyro->dataReady = true;
|
||||
|
||||
return BUS_READY;
|
||||
}
|
||||
|
||||
void asm330lhhExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
// Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does
|
||||
// not have an associated timer
|
||||
uint32_t nowCycles = getCycleCounter();
|
||||
gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
|
||||
gyro->gyroLastEXTI = nowCycles;
|
||||
|
||||
if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
|
||||
spiSequence(&gyro->dev, gyro->segments);
|
||||
}
|
||||
|
||||
gyro->detectedEXTI++;
|
||||
|
||||
}
|
||||
#else
|
||||
void asm330lhhExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
gyro->dataReady = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool asm330lhhAccRead(accDev_t *acc)
|
||||
{
|
||||
switch (acc->gyro->gyroModeSPI) {
|
||||
case GYRO_EXTI_INT:
|
||||
case GYRO_EXTI_NO_INT:
|
||||
{
|
||||
acc->gyro->dev.txBuf[1] = ASM330LHH_REG_OUTX_L_A | 0x80;
|
||||
|
||||
busSegment_t segments[] = {
|
||||
{.u.buffers = {NULL, NULL}, 8, true, NULL},
|
||||
{.u.link = {NULL, NULL}, 0, true, NULL},
|
||||
};
|
||||
segments[0].u.buffers.txData = &acc->gyro->dev.txBuf[1];
|
||||
segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1];
|
||||
|
||||
spiSequence(&acc->gyro->dev, &segments[0]);
|
||||
|
||||
// Wait for completion
|
||||
spiWait(&acc->gyro->dev);
|
||||
|
||||
int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf;
|
||||
acc->ADCRaw[X] = accData[1];
|
||||
acc->ADCRaw[Y] = accData[2];
|
||||
acc->ADCRaw[Z] = accData[3];
|
||||
break;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INT_DMA:
|
||||
{
|
||||
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
|
||||
// up an old value.
|
||||
|
||||
// This data was read from the gyro, which is the same SPI device as the acc
|
||||
int16_t *accData = (int16_t *)acc->gyro->dev.rxBuf;
|
||||
acc->ADCRaw[X] = accData[4];
|
||||
acc->ADCRaw[Y] = accData[5];
|
||||
acc->ADCRaw[Z] = accData[6];
|
||||
break;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INIT:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool asm330lhhGyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
int16_t *gyroData = (int16_t *)gyro->dev.rxBuf;
|
||||
switch (gyro->gyroModeSPI) {
|
||||
case GYRO_EXTI_INIT:
|
||||
{
|
||||
// Initialise the tx buffer to all 0x00
|
||||
memset(gyro->dev.txBuf, 0x00, 14);
|
||||
#ifdef USE_GYRO_EXTI
|
||||
// Check that minimum number of interrupts have been detected
|
||||
|
||||
// We need some offset from the gyro interrupts to ensure sampling after the interrupt
|
||||
gyro->gyroDmaMaxDuration = 5;
|
||||
// Using DMA for gyro access upsets the scheduler on the F4
|
||||
if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
|
||||
if (spiUseDMA(&gyro->dev)) {
|
||||
gyro->dev.callbackArg = (uint32_t)gyro;
|
||||
gyro->dev.txBuf[1] = ASM330LHH_REG_OUTX_L_G | 0x80;
|
||||
gyro->segments[0].len = 13;
|
||||
gyro->segments[0].callback = asm330lhhIntcallback;
|
||||
gyro->segments[0].u.buffers.txData = &gyro->dev.txBuf[1];
|
||||
gyro->segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
|
||||
gyro->segments[0].negateCS = true;
|
||||
gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
|
||||
} else {
|
||||
// Interrupts are present, but no DMA
|
||||
gyro->gyroModeSPI = GYRO_EXTI_INT;
|
||||
}
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INT:
|
||||
case GYRO_EXTI_NO_INT:
|
||||
{
|
||||
gyro->dev.txBuf[1] = ASM330LHH_REG_OUTX_L_G | 0x80;
|
||||
|
||||
busSegment_t segments[] = {
|
||||
{.u.buffers = {NULL, NULL}, 7, true, NULL},
|
||||
{.u.link = {NULL, NULL}, 0, true, NULL},
|
||||
};
|
||||
segments[0].u.buffers.txData = &gyro->dev.txBuf[1];
|
||||
segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
|
||||
|
||||
spiSequence(&gyro->dev, &segments[0]);
|
||||
|
||||
// Wait for completion
|
||||
spiWait(&gyro->dev);
|
||||
|
||||
// Fall through
|
||||
FALLTHROUGH;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INT_DMA:
|
||||
{
|
||||
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
|
||||
// up an old value.
|
||||
gyro->gyroADCRaw[X] = gyroData[1];
|
||||
gyro->gyroADCRaw[Y] = gyroData[2];
|
||||
gyro->gyroADCRaw[Z] = gyroData[3];
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif
|
|
@ -0,0 +1,67 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and ATBetaflight (forked by flightng).
|
||||
*
|
||||
* Cleanflight and ATBetaflight (forked by flightng) are 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.
|
||||
*
|
||||
* Cleanflight and ATBetaflight (forked by flightng) are distributed in the hope that they
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/exti.h"
|
||||
|
||||
// ASM330LHH registers (not the complete list)
|
||||
typedef enum {
|
||||
ASM330LHH_REG_COUNTER_BDR1 = 0x0B,// Counter batch data rate register
|
||||
ASM330LHH_REG_INT1_CTRL = 0x0D, // int pin 1 control
|
||||
ASM330LHH_REG_INT2_CTRL = 0x0E, // int pin 2 control
|
||||
ASM330LHH_REG_WHO_AM_I = 0x0F, // chip ID
|
||||
ASM330LHH_REG_CTRL1_XL = 0x10, // accelerometer control
|
||||
ASM330LHH_REG_CTRL2_G = 0x11, // gyro control
|
||||
ASM330LHH_REG_CTRL3_C = 0x12, // control register 3
|
||||
ASM330LHH_REG_CTRL4_C = 0x13, // control register 4
|
||||
ASM330LHH_REG_CTRL5_C = 0x14, // control register 5
|
||||
ASM330LHH_REG_CTRL6_C = 0x15, // control register 6
|
||||
ASM330LHH_REG_CTRL7_G = 0x16, // control register 7
|
||||
ASM330LHH_REG_CTRL8_XL = 0x17, // control register 8
|
||||
ASM330LHH_REG_CTRL9_XL = 0x18, // control register 9
|
||||
ASM330LHH_REG_CTRL10_C = 0x19, // control register 10
|
||||
ASM330LHH_REG_STATUS = 0x1E, // status register
|
||||
ASM330LHH_REG_OUT_TEMP_L = 0x20, // temperature LSB
|
||||
ASM330LHH_REG_OUT_TEMP_H = 0x21, // temperature MSB
|
||||
ASM330LHH_REG_OUTX_L_G = 0x22, // gyro X axis LSB
|
||||
ASM330LHH_REG_OUTX_H_G = 0x23, // gyro X axis MSB
|
||||
ASM330LHH_REG_OUTY_L_G = 0x24, // gyro Y axis LSB
|
||||
ASM330LHH_REG_OUTY_H_G = 0x25, // gyro Y axis MSB
|
||||
ASM330LHH_REG_OUTZ_L_G = 0x26, // gyro Z axis LSB
|
||||
ASM330LHH_REG_OUTZ_H_G = 0x27, // gyro Z axis MSB
|
||||
ASM330LHH_REG_OUTX_L_A = 0x28, // acc X axis LSB
|
||||
ASM330LHH_REG_OUTX_H_A = 0x29, // acc X axis MSB
|
||||
ASM330LHH_REG_OUTY_L_A = 0x2A, // acc Y axis LSB
|
||||
ASM330LHH_REG_OUTY_H_A = 0x2B, // acc Y axis MSB
|
||||
ASM330LHH_REG_OUTZ_L_A = 0x2C, // acc Z axis LSB
|
||||
ASM330LHH_REG_OUTZ_H_A = 0x2D, // acc Z axis MSB
|
||||
} asm330lhhRegister_e;
|
||||
|
||||
// Contained in accgyro_spi_asm330lhh_init.c which is size-optimized
|
||||
uint8_t asm330lhhDetect(const extDevice_t *dev);
|
||||
bool asm330lhhSpiAccDetect(accDev_t *acc);
|
||||
bool asm330lhhSpiGyroDetect(gyroDev_t *gyro);
|
||||
|
||||
// Contained in accgyro_spi_asm330lhh.c which is speed-optimized
|
||||
void asm330lhhExtiHandler(extiCallbackRec_t *cb);
|
||||
bool asm330lhhAccRead(accDev_t *acc);
|
||||
bool asm330lhhGyroRead(gyroDev_t *gyro);
|
|
@ -0,0 +1,244 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and ATBetaflight (forked by flightng).
|
||||
*
|
||||
* Cleanflight and ATBetaflight (forked by flightng) are 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.
|
||||
*
|
||||
* Cleanflight and ATBetaflight (forked by flightng) are distributed in the hope that they
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_ACCGYRO_ASM330LHH
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/accgyro/accgyro_spi_asm330lhh.h"
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/io_impl.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
// 10 MHz max SPI frequency
|
||||
#define ASM330LHH_MAX_SPI_CLK_HZ 10000000
|
||||
|
||||
#define ASM330LHH_CHIP_ID 0x6B
|
||||
|
||||
// ASM330LHH register configuration values
|
||||
typedef enum {
|
||||
ASM330LHH_VAL_COUNTER_BDR1_DDRY_PM = BIT(7),// (bit 7) enable data ready pulsed mode
|
||||
ASM330LHH_VAL_INT1_CTRL = 0x02, // enable gyro data ready interrupt pin 1
|
||||
ASM330LHH_VAL_INT2_CTRL = 0x00, // disable gyro data ready interrupt pin 2
|
||||
ASM330LHH_VAL_CTRL1_XL_ODR833 = 0x07, // accelerometer 833hz output data rate (gyro/8)
|
||||
ASM330LHH_VAL_CTRL1_XL_ODR1667 = 0x08, // accelerometer 1666hz output data rate (gyro/4)
|
||||
ASM330LHH_VAL_CTRL1_XL_ODR3332 = 0x09, // accelerometer 3332hz output data rate (gyro/2)
|
||||
ASM330LHH_VAL_CTRL1_XL_ODR6664 = 0x0A, // accelerometer 6664hz output data rate (gyro/1)
|
||||
ASM330LHH_VAL_CTRL1_XL_8G = 0x03, // accelerometer 8G scale
|
||||
ASM330LHH_VAL_CTRL1_XL_16G = 0x01, // accelerometer 16G scale
|
||||
ASM330LHH_VAL_CTRL1_XL_LPF1 = 0x00, // accelerometer output from LPF1
|
||||
ASM330LHH_VAL_CTRL1_XL_LPF2 = 0x01, // accelerometer output from LPF2
|
||||
ASM330LHH_VAL_CTRL2_G_ODR6664 = 0x0A, // gyro 6664hz output data rate
|
||||
ASM330LHH_VAL_CTRL2_G_2000DPS = 0x03, // gyro 2000dps scale
|
||||
// ASM330LHH_VAL_CTRL3_C_BDU = BIT(6), // (bit 6) output registers are not updated until MSB and LSB have been read (prevents MSB from being updated while burst reading LSB/MSB)
|
||||
ASM330LHH_VAL_CTRL3_C_H_LACTIVE = 0, // (bit 5) interrupt pins active high
|
||||
ASM330LHH_VAL_CTRL3_C_PP_OD = 0, // (bit 4) interrupt pins push/pull
|
||||
ASM330LHH_VAL_CTRL3_C_SIM = 0, // (bit 3) SPI 4-wire interface mode
|
||||
ASM330LHH_VAL_CTRL3_C_IF_INC = BIT(2), // (bit 2) auto-increment address for burst reads
|
||||
ASM330LHH_VAL_CTRL4_C_DRDY_MASK = BIT(3), // (bit 3) data ready interrupt mask
|
||||
ASM330LHH_VAL_CTRL4_C_I2C_DISABLE = BIT(2), // (bit 2) disable I2C interface
|
||||
ASM330LHH_VAL_CTRL4_C_LPF1_SEL_G = BIT(1), // (bit 1) enable gyro LPF1
|
||||
ASM330LHH_VAL_CTRL6_C_XL_HM_MODE = 0, // (bit 4) enable accelerometer high performance mode
|
||||
ASM330LHH_VAL_CTRL6_C_FTYPE_297HZ = 0x00, // (bits 2:0) gyro LPF1 cutoff 297Hz
|
||||
ASM330LHH_VAL_CTRL6_C_FTYPE_223HZ = 0x01, // (bits 2:0) gyro LPF1 cutoff 223Hz
|
||||
ASM330LHH_VAL_CTRL6_C_FTYPE_154HZ = 0x02, // (bits 2:0) gyro LPF1 cutoff 154Hz
|
||||
ASM330LHH_VAL_CTRL6_C_FTYPE_470HZ = 0x03, // (bits 2:0) gyro LPF1 cutoff 470Hz
|
||||
ASM330LHH_VAL_CTRL7_G_HP_EN_G = BIT(6), // (bit 6) enable gyro high-pass filter
|
||||
ASM330LHH_VAL_CTRL7_G_HPM_G_16 = 0x00, // (bits 5:4) gyro HPF cutoff 16mHz
|
||||
ASM330LHH_VAL_CTRL7_G_HPM_G_65 = 0x01, // (bits 5:4) gyro HPF cutoff 65mHz
|
||||
ASM330LHH_VAL_CTRL7_G_HPM_G_260 = 0x02, // (bits 5:4) gyro HPF cutoff 260mHz
|
||||
ASM330LHH_VAL_CTRL7_G_HPM_G_1040 = 0x03, // (bits 5:4) gyro HPF cutoff 1.04Hz
|
||||
ASM330LHH_VAL_CTRL9_XL_DEVICE_CONF = BIT(1),// (bit 1) Enables the proper device configuration
|
||||
} asm330lhhConfigValues_e;
|
||||
|
||||
// ASM330LHH register configuration bit masks
|
||||
typedef enum {
|
||||
ASM330LHH_MASK_COUNTER_BDR1 = 0x80, // 0b10000000
|
||||
ASM330LHH_MASK_CTRL3_C = 0x3C, // 0b00111100
|
||||
ASM330LHH_MASK_CTRL3_C_RESET = BIT(0), // 0b00000001
|
||||
ASM330LHH_MASK_CTRL4_C = 0x0E, // 0b00001110
|
||||
ASM330LHH_MASK_CTRL6_C = 0x17, // 0b00010111
|
||||
ASM330LHH_MASK_CTRL7_G = 0x70, // 0b01110000
|
||||
ASM330LHH_MASK_CTRL9_XL = 0x02, // 0b00000010
|
||||
} asm330lhhConfigMasks_e;
|
||||
|
||||
uint8_t asm330lhhDetect(const extDevice_t *dev)
|
||||
{
|
||||
uint8_t chipID = 0;
|
||||
|
||||
if (busReadRegisterBuffer(dev, ASM330LHH_REG_WHO_AM_I, &chipID, 1)) {
|
||||
if (chipID == ASM330LHH_CHIP_ID) {
|
||||
return ASM330LHH_SPI;
|
||||
}
|
||||
}
|
||||
|
||||
return MPU_NONE;
|
||||
}
|
||||
|
||||
static void asm330lhhWriteRegister(const extDevice_t *dev, asm330lhhRegister_e registerID, uint8_t value, unsigned delayMs)
|
||||
{
|
||||
busWriteRegister(dev, registerID, value);
|
||||
if (delayMs) {
|
||||
delay(delayMs);
|
||||
}
|
||||
}
|
||||
|
||||
static void asm330lhhWriteRegisterBits(const extDevice_t *dev, asm330lhhRegister_e registerID, asm330lhhConfigMasks_e mask, uint8_t value, unsigned delayMs)
|
||||
{
|
||||
uint8_t newValue;
|
||||
if (busReadRegisterBuffer(dev, registerID, &newValue, 1)) {
|
||||
delayMicroseconds(2);
|
||||
newValue = (newValue & ~mask) | value;
|
||||
asm330lhhWriteRegister(dev, registerID, newValue, delayMs);
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t getAsmDlpfBandwidth()
|
||||
{
|
||||
switch(gyroConfig()->gyro_hardware_lpf) {
|
||||
case GYRO_HARDWARE_LPF_NORMAL:
|
||||
return ASM330LHH_VAL_CTRL6_C_FTYPE_223HZ;
|
||||
case GYRO_HARDWARE_LPF_OPTION_1:
|
||||
return ASM330LHH_VAL_CTRL6_C_FTYPE_297HZ;
|
||||
case GYRO_HARDWARE_LPF_OPTION_2:
|
||||
return ASM330LHH_VAL_CTRL6_C_FTYPE_470HZ;
|
||||
case GYRO_HARDWARE_LPF_EXPERIMENTAL:
|
||||
return ASM330LHH_VAL_CTRL6_C_FTYPE_470HZ;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void asm330lhhConfig(gyroDev_t *gyro)
|
||||
{
|
||||
extDevice_t *dev = &gyro->dev;
|
||||
|
||||
// Reset the device (wait 100ms before continuing config)
|
||||
asm330lhhWriteRegisterBits(dev, ASM330LHH_REG_CTRL3_C, ASM330LHH_MASK_CTRL3_C_RESET, BIT(0), 100);
|
||||
|
||||
// Configure data ready pulsed mode
|
||||
asm330lhhWriteRegisterBits(dev, ASM330LHH_REG_COUNTER_BDR1, ASM330LHH_MASK_COUNTER_BDR1, ASM330LHH_VAL_COUNTER_BDR1_DDRY_PM, 0);
|
||||
|
||||
// Configure interrupt pin 1 for gyro data ready only
|
||||
asm330lhhWriteRegister(dev, ASM330LHH_REG_INT1_CTRL, ASM330LHH_VAL_INT1_CTRL, 1);
|
||||
|
||||
// Disable interrupt pin 2
|
||||
asm330lhhWriteRegister(dev, ASM330LHH_REG_INT2_CTRL, ASM330LHH_VAL_INT2_CTRL, 1);
|
||||
|
||||
// Configure the accelerometer
|
||||
// 833hz ODR, 16G scale, use LPF2 output (default with ODR/4 cutoff)
|
||||
asm330lhhWriteRegister(dev, ASM330LHH_REG_CTRL1_XL, (ASM330LHH_VAL_CTRL1_XL_ODR833 << 4) | (ASM330LHH_VAL_CTRL1_XL_16G << 2) | (ASM330LHH_VAL_CTRL1_XL_LPF2 << 1), 1);
|
||||
|
||||
// Configure the gyro
|
||||
// 6664hz ODR, 2000dps scale
|
||||
asm330lhhWriteRegister(dev, ASM330LHH_REG_CTRL2_G, (ASM330LHH_VAL_CTRL2_G_ODR6664 << 4) | (ASM330LHH_VAL_CTRL2_G_2000DPS << 2), 1);
|
||||
|
||||
// Configure control register 3
|
||||
// latch LSB/MSB during reads; set interrupt pins active high; set interrupt pins push/pull; set 4-wire SPI; enable auto-increment burst reads
|
||||
asm330lhhWriteRegisterBits(dev, ASM330LHH_REG_CTRL3_C, ASM330LHH_MASK_CTRL3_C, (ASM330LHH_VAL_CTRL3_C_H_LACTIVE | ASM330LHH_VAL_CTRL3_C_PP_OD | ASM330LHH_VAL_CTRL3_C_SIM | ASM330LHH_VAL_CTRL3_C_IF_INC), 1);
|
||||
|
||||
// Configure control register 4
|
||||
// enable accelerometer high performane mode; enable gyro LPF1
|
||||
asm330lhhWriteRegisterBits(dev, ASM330LHH_REG_CTRL4_C, ASM330LHH_MASK_CTRL4_C, (ASM330LHH_VAL_CTRL4_C_DRDY_MASK | ASM330LHH_VAL_CTRL4_C_I2C_DISABLE | ASM330LHH_VAL_CTRL4_C_LPF1_SEL_G), 1);
|
||||
|
||||
// Configure control register 6
|
||||
// disable I2C interface; set gyro LPF1 cutoff according to gyro_hardware_lpf setting
|
||||
asm330lhhWriteRegisterBits(dev, ASM330LHH_REG_CTRL6_C, ASM330LHH_MASK_CTRL6_C, (ASM330LHH_VAL_CTRL6_C_XL_HM_MODE | getAsmDlpfBandwidth()), 1);
|
||||
|
||||
// // Configure control register 7
|
||||
// asm330lhhWriteRegisterBits(dev, ASM330LHH_REG_CTRL7_G, ASM330LHH_MASK_CTRL7_G, (ASM330LHH_VAL_CTRL7_G_HP_EN_G | ASM330LHH_VAL_CTRL7_G_HPM_G_16), 1);
|
||||
|
||||
// Configure control register 9
|
||||
// Enables the proper device configuration
|
||||
asm330lhhWriteRegisterBits(dev, ASM330LHH_REG_CTRL9_XL, ASM330LHH_MASK_CTRL9_XL, ASM330LHH_VAL_CTRL9_XL_DEVICE_CONF, 1);
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_EXTI
|
||||
static void asm330lhhIntExtiInit(gyroDev_t *gyro)
|
||||
{
|
||||
if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
|
||||
return;
|
||||
}
|
||||
|
||||
IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
|
||||
|
||||
IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
|
||||
EXTIHandlerInit(&gyro->exti, asm330lhhExtiHandler);
|
||||
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
|
||||
EXTIEnable(mpuIntIO);
|
||||
}
|
||||
#endif
|
||||
|
||||
static void asm330lhhSpiGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
extDevice_t *dev = &gyro->dev;
|
||||
|
||||
asm330lhhConfig(gyro);
|
||||
|
||||
#ifdef USE_GYRO_EXTI
|
||||
asm330lhhIntExtiInit(gyro);
|
||||
#endif
|
||||
|
||||
spiSetClkDivisor(dev, spiCalculateDivider(ASM330LHH_MAX_SPI_CLK_HZ));
|
||||
}
|
||||
|
||||
static void asm330lhhSpiAccInit(accDev_t *acc)
|
||||
{
|
||||
// sensor is configured during gyro init
|
||||
acc->acc_1G = 512 * 4; // 16G sensor scale
|
||||
}
|
||||
|
||||
bool asm330lhhSpiAccDetect(accDev_t *acc)
|
||||
{
|
||||
if (acc->mpuDetectionResult.sensor != ASM330LHH_SPI) {
|
||||
return false;
|
||||
}
|
||||
|
||||
acc->initFn = asm330lhhSpiAccInit;
|
||||
acc->readFn = asm330lhhAccRead;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool asm330lhhSpiGyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
if (gyro->mpuDetectionResult.sensor != ASM330LHH_SPI) {
|
||||
return false;
|
||||
}
|
||||
|
||||
gyro->initFn = asm330lhhSpiGyroInit;
|
||||
gyro->readFn = asm330lhhGyroRead;
|
||||
gyro->scale = GYRO_SCALE_2000DPS;
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif // USE_ACCGYRO_ASM330LHH
|
|
@ -77,6 +77,11 @@ uint16_t gyroSetSampleRate(gyroDev_t *gyro)
|
|||
gyroSampleRateHz = 9000;
|
||||
accSampleRateHz = 1125;
|
||||
break;
|
||||
case ASM330LHH_SPI:
|
||||
gyro->gyroRateKHz = GYRO_RATE_6664_Hz;
|
||||
gyroSampleRateHz = 6664;
|
||||
accSampleRateHz = 833;
|
||||
break;
|
||||
case LSM6DS3_SPI:
|
||||
gyro->gyroRateKHz = GYRO_RATE_1667_Hz;
|
||||
gyroSampleRateHz = 1667;
|
||||
|
|
|
@ -46,6 +46,7 @@ typedef enum {
|
|||
ACC_ICM42688P,
|
||||
ACC_BMI160,
|
||||
ACC_BMI270,
|
||||
ACC_ASM330LHH,
|
||||
ACC_LSM6DS3,
|
||||
ACC_LSM6DSL,
|
||||
ACC_LSM6DSO,
|
||||
|
|
|
@ -47,6 +47,7 @@
|
|||
#include "drivers/accgyro/accgyro_spi_icm20649.h"
|
||||
#include "drivers/accgyro/accgyro_spi_icm20689.h"
|
||||
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
|
||||
#include "drivers/accgyro/accgyro_spi_asm330lhh.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6dsl.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
|
||||
|
@ -310,6 +311,15 @@ retry:
|
|||
FALLTHROUGH;
|
||||
#endif
|
||||
|
||||
#ifdef USE_ACCGYRO_ASM330LHH
|
||||
case ACC_ASM330LHH:
|
||||
if (asm330lhhSpiAccDetect(dev)) {
|
||||
accHardware = ACC_ASM330LHH;
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
#endif
|
||||
|
||||
#ifdef USE_ACCGYRO_LSM6DS3
|
||||
case ACC_LSM6DS3:
|
||||
if (lsm6ds3SpiAccDetect(dev)) {
|
||||
|
|
|
@ -46,6 +46,7 @@
|
|||
#include "drivers/accgyro/accgyro_spi_icm20689.h"
|
||||
#include "drivers/accgyro/accgyro_spi_icm20689.h"
|
||||
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
|
||||
#include "drivers/accgyro/accgyro_spi_asm330lhh.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6dsl.h"
|
||||
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
|
||||
|
@ -338,6 +339,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
|
|||
case GYRO_MPU6000:
|
||||
case GYRO_MPU6500:
|
||||
case GYRO_MPU9250:
|
||||
case GYRO_ASM330LHH:
|
||||
case GYRO_LSM6DS3:
|
||||
case GYRO_LSM6DSL:
|
||||
case GYRO_LSM6DSO:
|
||||
|
@ -511,6 +513,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
FALLTHROUGH;
|
||||
#endif
|
||||
|
||||
#ifdef USE_ACCGYRO_ASM330LHH
|
||||
case GYRO_ASM330LHH:
|
||||
if (asm330lhhSpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_ASM330LHH;
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
#endif
|
||||
|
||||
#ifdef USE_ACCGYRO_LSM6DS3
|
||||
case GYRO_LSM6DS3:
|
||||
if (lsm6ds3SpiGyroDetect(dev)) {
|
||||
|
@ -582,8 +593,8 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
|
|||
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|
||||
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
|
||||
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) \
|
||||
|| defined(USE_ACCGYRO_LSM6DS3) || defined(USE_ACCGYRO_LSM6DSL) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_ACCGYRO_QMI8658) \
|
||||
|| defined(USE_ACCGYRO_SH3001) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
|
||||
|| defined(USE_ACCGYRO_ASM330LHH) || defined(USE_ACCGYRO_LSM6DS3) || defined(USE_ACCGYRO_LSM6DSL) || defined(USE_ACCGYRO_LSM6DSO) \
|
||||
|| defined(USE_ACCGYRO_QMI8658) || defined(USE_ACCGYRO_SH3001) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
|
||||
|
||||
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
|
||||
|
||||
|
@ -608,8 +619,8 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
|
|||
{
|
||||
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|
||||
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) \
|
||||
|| defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DS3) || defined(USE_ACCGRYO_LSM6DSL) ||defined(USE_ACCGRYO_LSM6DSO) \
|
||||
|| defined(USE_ACCGYRO_QMI8658) || defined(USE_ACCGYRO_SH3001)
|
||||
|| defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_ASM330LHH) || defined(USE_ACCGRYO_LSM6DS3) || defined(USE_ACCGRYO_LSM6DSL) \
|
||||
|| defined(USE_ACCGRYO_LSM6DSO) || defined(USE_ACCGYRO_QMI8658) || defined(USE_ACCGYRO_SH3001)
|
||||
mpuPreInit(config);
|
||||
#else
|
||||
UNUSED(config);
|
||||
|
|
|
@ -109,6 +109,7 @@
|
|||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define USE_GYRO_SPI_ICM42688P
|
||||
#define USE_ACCGYRO_ASM330LHH
|
||||
#define USE_ACCGYRO_LSM6DS3
|
||||
#define USE_ACCGYRO_LSM6DSL
|
||||
#define USE_ACCGYRO_LSM6DSO
|
||||
|
|
|
@ -103,6 +103,7 @@
|
|||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_ICM42688P
|
||||
#define USE_ACCGYRO_BMI270
|
||||
#define USE_ACCGYRO_ASM330LHH
|
||||
#define USE_ACCGYRO_LSM6DSL
|
||||
#define USE_ACCGYRO_LSM6DSO
|
||||
#define USE_ACCGYRO_QMI8658
|
||||
|
|
|
@ -14,6 +14,8 @@ TARGET_SRC = \
|
|||
drivers/compass/compass_qmc5883l.c\
|
||||
$(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \
|
||||
drivers/accgyro/accgyro_spi_bmi270.c\
|
||||
drivers/accgyro/accgyro_spi_asm330lhh_init.c \
|
||||
drivers/accgyro/accgyro_spi_asm330lhh.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl_init.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
|
||||
|
|
|
@ -103,6 +103,7 @@
|
|||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_ICM42688P
|
||||
#define USE_ACCGYRO_BMI270
|
||||
#define USE_ACCGYRO_ASM330LHH
|
||||
#define USE_ACCGYRO_LSM6DSL
|
||||
#define USE_ACCGYRO_LSM6DSO
|
||||
#define USE_ACCGYRO_SH3001
|
||||
|
|
|
@ -14,6 +14,8 @@ TARGET_SRC = \
|
|||
drivers/compass/compass_qmc5883l.c\
|
||||
$(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \
|
||||
drivers/accgyro/accgyro_spi_bmi270.c\
|
||||
drivers/accgyro/accgyro_spi_asm330lhh_init.c \
|
||||
drivers/accgyro/accgyro_spi_asm330lhh.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl_init.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
|
||||
|
|
|
@ -90,6 +90,7 @@
|
|||
// #define USE_ACC_SPI_MPU6500 //debug only
|
||||
#define USE_GYRO_SPI_ICM42688P
|
||||
#define USE_ACCGYRO_BMI270
|
||||
#define USE_ACCGYRO_ASM330LHH
|
||||
#define USE_ACCGYRO_LSM6DSL
|
||||
#define USE_ACCGYRO_LSM6DSO
|
||||
#define USE_ACCGYRO_QMI8658
|
||||
|
|
|
@ -13,6 +13,8 @@ TARGET_SRC = \
|
|||
drivers/compass/compass_qmc5883l.c\
|
||||
$(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \
|
||||
drivers/accgyro/accgyro_spi_bmi270.c\
|
||||
drivers/accgyro/accgyro_spi_asm330lhh_init.c \
|
||||
drivers/accgyro/accgyro_spi_asm330lhh.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dso.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl_init.c \
|
||||
|
|
|
@ -93,6 +93,7 @@
|
|||
// #define USE_ACC_SPI_MPU6500 //debug only
|
||||
#define USE_GYRO_SPI_ICM42688P
|
||||
#define USE_ACCGYRO_BMI270
|
||||
#define USE_ACCGYRO_ASM330LHH
|
||||
#define USE_ACCGYRO_LSM6DSL
|
||||
#define USE_ACCGYRO_LSM6DSO
|
||||
#define USE_ACCGYRO_QMI8658
|
||||
|
|
|
@ -13,6 +13,8 @@ TARGET_SRC = \
|
|||
drivers/compass/compass_qmc5883l.c\
|
||||
$(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c \
|
||||
drivers/accgyro/accgyro_spi_bmi270.c\
|
||||
drivers/accgyro/accgyro_spi_asm330lhh_init.c \
|
||||
drivers/accgyro/accgyro_spi_asm330lhh.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl_init.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dsl.c \
|
||||
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
|
||||
|
|
Loading…
Reference in New Issue