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

Configurable gyro Denominator when gyro_lpf set to OFF

This commit is contained in:
borisbstyle 2016-02-06 13:09:40 +01:00
parent 9f15da0641
commit cdb671b0d6
9 changed files with 46 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/config/config.c
View File

@ -400,6 +400,7 @@ static void resetConf(void)
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 1; // 188HZ
masterConfig.gyro_sync_denom = 1;
masterConfig.gyro_soft_lpf_hz = 60;
resetAccelerometerTrims(&masterConfig.accZero);

1
src/main/config/config_master.h
View File

@ -58,6 +58,7 @@ typedef struct master_t {
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_sync_denom; // Gyro sample divider
float gyro_soft_lpf_hz; // Biqyad gyro lpf hz
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

19
src/main/drivers/gyro_sync.c
View File

@ -41,27 +41,14 @@ bool gyroSyncCheckUpdate(void) {
return getMpuDataStatus(&gyro);
}
void gyroUpdateSampleRate(uint8_t lpf) {
int gyroSamplePeriod, gyroSyncDenominator;
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
int gyroSamplePeriod;
if (!lpf) {
gyroSamplePeriod = 125;
#ifdef STM32F303xC
#ifdef COLIBRI_RACE // Leave out LUX target for now. Need to test 2.6Khz
gyroSyncDenominator = 3; // Sample every 3d gyro measurement 2,6khz
#else
gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
#endif
#else
if (!sensors(SENSOR_ACC) && !sensors(SENSOR_BARO) && !sensors(SENSOR_MAG)) {
gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
} else {
gyroSyncDenominator = 8; // Sample every 8th gyro measurement 1khz
}
#endif
} else {
gyroSamplePeriod = 1000;
gyroSyncDenominator = 1; // Full Sampling 1khz
gyroSyncDenominator = 1; // Always full Sampling 1khz
}
// calculate gyro divider and targetLooptime (expected cycleTime)

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

@ -11,4 +11,4 @@ extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxGetDividerDrops(void);
void gyroUpdateSampleRate(uint8_t lpf);
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

1
src/main/io/serial_cli.c
View File

@ -597,6 +597,7 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_soft_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },

63
src/main/io/serial_msp.c
View File

@ -101,6 +101,39 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
extern uint16_t rssi; // FIXME dependency on mw.c
extern void resetPidProfile(pidProfile_t *pidProfile);
void setGyroSamplingSpeed(uint16_t looptime) {
uint16_t gyroSampleRate = 1000;
uint8_t maxDivider = 1;
if (looptime != targetLooptime) {
#ifdef STM32F303xC
if (looptime < 1000) {
masterConfig.gyro_lpf = 1;
gyroSampleRate = 125;
maxDivider = 8;
} else {
masterConfig.gyro_lpf = 1;
}
#else
if (looptime < 1000) {
masterConfig.gyro_lpf = 0;
masterConfig.acc_hardware = 1;
masterConfig.baro_hardware = 1;
masterConfig.mag_hardware = 1;
gyroSampleRate = 125;
maxDivider = 8;
} else {
masterConfig.gyro_lpf = 1;
masterConfig.acc_hardware = 0;
masterConfig.baro_hardware = 0;
masterConfig.mag_hardware = 0;
}
#endif
}
masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
@ -1203,7 +1236,6 @@ static bool processInCommand(void)
uint32_t i;
uint16_t tmp;
uint8_t rate;
uint16_t gyroRefreshRate;
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
@ -1248,34 +1280,7 @@ static bool processInCommand(void)
masterConfig.disarm_kill_switch = read8();
break;
case MSP_SET_LOOP_TIME:
gyroRefreshRate = read16();
if (gyroRefreshRate != targetLooptime) {
switch (gyroRefreshRate) {
#ifdef STM32F303xC
default:
case(1000):
masterConfig.gyro_lpf = 1;
break;
case(500):
masterConfig.gyro_lpf = 0;
break;
#else
default:
case(1000):
masterConfig.gyro_lpf = 1;
masterConfig.acc_hardware = 0;
masterConfig.baro_hardware = 0;
masterConfig.mag_hardware = 0;
break;
case(500):
masterConfig.gyro_lpf = 0;
masterConfig.acc_hardware = 1;
masterConfig.baro_hardware = 1;
masterConfig.mag_hardware = 1;
break;
#endif
}
}
setGyroSamplingSpeed(read16());
break;
case MSP_SET_PID_CONTROLLER:
currentProfile->pidProfile.pidController = constrain(read8(), 1, 2);

2
src/main/main.c
View File

@ -455,7 +455,7 @@ void init(void)
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf)) {
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}

4
src/main/sensors/initialisation.c
View File

@ -689,7 +689,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator)
{
int16_t deg, min;
@ -715,7 +715,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyroUpdateSampleRate(gyroLpf); // Set gyro refresh rate before initialisation
gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation
gyro.init(gyroLpf);
detectMag(magHardwareToUse);

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

@ -17,4 +17,4 @@
#pragma once
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator);