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

Gyro Sensors: Remove 32kHz gyro sampling mode(s) and associated code

This commit is contained in:
AJ Christensen 2019-02-12 15:15:44 +13:00
parent 988d0a9b5c
commit 4fdee6ec1c
19 changed files with 17 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/blackbox/blackbox.c
View File

@ -1324,9 +1324,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_hardware_lpf", "%d", gyroConfig()->gyro_hardware_lpf);
#ifdef USE_32K_CAPABLE_GYRO
BLACKBOX_PRINT_HEADER_LINE("gyro_32khz_hardware_lpf", "%d", gyroConfig()->gyro_32khz_hardware_lpf);
#endif
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_lowpass_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d", gyroConfig()->gyro_lowpass2_type);

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

@ -247,13 +247,6 @@ static const char * const lookupTableGyroHardwareLpf[] = {
#endif
};
#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
static const char * const lookupTableGyro32khzHardwareLpf[] = {
"NORMAL",
"EXPERIMENTAL"
};
#endif
#ifdef USE_CAMERA_CONTROL
static const char * const lookupTableCameraControlMode[] = {
"HARDWARE_PWM",
@ -487,9 +480,6 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRxSpi),
#endif
LOOKUP_TABLE_ENTRY(lookupTableGyroHardwareLpf),
#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
LOOKUP_TABLE_ENTRY(lookupTableGyro32khzHardwareLpf),
#endif
LOOKUP_TABLE_ENTRY(lookupTableAccHardware),
#ifdef USE_BARO
LOOKUP_TABLE_ENTRY(lookupTableBaroHardware),
@ -579,9 +569,6 @@ const lookupTableEntry_t lookupTables[] = {
const clivalue_t valueTable[] = {
// PG_GYRO_CONFIG
{ "gyro_hardware_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_hardware_lpf) },
#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
{ "gyro_32khz_hardware_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_32KHZ_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_32khz_hardware_lpf) },
#endif
#if defined(USE_GYRO_SPI_ICM20649)
{ "gyro_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
#endif
@ -609,9 +596,6 @@ const clivalue_t valueTable[] = {
{ "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 500, 1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
#endif
#if defined(GYRO_USES_SPI) && defined(USE_32K_CAPABLE_GYRO)
{ "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) },
#endif
#ifdef USE_MULTI_GYRO
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif

3
src/main/cli/settings.h
View File

@ -52,9 +52,6 @@ typedef enum {
TABLE_RX_SPI,
#endif
TABLE_GYRO_HARDWARE_LPF,
#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
TABLE_GYRO_32KHZ_HARDWARE_LPF,
#endif
TABLE_ACC_HARDWARE,
#ifdef USE_BARO
TABLE_BARO_HARDWARE,

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

@ -324,7 +324,7 @@ void mpuGyroInit(gyroDev_t *gyro)
uint8_t mpuGyroDLPF(gyroDev_t *gyro)
{
uint8_t ret = 0;
// If gyro is in 32KHz mode then the DLPF bits aren't used
if (gyro->gyroRateKHz <= GYRO_RATE_8_kHz) {
switch (gyro->hardware_lpf) {
@ -342,7 +342,7 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro)
case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
ret = 1;
break;
case GYRO_HARDWARE_LPF_NORMAL:
default:
ret = 0;
@ -352,23 +352,6 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro)
return ret;
}
uint8_t mpuGyroFCHOICE(gyroDev_t *gyro)
{
if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) {
#ifdef USE_GYRO_DLPF_EXPERIMENTAL
if (gyro->hardware_32khz_lpf == GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL) {
return FCB_8800_32;
} else {
return FCB_3600_32;
}
#else
return FCB_3600_32;
#endif
} else {
return FCB_DISABLED; // Not in 32KHz mode, set FCHOICE to select 8KHz sampling
}
}
#ifdef USE_GYRO_REGISTER_DUMP
uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg)
{

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

@ -156,12 +156,6 @@ enum icm_high_range_gyro_fsr_e {
NUM_ICM_HIGH_RANGE_GYRO_FSR
};
enum fchoice_b {
FCB_DISABLED = 0x00,
FCB_8800_32 = 0x01,
FCB_3600_32 = 0x02
};
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
@ -226,7 +220,6 @@ bool mpuGyroReadSPI(struct gyroDev_s *gyro);
void mpuPreInit(const struct gyroDeviceConfig_s *config);
void mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
uint8_t mpuGyroDLPF(struct gyroDev_s *gyro);
uint8_t mpuGyroFCHOICE(struct gyroDev_s *gyro);
uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg);
struct accDev_s;

2
src/main/drivers/accgyro/accgyro_mpu6500.c
View File

@ -72,7 +72,7 @@ void mpu6500GyroInit(gyroDev_t *gyro)
delay(100);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, gyro_range << 3 | mpuGyroFCHOICE(gyro));
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, gyro_range << 3);
delay(15);
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, accel_range << 3);
delay(15);

2
src/main/drivers/accgyro/accgyro_spi_icm20689.c
View File

@ -129,7 +129,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
// delay(100);
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro));
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delay(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);

2
src/main/drivers/accgyro/accgyro_spi_mpu9250.c
View File

@ -131,7 +131,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro));
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));

