rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #28 from flightng/add-support-for-sh3001-imu 

Add support for SH3001 IMU
This commit is contained in:
EMSR 2023-02-11 16:47:46 +08:00 committed by GitHub
parent 212aa64100 e06cc3154a
commit fcfe283505
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 804 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
make/source.mk
View File

@ -237,6 +237,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/accgyro/accgyro_spi_lsm6ds3.c \
drivers/accgyro/accgyro_spi_lsm6dsl.c \
drivers/accgyro/accgyro_spi_lsm6dso.c \
drivers/accgyro/accgyro_spi_sh3001.c \
drivers/accgyro_legacy/accgyro_adxl345.c \
drivers/accgyro_legacy/accgyro_bma280.c \
drivers/accgyro_legacy/accgyro_l3g4200d.c \
@ -376,7 +377,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_spi_lsm6ds3_init.c \
drivers/accgyro/accgyro_spi_lsm6dsl_init.c \
drivers/accgyro/accgyro_spi_lsm6dso_init.c
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
drivers/accgyro/accgyro_spi_sh3001_init.c
# F4 and F7 optimizations

4
src/main/cli/settings.c
View File

@ -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", "FAKE"
"BMI160", "BMI270", "LSM6DS3", "LSM6DSL", "LSM6DSO", "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", "LSM6SDO", "FAKE"
"BMI160", "BMI270", "LSM6DS3", "LSM6DSL", "LSM6SDO", "SH3001", "FAKE"
};
#if defined(USE_SENSOR_NAMES) || defined(USE_BARO)

1
src/main/drivers/accgyro/accgyro.h
View File

@ -61,6 +61,7 @@ typedef enum {
GYRO_LSM6DS3,
GYRO_LSM6DSL,
GYRO_LSM6DSO,
GYRO_SH3001,
GYRO_FAKE
} gyroHardware_e;

4
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -57,6 +57,7 @@
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/accgyro/accgyro_spi_sh3001.h"
#include "drivers/accgyro/accgyro_spi_l3gd20.h"
#include "drivers/accgyro/accgyro_mpu.h"
@ -366,6 +367,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_ACCGYRO_LSM6DSO
lsm6dsoDetect,
#endif
#ifdef USE_ACCGYRO_SH3001
sh3001Detect,
#endif
#ifdef USE_ACCGYRO_BMI160
bmi160Detect,
#endif

1
src/main/drivers/accgyro/accgyro_mpu.h
View File

