rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1012 from martinbudden/bf_acc_init 

Improved acc initialisation
This commit is contained in:
borisbstyle 2016-08-15 21:49:54 +02:00 committed by GitHub
parent 9b565384f8 c077bacee6
commit f664bfecbc
7 changed files with 82 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/config/config_master.h
View File

@ -78,7 +78,7 @@ typedef struct master_t {
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
float acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
accDeadband_t accDeadband;
barometerConfig_t barometerConfig;
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off

2
src/main/io/serial_cli.c
View File

@ -820,7 +820,7 @@ const clivalue_t valueTable[] = {
#endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
{ "acc_lpf_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } },
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } },
{ "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0, 100 } },
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0, 100 } },
{ "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } },

17
src/main/main.c
View File

@ -683,22 +683,7 @@ void main_init(void)
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
switch (gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future
case 500:
case 375:
case 250:
case 125:
accTargetLooptime = 1000;
break;
default:
case 1000:
#ifdef STM32F10X
accTargetLooptime = 1000;
#else
accTargetLooptime = 1000;
#endif
}
rescheduleTask(TASK_ACCEL, accTargetLooptime);
rescheduleTask(TASK_ACCEL, accSamplingInterval);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));

52
src/main/sensors/acceleration.c
View File

@ -41,7 +41,7 @@ int32_t accSmooth[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
sensor_align_e accAlign = 0;
uint32_t accTargetLooptime;
uint32_t accSamplingInterval;
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.
@ -52,9 +52,33 @@ extern bool AccInflightCalibrationActive;
static flightDynamicsTrims_t *accelerationTrims;
static float accLpfCutHz = 0;
static uint16_t accLpfCutHz = 0;
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
static bool accFilterInitialised = false;
void accInit(uint32_t gyroSamplingInverval)
{
// set the acc sampling interval according to the gyro sampling interval
switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future
case 500:
case 375:
case 250:
case 125:
accSamplingInterval = 1000;
break;
case 1000:
default:
#ifdef STM32F10X
accSamplingInterval = 1000;
#else
accSamplingInterval = 1000;
#endif
}
if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval);
}
}
}
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
@ -188,19 +212,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
}
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
}
@ -222,7 +235,12 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
accelerationTrims = accelerationTrimsToUse;
}
void setAccelerationFilter(float initialAccLpfCutHz)
void setAccelerationFilter(uint16_t initialAccLpfCutHz)
{
accLpfCutHz = initialAccLpfCutHz;
if (accSamplingInterval) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval);
}
}
}

9
src/main/sensors/acceleration.h
View File

@ -34,7 +34,7 @@ typedef enum {
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint32_t accTargetLooptime;
extern uint32_t accSamplingInterval;
extern int32_t accSmooth[XYZ_AXIS_COUNT];
@ -48,9 +48,12 @@ typedef union rollAndPitchTrims_u {
rollAndPitchTrims_t_def values;
} rollAndPitchTrims_t;
void accInit(uint32_t gyroTargetLooptime);
bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse);
void setAccelerationFilter(float initialAccLpfCutHz);
union flightDynamicsTrims_u;
void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse);
void setAccelerationFilter(uint16_t initialAccLpfCutHz);

4
src/main/sensors/gyro.c
View File

@ -103,7 +103,7 @@ void gyroSetCalibrationCycles(void)
calibratingG = gyroCalculateCalibratingCycles();
}
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
{
static int32_t g[3];
static stdev_t var[3];
@ -166,7 +166,7 @@ void gyroUpdate(void)
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();

61
src/main/sensors/initialisation.c
View File

@ -277,7 +277,7 @@ bool detectGyro(void)
return true;
}
static void detectAcc(accelerationSensor_e accHardwareToUse)
static bool detectAcc(accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware;
@ -407,24 +407,22 @@ retry:
if (accHardware == ACC_NONE) {
return;
return false;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
return true;
}
static void detectBaro(baroSensor_e baroHardwareToUse)
#ifdef BARO
static bool detectBaro(baroSensor_e baroHardwareToUse)
{
#ifndef BARO
UNUSED(baroHardwareToUse);
#else
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = baroHardwareToUse;
#ifdef USE_BARO_BMP085
const bmp085Config_t *bmp085Config = NULL;
#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
@ -476,15 +474,17 @@ static void detectBaro(baroSensor_e baroHardwareToUse)
}
if (baroHardware == BARO_NONE) {
return;
return false;
}
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
#endif
return true;
}
#endif
static void detectMag(magSensor_e magHardwareToUse)
#ifdef MAG
static bool detectMag(magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
@ -571,12 +571,14 @@ retry:
}
if (magHardware == MAG_NONE) {
return;
return false;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
return true;
}
#endif
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
{
@ -613,32 +615,41 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
if (!detectGyro()) {
return false;
}
detectAcc(accHardwareToUse);
detectBaro(baroHardwareToUse);
// Now time to init things, acc first
if (sensors(SENSOR_ACC)) {
acc.acc_1G = 256; // set default
acc.init(&acc);
}
// Now time to init things
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation
gyro.init(gyroLpf);
gyroInit();
gyro.init(gyroLpf); // driver initialisation
gyroInit(); // sensor initialisation
detectMag(magHardwareToUse);
if (detectAcc(accHardwareToUse)) {
acc.acc_1G = 256; // set default
acc.init(&acc); // driver initialisation
accInit(gyro.targetLooptime); // sensor initialisation
}
reconfigureAlignment(sensorAlignmentConfig);
magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
#ifdef MAG
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (sensors(SENSOR_MAG)) {
if (detectMag(magHardwareToUse)) {
// calculate magnetic declination
const int16_t deg = magDeclinationFromConfig / 100;
const int16_t min = magDeclinationFromConfig % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
} else {
magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
}
#else
UNUSED(magHardwareToUse);
UNUSED(magDeclinationFromConfig);
#endif
#ifdef BARO
detectBaro(baroHardwareToUse);
#else
UNUSED(baroHardwareToUse);
#endif
reconfigureAlignment(sensorAlignmentConfig);
return true;
}