9
src/main/drivers/accgyro/gyro_sync.c
View File

@ -47,16 +47,12 @@ bool gyroSyncCheckUpdate(gyroDev_t *gyro)
return ret;
}
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz)
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator)
{
float gyroSamplePeriod;
if (lpf != GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
if (gyro_use_32khz) {
gyro->gyroRateKHz = GYRO_RATE_32_kHz;
gyroSamplePeriod = 31.25f;
} else {
switch (gyro->mpuDetectionResult.sensor) {
switch (gyro->mpuDetectionResult.sensor) {
case BMI_160_SPI:
gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
gyroSamplePeriod = 312.0f;
@ -69,7 +65,6 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
gyroSamplePeriod = 125.0f;
break;
}
}
} else {
switch (gyro->mpuDetectionResult.sensor) {

2
src/main/drivers/accgyro/gyro_sync.h
View File

@ -30,4 +30,4 @@
#include "drivers/accgyro/accgyro.h"
bool gyroSyncCheckUpdate(gyroDev_t *gyro);
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz);
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator);

16
src/main/fc/config.c
View File

@ -466,21 +466,11 @@ void validateAndFixGyroConfig(void)
if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
}
if (gyroConfig()->gyro_use_32khz) {
// F1 and F3 can't handle high sample speed.
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
#elif defined(STM32F3)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
#endif
} else {
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
#endif
}
float samplingTime;
switch (gyroMpuDetectionResult()->sensor) {
@ -504,9 +494,7 @@ void validateAndFixGyroConfig(void)
break;
}
}
if (gyroConfig()->gyro_use_32khz) {
samplingTime = 0.00003125;
}
// check for looptime restrictions based on motor protocol. Motor times have safety margin
float motorUpdateRestriction;

8
src/main/msp/msp.c
View File

@ -1370,7 +1370,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz
sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
@ -1392,7 +1392,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
sbufWriteU8(dst, gyroConfig()->gyro_32khz_hardware_lpf);
sbufWriteU8(dst, 0); // DEPRECATED: gyro_32khz_hardware_lpf
sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz);
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type);
@ -1979,7 +1979,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
motorConfigMutable()->digitalIdleOffsetValue = sbufReadU16(src);
}
if (sbufBytesRemaining(src)) {
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
sbufReadU8(src); // DEPRECATED: gyro_use_32khz
}
if (sbufBytesRemaining(src)) {
motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
@ -2015,7 +2015,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
}
if (sbufBytesRemaining(src) >= 10) {
gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
gyroConfigMutable()->gyro_32khz_hardware_lpf = sbufReadU8(src);
sbufReadU8(src); // DEPRECATED: gyro_32khz_hardware_lpf
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass2_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);

20
src/main/sensors/gyro.c
View File