@ -206,6 +206,7 @@ typedef enum {
LSM6DSL_SPI,
LSM6DSO_SPI,
L3GD20_SPI,
SH3001_SPI,
} mpuSensor_e;
typedef enum {

286
src/main/drivers/accgyro/accgyro_spi_sh3001.c Normal file
View File

@ -0,0 +1,286 @@
/*
* 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_SH3001
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_spi_sh3001.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"
//use global compCoef defined in accgryo_spi_init.c
extern compCoef_t sh3001CompCoef;
// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
#define SH3001_EXTI_DETECT_THRESHOLD 0 // since the no-latched clean time is too long
#ifdef USE_GYRO_EXTI
// Called in ISR context
// Gyro read has just completed
busStatus_e sh3001Intcallback(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 sh3001ExtiHandler(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 sh3001ExtiHandler(extiCallbackRec_t* cb)
{
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
gyro->dataReady = true;
}
#endif
bool sh3001AccRead(accDev_t *acc)
{
switch (acc->gyro->gyroModeSPI) {
case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT:
{
acc->gyro->dev.txBuf[1] = SH3001_REG_ACC_X_DATA_L | 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);
// 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.
// 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;
int32_t accTempX = (int32_t)( accData[1] + \
accData[2] * ((float)sh3001CompCoef.cXY/1024.0f) + \
accData[3] * ((float)sh3001CompCoef.cXZ/1024.0f) );
int32_t accTempY = (int32_t)( accData[1] * ((float)sh3001CompCoef.cYX/1024.0f) + \
accData[2] + \
accData[3] * ((float)sh3001CompCoef.cYZ/1024.0f) );
int32_t accTempZ = (int32_t)( accData[1] * ((float)sh3001CompCoef.cZX/1024.0f) + \
accData[2] * ((float)sh3001CompCoef.cZY/1024.0f) + \
accData[3] );
if (accTempX > 32767) {
accTempX = 32767;
} else if (accTempX < -32768) {
accTempX = -32768;
}
if (accTempY > 32767) {
accTempY = 32767;
} else if (accTempY < -32768) {
accTempY = -32768;
}
if (accTempZ > 32767) {
accTempZ = 32767;
} else if (accTempZ < -32768) {
accTempZ = -32768;
}
acc->ADCRaw[X] = accTempX;
acc->ADCRaw[Y] = accTempY;
acc->ADCRaw[Z] = accTempZ;
break;
}
case GYRO_EXTI_INIT:
default:
break;
}
return true;
}
static uint8_t sh3001InitCont = 0;
bool sh3001GyroRead(gyroDev_t *gyro)
{
int16_t *gyroData = (int16_t *)gyro->dev.rxBuf;
switch (gyro->gyroModeSPI) {
case GYRO_EXTI_INIT:
{
// 0 R 1L 1H 2L 2H 3L 3H AL AH BL BH CL CH TL TH PP
// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
// 0 1 2 3 4 5 6 7 8
sh3001InitCont++;
if (sh3001InitCont > 200 ){
#ifdef USE_GYRO_EXTI
// Initialise the tx buffer to all 0x00
memset(gyro->dev.txBuf, 0x00, 18);
// We need some offset from the gyro interrupts to ensure sampling after the interrupt
gyro->gyroDmaMaxDuration = 5;
// Check that minimum number of interrupts have been detected
if (gyro->detectedEXTI > SH3001_EXTI_DETECT_THRESHOLD) {
if (spiUseDMA(&gyro->dev)) {
gyro->dev.callbackArg = (uint32_t)gyro;
gyro->dev.txBuf[1] = SH3001_REG_ACC_X_DATA_L | 0x80;
gyro->segments[0].len = 16;
gyro->segments[0].callback = sh3001Intcallback;
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;
// clear the data ready flag inside of the sensor
spiSequence(&gyro->dev, gyro->segments);
spiWait(&gyro->dev);
} else {
// Interrupts are present, but no DMA
gyro->gyroModeSPI = GYRO_EXTI_INT;
}
} else
#endif
{
gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
}
break;
}
FALLTHROUGH;
}
case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT:
{
gyro->dev.txBuf[1] = SH3001_REG_GYRO_X_DATA_L | 0x80;
// 0 R 1L 1H 2L 2H 3L 3H TL TH PP
// 1 2 3 4 5 6 7 8 9 10 txBuf & rxBuf
// 0 1 2 3 4 5
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 10, 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);
uint8_t paramP = gyro->dev.rxBuf[10] & 0x1F;
int32_t gyroTempX = gyroData[1] - (paramP - sh3001CompCoef.paramP0) * sh3001CompCoef.jX * sh3001CompCoef.xMulti;
int32_t gyroTempY = gyroData[2] - (paramP - sh3001CompCoef.paramP0) * sh3001CompCoef.jY * sh3001CompCoef.yMulti;
int32_t gyroTempZ = gyroData[3] - (paramP - sh3001CompCoef.paramP0) * sh3001CompCoef.jZ * sh3001CompCoef.zMulti;
if (gyroTempX > 32767) {
gyroTempX = 32767;
} else if (gyroTempX < -32768) {
gyroTempX = -32768;
}
if (gyroTempY > 32767) {
gyroTempY = 32767;
} else if (gyroTempY < -32768) {
gyroTempY = -32768;
}
if (gyroTempZ > 32767) {
gyroTempZ = 32767;
} else if (gyroTempZ < -32768) {
gyroTempZ = -32768;
}
gyro->gyroADCRaw[X] = (int16_t)gyroTempX;
gyro->gyroADCRaw[Y] = (int16_t)gyroTempY;
gyro->gyroADCRaw[Z] = (int16_t)gyroTempZ;
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.
uint8_t paramP = gyro->dev.rxBuf[15] & 0x1F;
int32_t gyroTempX = gyroData[4] - (paramP - sh3001CompCoef.paramP0) * sh3001CompCoef.jX * sh3001CompCoef.xMulti;
int32_t gyroTempY = gyroData[5] - (paramP - sh3001CompCoef.paramP0) * sh3001CompCoef.jY * sh3001CompCoef.yMulti;
int32_t gyroTempZ = gyroData[6] - (paramP - sh3001CompCoef.paramP0) * sh3001CompCoef.jZ * sh3001CompCoef.zMulti;
if (gyroTempX > 32767) {
gyroTempX = 32767;
} else if (gyroTempX < -32768) {
gyroTempX = -32768;
}
if (gyroTempY > 32767) {
gyroTempY = 32767;
} else if (gyroTempY < -32768) {
gyroTempY = -32768;
}
if (gyroTempZ > 32767) {
gyroTempZ = 32767;
} else if (gyroTempZ < -32768) {
gyroTempZ = -32768;
}
gyro->gyroADCRaw[X] = (int16_t)gyroTempX;
gyro->gyroADCRaw[Y] = (int16_t)gyroTempY;
gyro->gyroADCRaw[Z] = (int16_t)gyroTempZ;
break;
}
default:
break;
}
return true;
}
#endif

89
src/main/drivers/accgyro/accgyro_spi_sh3001.h Normal file
View File

@ -0,0 +1,89 @@
/*
* 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"
// SH3001 registers (not the complete list)
typedef enum {
SH3001_REG_ACC_X_DATA_L = 0x00,
SH3001_REG_ACC_X_DATA_H = 0x01,
SH3001_REG_ACC_Y_DATA_L = 0x02,
SH3001_REG_ACC_Y_DATA_H = 0x03,
SH3001_REG_ACC_Z_DATA_L = 0x04,
SH3001_REG_ACC_Z_DATA_H = 0x05,
SH3001_REG_GYRO_X_DATA_L = 0x06,
SH3001_REG_GYRO_X_DATA_H = 0x07,
SH3001_REG_GYRO_Y_DATA_L = 0x08,
SH3001_REG_GYRO_Y_DATA_H = 0x09,
SH3001_REG_GYRO_Z_DATA_L = 0x0A,
SH3001_REG_GYRO_Z_DATA_H = 0x0B,
SH3001_REG_TEMP_DATA_L = 0x0C,
SH3001_REG_TEMP_DATA_H = 0x0D,
// SH3001_REG_MAGIC
SH3001_REG_CHIP_ID = 0x0F, // chip id, should be 0x61
SH3001_REG_TEMP_CONFIG_0 = 0x20, // temperature sensor ODR
SH3001_REG_TEMP_CONFIG_1 = 0x21, // temperature sensor
SH3001_REG_ACC_CONFIG_0 = 0x22, // work mode, ADC dither, digital filter
SH3001_REG_ACC_CONFIG_1 = 0x23, // output data rate
SH3001_REG_ACC_CONFIG_2 = 0x25, // range
SH3001_REG_ACC_CONFIG_3 = 0x26, // lpf cutoff frequency
SH3001_REG_GYRO_CONFIG_0 = 0x28, // digital filter
SH3001_REG_GYRO_CONFIG_1 = 0x29, // output data rate
SH3001_REG_GYRO_CONFIG_2 = 0x2B, // lpf cutoff frequency
SH3001_REG_INTERRUPT_EN_1 = 0x41, // gyro DDRY interrupt enable
SH3001_REG_INTERRUPT_CONFIG = 0x44, // interrupt pin config
SH3001_REG_INTERRUPT_CONT_LIM = 0x45,// interrupt pin config
SH3001_REG_INT_PINMAP_1 = 0x7A, // interrupt pin config
SH3001_REG_PAGE = 0x7F, // page select
SH3001_REG_GYRO_CONFIG_3 = 0x8F, // x range
SH3001_REG_GYRO_CONFIG_4 = 0x9F, // y range
SH3001_REG_GYRO_CONFIG_5 = 0xAF, // z range
SH3001_REG_CHIP_VERSION = 0xDD, // chip version
} sh3001Register_e;
typedef struct {
int8_t cXY;
int8_t cXZ;
int8_t cYX;
int8_t cYZ;
int8_t cZX;
int8_t cZY;
int8_t jX;
int8_t jY;
int8_t jZ;
uint8_t xMulti;
uint8_t yMulti;
uint8_t zMulti;
uint8_t paramP0;
} compCoef_t;
// Contained in accgyro_spi_sh3001_init.c which is size optimized
uint8_t sh3001Detect(const extDevice_t *dev);
bool sh3001SpiAccDetect(accDev_t *acc);
bool sh3001SpiGyroDetect(gyroDev_t *gyro);
// Contained in accgyro_spi_sh3001.c which is speed optimized
void sh3001ExtiHandler(extiCallbackRec_t *cb);
bool sh3001AccRead(accDev_t *acc);
bool sh3001GyroRead(gyroDev_t *gyro);

380
src/main/drivers/accgyro/accgyro_spi_sh3001_init.c Normal file
View File

@ -0,0 +1,380 @@
/*
* 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_SH3001
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_spi_sh3001.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"
// 8 MHz max SPI frequency
#define SH3001_MAX_SPI_CLK_HZ 8000000
#define SH3001_CHIP_ID 0x61
compCoef_t sh3001CompCoef;
// SH3001 register configuration values
typedef enum {
SH3001_VAL_ACC_CONFIG_0_WORK_MODE = 0, // bit 7, normal mode
SH3001_VAL_ACC_CONFIG_0_ADC_DITHER_ENABLE = BIT(6), // bit 6, enable dither
SH3001_VAL_ACC_CONFIG_0_DIGITAL_FILTER_ENABLE = BIT(0), // bit 0, digital filter enable
SH3001_VAL_ACC_CONFIG_1_1KHZ_ODR = 0x00, // bit [3:0], 1000Hz ODR
SH3001_VAL_ACC_CONFIG_2_16G_RANGE = 0x02, // bit [2:0], 16g range
SH3001_VAL_ACC_CONFIG_3_LPF_CUTOFF = 0x01, // bit [7:5], ODR*0.25
SH3001_VAL_GYRO_CONFIG_1_8KHZ_ODR = 0x0A, // bit [3:0], 8000Hz ODR
SH3001_VAL_GYRO_CONFIG_2_DLPF_213 = 0x03, // bit [3:2], DLPF 213Hz
SH3001_VAL_GYRO_CONFIG_2_DLPF_313 = 0x02, // bit [3:2], DLPF 313Hz
SH3001_VAL_GYRO_CONFIG_2_DLPF_438 = 0x01, // bit [3:2], DLPF 438Hz
SH3001_VAL_GYRO_CONFIG_2_DLPF_1313 = 0x00, // bit [3:2], DLPF 1313Hz
SH3001_VAL_GYRO_RANGE_2000DPS = 0x06, // bit [2:0], 2000dps range
SH3001_VAL_TEMP_CONFIG_0_500HZ_ODR = 0x00, // bit [5:4], 500Hz ODR
SH3001_VAL_INTERRUPT_EN_1_GYRO_DRDY_ENABLE = BIT(3), // bit 3, enable gyro data ready interrupt
SH3001_VAL_INTERRUPT_CONFIG_ACTIVE_HIGH = 0x00, // bit 7, active high
SH3001_VAL_INTERRUPT_CONFIG_ANY_READ = BIT(4), // bit 4, clear interrupt flag after any register read
// SH3001_VAL_SH3001_REG_INTERRUPT_CONT_LIM = 0x01, // interrupt count limit
SH3001_VAL_INT_PINMAP_1_GYRO_DRDY_PIN = 0x00, // bit 4, gyro data ready interrupt pin on INT pin
SH3001_VAL_CHIP_VERSION_MCC = 0x08, // version C
SH3001_VAL_CHIP_VERSION_MCD = 0x10, // version D
SH3001_VAL_CHIP_VERSION_MCF = 0x20, // version F
} sh3001ConfigValues_e;
typedef enum {
SH3001_MASK_ACC_CONFIG_0 = 0xC1,
SH3001_MASK_ACC_CONFIG_1 = 0x0F,
SH3001_MASK_ACC_CONFIG_2 = 0x07,
SH3001_MASK_ACC_CONFIG_3 = 0xE0,
SH3001_MASK_GYRO_CONFIG_1 = 0x0F,
SH3001_MASK_GYRO_CONFIG_2 = 0x0C,
SH3001_MASK_TEMP_CONFIG_0 = 0x30,
SH3001_MASK_INTERRUPT_EN_1 = 0x08,
SH3001_MASK_INTERRUPT_CONFIG = 0x90, // 1001 0000
// SH3001_MASK_INTERRUPT_CONT_LIM = 0xFF,
SH3001_MASK_INT_PINMAP_1 = 0x10,
SH3001_MASK_GYRO_RANGE = 0x07, // used on REG_GYRO_CONFIG_3 4 5
} sh3001ConfigMasks_e;
uint8_t sh3001Detect(const extDevice_t *dev)
{
uint8_t chipID = 0;
uint8_t i = 0;
while ((chipID != SH3001_CHIP_ID) && (i++ < 3)) {
busReadRegisterBuffer(dev, SH3001_REG_CHIP_ID, &chipID, 1);
if ((i == 3) && (chipID != SH3001_CHIP_ID)) {
return MPU_NONE;
}
}
return SH3001_SPI;
}
static void sh3001WriteRegister(const extDevice_t *dev, sh3001Register_e registerID, uint8_t value, unsigned delayMs)
{
uint8_t page = (registerID > 0x7F) ? 0x01 : 0x00;
busWriteRegister(dev, SH3001_REG_PAGE, page);
delayMicroseconds(1);
busWriteRegister(dev, (registerID & 0x7F), value);
if (delayMs) {
delay(delayMs);
}
}
static void sh3001WriteRegisterBits(const extDevice_t *dev, sh3001Register_e registerID, sh3001ConfigMasks_e mask, uint8_t value, unsigned delayMs)
{
uint8_t regValue = 0;
if ( busReadRegisterBuffer(dev, registerID, &regValue, 1)){
delay(2);
regValue = (regValue & ~mask) | value;
sh3001WriteRegister(dev, registerID, regValue, delayMs);
}
}
static uint8_t sh3001ReadRegister(const extDevice_t *dev, sh3001Register_e registerID)
{
uint8_t value = 0;
uint8_t page = (registerID > 0x7F) ? 0x01 : 0x00;
busWriteRegister(dev, SH3001_REG_PAGE, page);
delay(1);
busReadRegisterBuffer(dev, (registerID & 0x7F), &value, 1);
return value;
}
static uint16_t sh3001ReadRegister16(const extDevice_t *dev, sh3001Register_e registerID)
{
uint8_t value[2] = {0};
uint8_t page = (registerID > 0x7F) ? 0x01 : 0x00;
busWriteRegister(dev, SH3001_REG_PAGE, page);
delay(1);
busReadRegisterBuffer(dev, (registerID & 0x7F), value, 2);
return (value[0] << 8) | value[1];
}
static void sh3001ResetMCX(const extDevice_t *dev, sh3001ConfigValues_e type)
{
uint8_t regAddr[9] = {0xC0, 0xD3, 0xC2, 0xD3, 0xD5, 0xD4, 0xBB, 0xB9, 0xBA};
uint8_t regDataA[9], regDataB[9] = {0};
if (type == SH3001_VAL_CHIP_VERSION_MCC) {
uint8_t regDataANew[9] = {0x38, 0xC6, 0x10, 0xC1, 0x02, 0x0C, 0x18, 0x18, 0x18};
uint8_t regDataBNew[9] = {0x3D, 0xC2, 0x20, 0xC2, 0x00, 0x04, 0x00, 0x00, 0x00};
memcpy(regDataA, regDataANew, 9);
memcpy(regDataB, regDataBNew, 9);
} else if (type == SH3001_VAL_CHIP_VERSION_MCD) {
uint8_t regDataANew[9] = {0x38, 0xD6, 0x10, 0xD1, 0x02, 0x08, 0x18, 0x18, 0x18};
uint8_t regDataBNew[9] = {0x3D, 0xD2, 0x20, 0xD2, 0x00, 0x00, 0x00, 0x00, 0x00};
memcpy(regDataA, regDataANew, 9);
memcpy(regDataB, regDataBNew, 9);
} else if (type == SH3001_VAL_CHIP_VERSION_MCF) {
uint8_t regDataANew[9] = {0x38, 0x16, 0x10, 0x11, 0x02, 0x08, 0x18, 0x18, 0x18};
uint8_t regDataBNew[9] = {0x3E, 0x12, 0x20, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00};
memcpy(regDataA, regDataANew, 9);
memcpy(regDataB, regDataBNew, 9);
}
// Drive start
sh3001WriteRegister(dev, regAddr[0], regDataA[0], 1);
sh3001WriteRegister(dev, regAddr[1], regDataA[1], 1);
sh3001WriteRegister(dev, regAddr[2], regDataA[2], 1);
delay(300);
sh3001WriteRegister(dev, regAddr[0], regDataB[0], 1);
sh3001WriteRegister(dev, regAddr[1], regDataB[1], 1);
sh3001WriteRegister(dev, regAddr[2], regDataB[2], 1);
delay(100);
// ADC reset
sh3001WriteRegister(dev, regAddr[3], regDataA[3], 1);
sh3001WriteRegister(dev, regAddr[4], regDataA[4], 1);
delay(1);
sh3001WriteRegister(dev, regAddr[3], regDataB[3], 1);
delay(1);
sh3001WriteRegister(dev, regAddr[4], regDataB[4], 1);
delay(50);
// CVA reset
sh3001WriteRegister(dev, regAddr[5], regDataA[5], 1);
delay(10);
sh3001WriteRegister(dev, regAddr[5], regDataB[5], 1);
delay(1);
sh3001WriteRegister(dev, regAddr[6], regDataA[6], 1);
delay(10);
sh3001WriteRegister(dev, regAddr[7], regDataA[7], 1);
delay(10);
sh3001WriteRegister(dev, regAddr[8], regDataA[8], 1);
delay(10);
sh3001WriteRegister(dev, regAddr[6], regDataB[6], 1);
delay(10);
sh3001WriteRegister(dev, regAddr[7], regDataB[7], 1);
delay(10);
sh3001WriteRegister(dev, regAddr[8], regDataB[8], 1);
delay(10);
}
static void sh3001Reset(const extDevice_t *dev)
{
uint8_t chipVersion = sh3001ReadRegister(dev, SH3001_REG_CHIP_VERSION);
if (chipVersion == SH3001_VAL_CHIP_VERSION_MCC) {
sh3001ResetMCX(dev, SH3001_VAL_CHIP_VERSION_MCC);
} else if (chipVersion == SH3001_VAL_CHIP_VERSION_MCD) {
sh3001ResetMCX(dev, SH3001_VAL_CHIP_VERSION_MCD);
} else if (chipVersion == SH3001_VAL_CHIP_VERSION_MCF) {
sh3001ResetMCX(dev, SH3001_VAL_CHIP_VERSION_MCF);
} else {
sh3001ResetMCX(dev, SH3001_VAL_CHIP_VERSION_MCD);
}
}
static void sh3001CompCoefInit(const extDevice_t *dev, compCoef_t *compCoef)
{
uint16_t coefData16 = 0;
uint8_t data = 0;
// Acc CAS
coefData16 = sh3001ReadRegister16(dev, 0x81);
compCoef->cYX = (int8_t)(coefData16 >> 8);
compCoef->cZX = (int8_t)(coefData16 & 0x00FF);
coefData16 = sh3001ReadRegister16(dev, 0x91);
compCoef->cXY = (int8_t)(coefData16 >> 8);
compCoef->cZY = (int8_t)(coefData16 & 0x00FF);
coefData16 = sh3001ReadRegister16(dev, 0xA1);
compCoef->cXZ = (int8_t)(coefData16 >> 8);
compCoef->cYZ = (int8_t)(coefData16 & 0x00FF);
// Gyro Zero
data = sh3001ReadRegister(dev, 0x60);
compCoef->jX = (int8_t)data;
data = sh3001ReadRegister(dev, 0x68);
compCoef->jY = (int8_t)data;
data = sh3001ReadRegister(dev, 0x70);
compCoef->jZ = (int8_t)data;
data = sh3001ReadRegister(dev, SH3001_REG_GYRO_CONFIG_3);
data = data & 0x07;
compCoef->xMulti = ((data<2)||(data>=7)) ? 1 : (1<<(6-data));
data = sh3001ReadRegister(dev, SH3001_REG_GYRO_CONFIG_4);
data = data & 0x07;
compCoef->yMulti = ((data<2)||(data>=7)) ? 1 : (1<<(6-data));
data = sh3001ReadRegister(dev, SH3001_REG_GYRO_CONFIG_5);
data = data & 0x07;
compCoef->zMulti = ((data<2)||(data>=7)) ? 1 : (1<<(6-data));
data = sh3001ReadRegister(dev, 0x2E);
compCoef->paramP0 = data & 0x1F;
}
static uint8_t getShDlpfBandwidth()
{
switch(gyroConfig()->gyro_hardware_lpf) {
case GYRO_HARDWARE_LPF_NORMAL:
return SH3001_VAL_GYRO_CONFIG_2_DLPF_213;
case GYRO_HARDWARE_LPF_OPTION_1:
return SH3001_VAL_GYRO_CONFIG_2_DLPF_313;
case GYRO_HARDWARE_LPF_OPTION_2:
return SH3001_VAL_GYRO_CONFIG_2_DLPF_438;
case GYRO_HARDWARE_LPF_EXPERIMENTAL:
return SH3001_VAL_GYRO_CONFIG_2_DLPF_1313;
}
return 0;
}
static void sh3001Config(gyroDev_t *gyro)
{
extDevice_t *dev = &gyro->dev;
// Reset device
sh3001Reset(dev);
// Configure accelerometer
sh3001WriteRegisterBits(dev, SH3001_REG_ACC_CONFIG_0, SH3001_MASK_ACC_CONFIG_0, (SH3001_VAL_ACC_CONFIG_0_WORK_MODE <<7 | SH3001_VAL_ACC_CONFIG_0_ADC_DITHER_ENABLE | SH3001_VAL_ACC_CONFIG_0_DIGITAL_FILTER_ENABLE), 1);
sh3001WriteRegisterBits(dev, SH3001_REG_ACC_CONFIG_1, SH3001_MASK_ACC_CONFIG_1, SH3001_VAL_ACC_CONFIG_1_1KHZ_ODR, 1);
sh3001WriteRegisterBits(dev, SH3001_REG_ACC_CONFIG_2, SH3001_MASK_ACC_CONFIG_2, SH3001_VAL_ACC_CONFIG_2_16G_RANGE, 1);
sh3001WriteRegisterBits(dev, SH3001_REG_ACC_CONFIG_3, SH3001_MASK_ACC_CONFIG_3, (SH3001_VAL_ACC_CONFIG_3_LPF_CUTOFF << 5), 1);
// Configure gyroscope
sh3001WriteRegisterBits(dev, SH3001_REG_GYRO_CONFIG_1, SH3001_MASK_GYRO_CONFIG_1, SH3001_VAL_GYRO_CONFIG_1_8KHZ_ODR, 1);
sh3001WriteRegisterBits(dev, SH3001_REG_GYRO_CONFIG_2, SH3001_MASK_GYRO_CONFIG_2, (getShDlpfBandwidth() << 2), 1);
sh3001WriteRegisterBits(dev, SH3001_REG_GYRO_CONFIG_3, SH3001_MASK_GYRO_RANGE, SH3001_VAL_GYRO_RANGE_2000DPS, 1);
sh3001WriteRegisterBits(dev, SH3001_REG_GYRO_CONFIG_4, SH3001_MASK_GYRO_RANGE, SH3001_VAL_GYRO_RANGE_2000DPS, 1);
sh3001WriteRegisterBits(dev, SH3001_REG_GYRO_CONFIG_5, SH3001_MASK_GYRO_RANGE, SH3001_VAL_GYRO_RANGE_2000DPS, 1);
// Configure temperature sensor
sh3001WriteRegisterBits(dev, SH3001_REG_TEMP_CONFIG_0, SH3001_MASK_TEMP_CONFIG_0, SH3001_VAL_TEMP_CONFIG_0_500HZ_ODR, 1);
// Configure interrupt
sh3001WriteRegisterBits(dev, SH3001_REG_INTERRUPT_EN_1, SH3001_MASK_INTERRUPT_EN_1, SH3001_VAL_INTERRUPT_EN_1_GYRO_DRDY_ENABLE, 1);
sh3001WriteRegisterBits(dev, SH3001_REG_INTERRUPT_CONFIG, SH3001_MASK_INTERRUPT_CONFIG, ( (SH3001_VAL_INTERRUPT_CONFIG_ACTIVE_HIGH << 7) | SH3001_VAL_INTERRUPT_CONFIG_ANY_READ ), 1);
// sh3001WriteRegisterBits(dev, SH3001_REG_INTERRUPT_CONT_LIM, SH3001_MASK_INTERRUPT_CONT_LIM, SH3001_VAL_SH3001_REG_INTERRUPT_CONT_LIM, 1);
sh3001WriteRegisterBits(dev, SH3001_REG_INT_PINMAP_1, SH3001_MASK_INT_PINMAP_1, SH3001_VAL_INT_PINMAP_1_GYRO_DRDY_PIN, 1);
// Read compensation coefficients
sh3001CompCoefInit(dev, &sh3001CompCoef);
// Make sure page 0 is selected
sh3001WriteRegister(dev, SH3001_REG_PAGE, 0x00, 1);
}
#ifdef USE_GYRO_EXTI
static void sh3001IntExtiInit(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, sh3001ExtiHandler);
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
EXTIEnable(mpuIntIO);
}
#endif
static void sh3001SpiGyroInit(gyroDev_t *gyro)
{
extDevice_t *dev = &gyro->dev;
sh3001Config(gyro);
#ifdef USE_GYRO_EXTI
sh3001IntExtiInit(gyro);
#endif
spiSetClkDivisor(dev, spiCalculateDivider(SH3001_MAX_SPI_CLK_HZ));
}
static void sh3001SpiAccInit(accDev_t *acc)
{
// sensor is configured during gyro init
acc->acc_1G = 512 * 4; // 16G sensor scale
}
bool sh3001SpiAccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor != SH3001_SPI) {
return false;
}
acc->initFn = sh3001SpiAccInit;
acc->readFn = sh3001AccRead;
return true;
}
bool sh3001SpiGyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != SH3001_SPI) {
return false;
}
gyro->initFn = sh3001SpiGyroInit;
gyro->readFn = sh3001GyroRead;
gyro->scale = GYRO_SCALE_2000DPS;
return true;
}
#endif // USE_ACCGYRO_SH3001

1
src/main/sensors/acceleration.h
View File

@ -49,6 +49,7 @@ typedef enum {
ACC_LSM6DS3,
ACC_LSM6DSL,
ACC_LSM6DSO,
ACC_SH3001,
ACC_FAKE
} accelerationSensor_e;

10
src/main/sensors/acceleration_init.c
View File

@ -53,6 +53,7 @@
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/accgyro/accgyro_spi_sh3001.h"
#ifdef USE_ACC_ADXL345
#include "drivers/accgyro_legacy/accgyro_adxl345.h"
@ -335,6 +336,15 @@ retry:
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_SH3001
case ACC_SH3001:
if (sh3001SpiAccDetect(dev)) {
accHardware = ACC_SH3001;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_FAKE_ACC
case ACC_FAKE:
if (fakeAccDetect(dev)) {

18
src/main/sensors/gyro_init.c
View File

@ -52,6 +52,7 @@
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/accgyro/accgyro_spi_sh3001.h"
#ifdef USE_GYRO_L3GD20
#include "drivers/accgyro/accgyro_spi_l3gd20.h"
@ -83,7 +84,7 @@
// The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer
// This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other
// gyro types are supported with SPI DMA.
#define GYRO_BUF_SIZE 32
#define GYRO_BUF_SIZE 64
static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK;
@ -339,6 +340,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
case GYRO_LSM6DS3:
case GYRO_LSM6DSL:
case GYRO_LSM6DSO:
case GYRO_SH3001:
gyroSensor->gyroDev.gyroHasOverflowProtection = true;
break;
@ -534,6 +536,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_SH3001
case GYRO_SH3001:
if (sh3001SpiGyroDetect(dev)) {
gyroHardware = GYRO_SH3001;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_FAKE_GYRO
case GYRO_FAKE:
if (fakeGyroDetect(dev)) {
@ -560,7 +571,7 @@ 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_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
|| defined(USE_ACCGYRO_LSM6DS3) || defined(USE_ACCGYRO_LSM6DSL) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_ACCGYRO_SH3001) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
@ -585,7 +596,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_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DS3) || defined(USE_ACCGRYO_LSM6DSL) ||defined(USE_ACCGRYO_LSM6DSO) \
|| defined(USE_ACCGYRO_SH3001)
mpuPreInit(config);
#else
UNUSED(config);

1
src/main/target/AT32F437DEV/target.h
View File

@ -112,6 +112,7 @@
#define USE_ACCGYRO_LSM6DS3
#define USE_ACCGYRO_LSM6DSL
#define USE_ACCGYRO_LSM6DSO
#define USE_ACCGYRO_SH3001
#define USE_ACC
#define USE_ACC_SPI_MPU6500

2
src/main/target/AT32F437DEV/target.mk
View File

@ -17,6 +17,8 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_lsm6dsl.c \
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
drivers/accgyro/accgyro_spi_lsm6dso.c \
drivers/accgyro/accgyro_spi_sh3001_init.c \
drivers/accgyro/accgyro_spi_sh3001.c \
drivers/max7456.c \
drivers/vtx_rtc6705.c \
drivers/vtx_rtc6705_soft_spi.c \

1
src/main/target/EMSRPROTO2/target.h
View File

@ -105,6 +105,7 @@
#define USE_ACCGYRO_BMI270
#define USE_ACCGYRO_LSM6DSL
#define USE_ACCGYRO_LSM6DSO
#define USE_ACCGYRO_SH3001

2
src/main/target/EMSRPROTO2/target.mk
View File

@ -14,5 +14,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_lsm6dsl.c \
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
drivers/accgyro/accgyro_spi_lsm6dso.c \
drivers/accgyro/accgyro_spi_sh3001_init.c \
drivers/accgyro/accgyro_spi_sh3001.c \
drivers/max7456.c \

1
src/main/target/NEUTRONRCF435MINI/target.h
View File

@ -91,6 +91,7 @@
#define USE_GYRO_SPI_ICM42688P
#define USE_ACCGYRO_BMI270
#define USE_ACCGYRO_LSM6DSO
#define USE_ACCGYRO_SH3001
#define USE_ACC
#define USE_ACC_SPI_ICM42688P

2
src/main/target/NEUTRONRCF435MINI/target.mk
View File

@ -17,6 +17,8 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_bmi270.c\
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
drivers/accgyro/accgyro_spi_lsm6dso.c \
drivers/accgyro/accgyro_spi_sh3001_init.c \
drivers/accgyro/accgyro_spi_sh3001.c \
drivers/max7456.c \
drivers/vtx_rtc6705.c \
drivers/vtx_rtc6705_soft_spi.c \

1
src/main/target/NEUTRONRCF435SE/target.h
View File

@ -95,6 +95,7 @@
#define USE_ACCGYRO_BMI270
#define USE_ACCGYRO_LSM6DSL
#define USE_ACCGYRO_LSM6DSO
#define USE_ACCGYRO_SH3001
#define USE_ACC
#define USE_ACC_SPI_ICM42688P

2
src/main/target/NEUTRONRCF435SE/target.mk
View File

@ -19,6 +19,8 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_lsm6dsl.c \
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
drivers/accgyro/accgyro_spi_lsm6dso.c \
drivers/accgyro/accgyro_spi_sh3001_init.c \
drivers/accgyro/accgyro_spi_sh3001.c \
drivers/max7456.c \
drivers/max7456.c \
drivers/vtx_rtc6705.c \