Complete decoupling of imu code from config.
The giant list of unrelated includes is now gone and the dependencies are now clear.
This commit is contained in:
parent
deda79cb14
commit
0b9c326ffe
|
@ -37,26 +37,9 @@
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
#include "flight/mixer.h"
|
||||
|
||||
// FIXME remove dependency on config.h
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "rx/rx.h"
|
||||
#include "io/escservo.h"
|
||||
#include "io/rc_controls.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "io/serial.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/imu.h"
|
||||
|
||||
int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
|
||||
|
@ -117,7 +100,7 @@ void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *ini
|
|||
accDeadband = initialAccDeadband;
|
||||
}
|
||||
|
||||
void computeIMU(rollAndPitchTrims_t *accelerometerTrims)
|
||||
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration)
|
||||
{
|
||||
uint32_t axis;
|
||||
static int16_t gyroYawSmooth = 0;
|
||||
|
@ -132,7 +115,7 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims)
|
|||
accADC[Z] = 0;
|
||||
}
|
||||
|
||||
if (masterConfig.mixerConfiguration == MULTITYPE_TRI) {
|
||||
if (mixerConfiguration == MULTITYPE_TRI) {
|
||||
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
|
||||
gyroYawSmooth = gyroData[FD_YAW];
|
||||
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
|
||||
|
|
|
@ -31,6 +31,6 @@ typedef struct imuRuntimeConfig_s {
|
|||
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband);
|
||||
|
||||
int getEstimatedAltitude(void);
|
||||
void computeIMU(rollAndPitchTrims_t *accelerometerTrims);
|
||||
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration);
|
||||
void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||
|
|
|
@ -535,7 +535,7 @@ void loop(void)
|
|||
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
|
||||
loopTime = currentTime + masterConfig.looptime;
|
||||
|
||||
computeIMU(¤tProfile.accelerometerTrims);
|
||||
computeIMU(¤tProfile.accelerometerTrims, masterConfig.mixerConfiguration);
|
||||
annexCode();
|
||||
// Measure loop rate just afer reading the sensors
|
||||
currentTime = micros();
|
||||
|
|
|
@ -22,9 +22,6 @@
|
|||
|
||||
#define BARO
|
||||
|
||||
|
||||
// FIXME this giant list of includes (below) and stubs (bottom) indicates there is too much going on in flight_imu.c and that it needs decoupling and breaking up.
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "flight/flight.h"
|
||||
|
||||
|
@ -35,27 +32,9 @@
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
#include "flight/mixer.h"
|
||||
|
||||
// FIXME remove dependency on config.h
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "io/battery.h"
|
||||
#include "io/escservo.h"
|
||||
#include "io/rc_controls.h"
|
||||
#include "rx/rx.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "io/serial.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/imu.h"
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
@ -104,8 +83,6 @@ TEST(FlightImuTest, IsThrustFacingDownwards)
|
|||
// STUBS
|
||||
|
||||
uint16_t acc_1G;
|
||||
profile_t currentProfile;
|
||||
master_t masterConfig;
|
||||
int16_t heading;
|
||||
flags_t f;
|
||||
gyro_t gyro;
|
||||
|
|
Loading…
Reference in New Issue