@ -201,13 +201,11 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyroMovementCalibrationThreshold = 48;
gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
gyroConfig->gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL;
gyroConfig->gyro_lowpass_type = FILTER_PT1;
gyroConfig->gyro_lowpass_hz = 100;
gyroConfig->gyro_lowpass2_type = FILTER_PT1;
gyroConfig->gyro_lowpass2_hz = 300;
gyroConfig->gyro_high_fsr = false;
gyroConfig->gyro_use_32khz = false;
gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT;
gyroConfig->gyro_soft_notch_hz_1 = 0;
gyroConfig->gyro_soft_notch_cutoff_1 = 0;
@ -432,25 +430,9 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
return false;
}
switch (gyroHardware) {
case GYRO_MPU6500:
case GYRO_MPU9250:
case GYRO_ICM20601:
case GYRO_ICM20602:
case GYRO_ICM20608G:
case GYRO_ICM20689:
// do nothing, as gyro supports 32kHz
break;
default:
// gyro does not support 32kHz
gyroConfigMutable()->gyro_use_32khz = false;
break;
}
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom);
gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
gyroSensor->gyroDev.hardware_32khz_lpf = gyroConfig()->gyro_32khz_hardware_lpf;
gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
// As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug

2
src/main/sensors/gyro.h
View File

@ -72,10 +72,8 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_hardware_lpf; // gyro DLPF setting
uint8_t gyro_32khz_hardware_lpf; // gyro 32khz DLPF setting
uint8_t gyro_high_fsr;
uint8_t gyro_use_32khz;
uint8_t gyro_to_use;
uint16_t gyro_lowpass_hz;

7
src/main/target/ALIENWHOOP/config.c
View File

@ -104,13 +104,6 @@ void targetConfiguration(void)
systemConfigMutable()->cpu_overclock = 2; //216MHZ
/* Default to 32kHz enabled at 16/16 */
gyroConfigMutable()->gyro_use_32khz = 1; // enable 32kHz sampling
gyroConfigMutable()->gyroMovementCalibrationThreshold = 200; // aka moron_threshold
gyroConfigMutable()->gyro_sync_denom = 2; // 16kHz gyro
pidConfigMutable()->pid_process_denom = 1; // 16kHz PID
gyroConfigMutable()->gyro_lowpass2_hz = 751;
pidConfigMutable()->runaway_takeoff_prevention = false;
featureEnable((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM);

18
src/main/target/DALRCF722DUAL/config.c
View File

@ -20,20 +20,4 @@
#include <stdint.h>
#include "platform.h"
#ifdef TARGET_VALIDATECONFIG
#include "fc/config.h"
#include "sensors/gyro.h"
void targetValidateConfiguration(void)
{
if (gyroConfig()->gyro_use_32khz && gyroConfig()->gyroMovementCalibrationThreshold < 148) {
gyroConfigMutable()->gyroMovementCalibrationThreshold = 148;
}
}
#endif
#include "platform.h"

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

@ -19,7 +19,6 @@
*/
#pragma once
#define TARGET_VALIDATECONFIG
#define TARGET_BOARD_IDENTIFIER "DLF7"
#define USBD_PRODUCT_STRING "DALRCF722DUAL"

4
src/main/target/common_post.h
View File

@ -164,10 +164,6 @@
#define USE_USB_ADVANCED_PROFILES
#endif
// Determine if the target could have a 32KHz capable gyro
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
#define USE_32K_CAPABLE_GYRO
#endif
#if defined(USE_FLASH_W25M512)
#define USE_FLASH_W25M

2
src/test/unit/blackbox_unittest.cc
View File

@ -73,7 +73,7 @@ TEST(BlackboxTest, TestInitIntervals)
EXPECT_EQ(4096, blackboxSInterval);
// 1kHz PIDloop
gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_1KHZ_SAMPLE, 1, false);
gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_1KHZ_SAMPLE, 1);
targetPidLooptime = gyro.targetLooptime * 1;
blackboxInit();
EXPECT_EQ(32, blackboxIInterval);