More USE_ACC application

This commit is contained in:
jflyper 2019-02-04 21:59:41 +09:00
parent cc0e689bb5
commit 869e25d385
2 changed files with 17 additions and 19 deletions

View File

@ -82,26 +82,8 @@
#include "hardware_revision.h" #include "hardware_revision.h"
#endif #endif
FAST_RAM_ZERO_INIT acc_t acc; // acc access functions FAST_RAM_ZERO_INIT acc_t acc; // acc access functions
static float accumulatedMeasurements[XYZ_AXIS_COUNT];
static int accumulatedMeasurementCount;
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
extern uint16_t InflightcalibratingA;
extern bool AccInflightCalibrationMeasurementDone;
extern bool AccInflightCalibrationSavetoEEProm;
extern bool AccInflightCalibrationActive;
static flightDynamicsTrims_t *accelerationTrims;
static uint16_t accLpfCutHz = 0;
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 1);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
{ {
RESET_CONFIG_2(rollAndPitchTrims_t, rollAndPitchTrims, RESET_CONFIG_2(rollAndPitchTrims_t, rollAndPitchTrims,
@ -138,7 +120,23 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
resetFlightDynamicsTrims(&instance->accZero); resetFlightDynamicsTrims(&instance->accZero);
} }
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 1);
#ifdef USE_ACC #ifdef USE_ACC
extern uint16_t InflightcalibratingA;
extern bool AccInflightCalibrationMeasurementDone;
extern bool AccInflightCalibrationSavetoEEProm;
extern bool AccInflightCalibrationActive;
static float accumulatedMeasurements[XYZ_AXIS_COUNT];
static int accumulatedMeasurementCount;
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static flightDynamicsTrims_t *accelerationTrims;
static uint16_t accLpfCutHz = 0;
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{ {

View File

@ -73,7 +73,7 @@
#define USE_EXTI #define USE_EXTI
#define USE_ACC //#define USE_ACC
#define USE_ACC_SPI_MPU6000 #define USE_ACC_SPI_MPU6000
#define USE_GYRO #define USE_GYRO