From 3643c08475e8407522e09ffadb199a5fa8af089a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 6 Jun 2014 21:37:41 +0100 Subject: [PATCH] Decoupling imu from config - acc deadband. --- src/main/config/config.c | 6 +++--- src/main/config/config_profile.h | 3 +-- src/main/flight/imu.c | 10 ++++++---- src/main/flight/imu.h | 2 +- src/main/io/serial_cli.c | 4 ++-- src/main/sensors/acceleration.h | 5 +++++ 6 files changed, 18 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b0b6aa89a..90999347e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -280,8 +280,8 @@ static void resetConf(void) currentProfile.mag_declination = 0; currentProfile.acc_lpf_factor = 4; - currentProfile.accz_deadband = 40; - currentProfile.accxy_deadband = 40; + currentProfile.accDeadband.xy = 40; + currentProfile.accDeadband.z = 40; resetBarometerConfig(¤tProfile.barometerConfig); @@ -389,7 +389,7 @@ void activateConfig(void) imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor; imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor; - configureImu(&imuRuntimeConfig, ¤tProfile.pidProfile, ¤tProfile.barometerConfig); + configureImu(&imuRuntimeConfig, ¤tProfile.pidProfile, ¤tProfile.barometerConfig, ¤tProfile.accDeadband); calculateThrottleAngleScale(currentProfile.throttle_correction_angle); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index feb2132bb..74149c028 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -34,8 +34,7 @@ typedef struct profile_s { // 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 accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations - uint8_t accxy_deadband; // set the acc deadband for xy-Axis + accDeadband_t accDeadband; barometerConfig_t barometerConfig; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 13592d900..d7acd2593 100755 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -107,12 +107,14 @@ void calculateThrottleAngleScale(uint16_t throttle_correction_angle) imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; 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; pidProfile = initialPidProfile; barometerConfig = intialBarometerConfig; + accDeadband = initialAccDeadband; } 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 // apply Deadband to reduce integration drift and vibration influence - accSum[X] += applyDeadband(lrintf(accel_ned.V.X), currentProfile.accxy_deadband); - accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), currentProfile.accxy_deadband); - accSum[Z] += applyDeadband(lrintf(accz_smooth), currentProfile.accz_deadband); + accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy); + accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy); + accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z); // sum up Values for later integration to get velocity and distance accTimeSum += deltaT; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index c52dd4840..acd7905df 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -27,7 +27,7 @@ typedef struct imuRuntimeConfig_s { float gyro_cmpfm_factor; } 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); void computeIMU(rollAndPitchTrims_t *accelerometerTrims); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f2792f0ac..74e31f533 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -264,8 +264,8 @@ const clivalue_t valueTable[] = { { "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 }, { "acc_lpf_factor", VAR_UINT8, ¤tProfile.acc_lpf_factor, 0, 250 }, - { "accxy_deadband", VAR_UINT8, ¤tProfile.accxy_deadband, 0, 100 }, - { "accz_deadband", VAR_UINT8, ¤tProfile.accz_deadband, 0, 100 }, + { "accxy_deadband", VAR_UINT8, ¤tProfile.accDeadband.xy, 0, 100 }, + { "accz_deadband", VAR_UINT8, ¤tProfile.accDeadband.z, 0, 100 }, { "acc_unarmedcal", VAR_UINT8, ¤tProfile.acc_unarmedcal, 0, 1 }, { "acc_trim_pitch", VAR_INT16, ¤tProfile.accelerometerTrims.values.pitch, -300, 300 }, { "acc_trim_roll", VAR_INT16, ¤tProfile.accelerometerTrims.values.roll, -300, 300 }, diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index f64b29e52..defb5f1a7 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -34,6 +34,11 @@ extern sensor_align_e accAlign; extern acc_t acc; 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); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);