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

Decoupling imu from config - barometer config.

Browse source
This commit is contained in:
Dominic Clifton 2014-06-06 20:59:59 +01:00
parent 82bb6b7982
commit 51eee3d62c
4 changed files with 14 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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); configureImu(&imuRuntimeConfig, &currentProfile.pidProfile, &currentProfile.barometerConfig);
calculateThrottleAngleScale(currentProfile.throttle_correction_angle); calculateThrottleAngleScale(currentProfile.throttle_correction_angle);

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

@ -106,11 +106,13 @@ void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
imuRuntimeConfig_t *imuRuntimeConfig; imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile; pidProfile_t *pidProfile;
barometerConfig_t *barometerConfig;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile) void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig)
{ {
imuRuntimeConfig = initialImuRuntimeConfig; imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile; pidProfile = initialPidProfile;
barometerConfig = intialBarometerConfig;
} }
void computeIMU(rollAndPitchTrims_t *accelerometerTrims) void computeIMU(rollAndPitchTrims_t *accelerometerTrims)
@ -455,7 +457,7 @@ int getEstimatedAltitude(void)
// Integrator - Altitude in cm // Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * currentProfile.barometerConfig.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.barometerConfig.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
EstAlt = accAlt; EstAlt = accAlt;
vel += vel_acc; vel += vel_acc;
@ -475,7 +477,7 @@ int getEstimatedAltitude(void)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * currentProfile.barometerConfig.baro_cf_vel + baroVel * (1 - currentProfile.barometerConfig.baro_cf_vel); vel = vel * barometerConfig->baro_cf_vel + baroVel * (1 - barometerConfig->baro_cf_vel);
vel_tmp = lrintf(vel); vel_tmp = lrintf(vel);
// set vario // set vario

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); void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig);
int getEstimatedAltitude(void); int getEstimatedAltitude(void);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims); void computeIMU(rollAndPitchTrims_t *accelerometerTrims);

14
src/main/mw.c
View file

@ -32,23 +32,23 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/sonar.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/gyro.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "io/buzzer.h" #include "io/buzzer.h"
#include "io/escservo.h" #include "io/escservo.h"
#include "flight/flight.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/autotune.h" #include "flight/autotune.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/gps.h" #include "io/gps.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/gyro.h"
#include "io/serial_cli.h" #include "io/serial_cli.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/statusindicator.h" #include "io/statusindicator.h"