From c077bacee6c4c5bcf0f2d7a373f093031f4d1942 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 15 Aug 2016 07:32:06 +0100 Subject: [PATCH] Improved acc initialisation --- src/main/config/config_master.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/main.c | 17 +-------- src/main/sensors/acceleration.c | 52 +++++++++++++++++--------- src/main/sensors/acceleration.h | 9 +++-- src/main/sensors/gyro.c | 4 +- src/main/sensors/initialisation.c | 61 ++++++++++++++++++------------- 7 files changed, 82 insertions(+), 65 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 0f79ee2f4..ad6cc09b5 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a3463f9de..f8491f4f2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 } }, diff --git a/src/main/main.c b/src/main/main.c index 9c4336e7c..94302e61d 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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)); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 3016e9732..07cd90b20 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -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); + } + } } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index dc98ae78e..4630faa3b 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -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); + diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c6d4c121a..bc1cae650 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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(); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index a122c8aac..6e96068f4 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -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; }