Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Decoupling imu from config - acc deadband.

Browse source
This commit is contained in:
Dominic Clifton 2014-06-06 21:37:41 +01:00
parent 51eee3d62c
commit 3643c08475
6 changed files with 18 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/config/config.c
View file

@ -280,8 +280,8 @@ static void resetConf(void)
currentProfile.mag_declination = 0; currentProfile.mag_declination = 0;
currentProfile.acc_lpf_factor = 4; currentProfile.acc_lpf_factor = 4;
currentProfile.accz_deadband = 40; currentProfile.accDeadband.xy = 40;
currentProfile.accxy_deadband = 40; currentProfile.accDeadband.z = 40;
resetBarometerConfig(&currentProfile.barometerConfig); resetBarometerConfig(&currentProfile.barometerConfig);
@ -389,7 +389,7 @@ void activateConfig(void)
imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor; imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor; imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor;
configureImu(&imuRuntimeConfig, &currentProfile.pidProfile, &currentProfile.barometerConfig); configureImu(&imuRuntimeConfig, &currentProfile.pidProfile, &currentProfile.barometerConfig, &currentProfile.accDeadband);
calculateThrottleAngleScale(currentProfile.throttle_correction_angle); calculateThrottleAngleScale(currentProfile.throttle_correction_angle);

3
src/main/config/config_profile.h
View file

@ -34,8 +34,7 @@ typedef struct profile_s {
// sensor-related stuff // sensor-related stuff
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations accDeadband_t accDeadband;
uint8_t accxy_deadband; // set the acc deadband for xy-Axis
barometerConfig_t barometerConfig; barometerConfig_t barometerConfig;

10
src/main/flight/imu.c
View file

@ -107,12 +107,14 @@ void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
imuRuntimeConfig_t *imuRuntimeConfig; imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile; pidProfile_t *pidProfile;
barometerConfig_t *barometerConfig; barometerConfig_t *barometerConfig;
accDeadband_t *accDeadband;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig) void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband)
{ {
imuRuntimeConfig = initialImuRuntimeConfig; imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile; pidProfile = initialPidProfile;
barometerConfig = intialBarometerConfig; barometerConfig = intialBarometerConfig;
accDeadband = initialAccDeadband;
} }
void computeIMU(rollAndPitchTrims_t *accelerometerTrims) void computeIMU(rollAndPitchTrims_t *accelerometerTrims)
@ -255,9 +257,9 @@ void acc_calc(uint32_t deltaT)
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence // apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), currentProfile.accxy_deadband); accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), currentProfile.accxy_deadband); accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), currentProfile.accz_deadband); accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
// sum up Values for later integration to get velocity and distance // sum up Values for later integration to get velocity and distance
accTimeSum += deltaT; accTimeSum += deltaT;

2
src/main/flight/imu.h
View file

@ -27,7 +27,7 @@ typedef struct imuRuntimeConfig_s {
float gyro_cmpfm_factor; float gyro_cmpfm_factor;
} imuRuntimeConfig_t; } imuRuntimeConfig_t;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig); void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband);
int getEstimatedAltitude(void); int getEstimatedAltitude(void);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims); void computeIMU(rollAndPitchTrims_t *accelerometerTrims);

4
src/main/io/serial_cli.c
View file

@ -264,8 +264,8 @@ const clivalue_t valueTable[] = {
{ "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 }, { "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 },
{ "acc_lpf_factor", VAR_UINT8, &currentProfile.acc_lpf_factor, 0, 250 }, { "acc_lpf_factor", VAR_UINT8, &currentProfile.acc_lpf_factor, 0, 250 },
{ "accxy_deadband", VAR_UINT8, &currentProfile.accxy_deadband, 0, 100 }, { "accxy_deadband", VAR_UINT8, &currentProfile.accDeadband.xy, 0, 100 },
{ "accz_deadband", VAR_UINT8, &currentProfile.accz_deadband, 0, 100 }, { "accz_deadband", VAR_UINT8, &currentProfile.accDeadband.z, 0, 100 },
{ "acc_unarmedcal", VAR_UINT8, &currentProfile.acc_unarmedcal, 0, 1 }, { "acc_unarmedcal", VAR_UINT8, &currentProfile.acc_unarmedcal, 0, 1 },
{ "acc_trim_pitch", VAR_INT16, &currentProfile.accelerometerTrims.values.pitch, -300, 300 }, { "acc_trim_pitch", VAR_INT16, &currentProfile.accelerometerTrims.values.pitch, -300, 300 },
{ "acc_trim_roll", VAR_INT16, &currentProfile.accelerometerTrims.values.roll, -300, 300 }, { "acc_trim_roll", VAR_INT16, &currentProfile.accelerometerTrims.values.roll, -300, 300 },

5
src/main/sensors/acceleration.h
View file

@ -34,6 +34,11 @@ extern sensor_align_e accAlign;
extern acc_t acc; extern acc_t acc;
extern uint16_t acc_1G; extern uint16_t acc_1G;
typedef struct accDeadband_s {
uint8_t xy; // set the acc deadband for xy-Axis
uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations
} accDeadband_t;
bool isAccelerationCalibrationComplete(void); bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims); void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);