From 4d238b27d5d33ed3145b9c5b0424e89ed7380409 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 08:41:24 +0100 Subject: [PATCH 1/3] Moved targetLooptime into gyro_t, tidied gyro_sync and gyro --- src/main/blackbox/blackbox.c | 5 +--- src/main/blackbox/blackbox_io.c | 1 - src/main/config/config.c | 1 - src/main/drivers/accgyro.h | 1 + src/main/drivers/accgyro_mpu.c | 10 ++++--- src/main/drivers/accgyro_mpu.h | 2 +- src/main/drivers/accgyro_mpu3050.c | 1 - src/main/drivers/accgyro_mpu6050.c | 2 +- src/main/drivers/gyro_sync.c | 44 +++++++++++++----------------- src/main/drivers/gyro_sync.h | 7 ++--- src/main/drivers/sensor.h | 2 +- src/main/flight/mixer.c | 1 - src/main/flight/pid.c | 3 +- src/main/io/osd.c | 1 - src/main/io/rc_controls.c | 1 - src/main/io/serial_cli.c | 1 - src/main/io/serial_msp.c | 7 ++--- src/main/main.c | 5 ++-- src/main/mw.c | 2 +- src/main/rx/rx.c | 1 - src/main/sensors/acceleration.c | 1 - src/main/sensors/gyro.c | 20 +++++++------- src/main/sensors/gyro.h | 2 -- src/main/sensors/initialisation.c | 3 +- 24 files changed, 52 insertions(+), 72 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ed88dd40b..46bae1958 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -325,9 +325,6 @@ extern uint32_t currentTime; //From rx.c: extern uint16_t rssi; -//From gyro.c -extern uint32_t targetLooptime; - //From rc_controls.c extern uint32_t rcModeActivationMask; @@ -1169,7 +1166,7 @@ static bool blackboxWriteSysinfo() } ); - BLACKBOX_PRINT_HEADER_LINE("looptime:%d", targetLooptime); + BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 82c105eb4..7f9b1ec5b 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -22,7 +22,6 @@ #include "drivers/accgyro.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" -#include "drivers/gyro_sync.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index c371ad294..b7d404d3d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -38,7 +38,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/serial.h" -#include "drivers/gyro_sync.h" #include "drivers/pwm_output.h" #include "drivers/max7456.h" diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 385ea90b9..3d90de1bc 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -27,6 +27,7 @@ typedef struct gyro_s { sensorReadFuncPtr temperature; // read temperature if available sensorInterruptFuncPtr intStatus; float scale; // scalefactor + uint32_t targetLooptime; } gyro_t; typedef struct acc_s { diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 0882581d3..1dbe97609 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -32,7 +32,6 @@ #include "gpio.h" #include "exti.h" #include "bus_i2c.h" -#include "gyro_sync.h" #include "sensor.h" #include "accgyro.h" @@ -300,11 +299,14 @@ bool mpuGyroRead(int16_t *gyroADC) return true; } -void checkMPUDataReady(bool *mpuDataReadyPtr) { +bool checkMPUDataReady(void) +{ + bool ret; if (mpuDataReady) { - *mpuDataReadyPtr = true; + ret = true; mpuDataReady= false; } else { - *mpuDataReadyPtr = false; + ret = false; } + return ret; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 6dbfcebf6..20389147e 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -185,4 +185,4 @@ void mpuIntExtiInit(void); bool mpuAccRead(int16_t *accData); bool mpuGyroRead(int16_t *gyroADC); mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse); -void checkMPUDataReady(bool *mpuDataReadyPtr); +bool checkMPUDataReady(void); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index c97472815..cefaf94b2 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -30,7 +30,6 @@ #include "accgyro.h" #include "accgyro_mpu.h" #include "accgyro_mpu3050.h" -#include "gyro_sync.h" // MPU3050, Standard address 0x68 #define MPU3050_ADDRESS 0x68 diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index d2717b135..d4fa0fb84 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -1,4 +1,4 @@ -/* +/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 934708f37..61305e181 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -4,48 +4,40 @@ * Created on: 3 aug. 2015 * Author: borisb */ + #include #include -#include #include "platform.h" #include "build_config.h" -#include "common/axis.h" -#include "common/maths.h" - #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/gyro_sync.h" -#include "sensors/sensors.h" -#include "sensors/acceleration.h" - -#include "config/runtime_config.h" -#include "config/config.h" - -extern gyro_t gyro; - -uint32_t targetLooptime; static uint8_t mpuDividerDrops; -bool getMpuDataStatus(gyro_t *gyro) +bool gyroSyncCheckUpdate(const gyro_t *gyro) { - bool mpuDataStatus; if (!gyro->intStatus) - return false; - gyro->intStatus(&mpuDataStatus); - return mpuDataStatus; + return false; + return gyro->intStatus(); } -bool gyroSyncCheckUpdate(void) { - return getMpuDataStatus(&gyro); -} +#define GYRO_LPF_256HZ 0 +#define GYRO_LPF_188HZ 1 +#define GYRO_LPF_98HZ 2 +#define GYRO_LPF_42HZ 3 +#define GYRO_LPF_20HZ 4 +#define GYRO_LPF_10HZ 5 +#define GYRO_LPF_5HZ 6 +#define GYRO_LPF_NONE 7 -void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) { +uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) +{ int gyroSamplePeriod; - if (!lpf || lpf == 7) { + if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) { gyroSamplePeriod = 125; } else { gyroSamplePeriod = 1000; @@ -54,9 +46,11 @@ void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) { // calculate gyro divider and targetLooptime (expected cycleTime) mpuDividerDrops = gyroSyncDenominator - 1; - targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod; + const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod; + return targetLooptime; } -uint8_t gyroMPU6xxxGetDividerDrops(void) { +uint8_t gyroMPU6xxxGetDividerDrops(void) +{ return mpuDividerDrops; } diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 5c229c689..f682e218c 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -5,8 +5,7 @@ * Author: borisb */ -extern uint32_t targetLooptime; - -bool gyroSyncCheckUpdate(void); +struct gyro_s; +bool gyroSyncCheckUpdate(const struct gyro_s *gyro); uint8_t gyroMPU6xxxGetDividerDrops(void); -void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); +uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 6554f8267..7ae17f510 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -22,4 +22,4 @@ typedef void (*sensorInitFuncPtr)(void); // sensor init proto typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype -typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready +typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e67723789..adc0145ba 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -36,7 +36,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/system.h" -#include "drivers/gyro_sync.h" #include "rx/rx.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e1ba58227..91ccbc2d8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -29,7 +29,6 @@ #include "common/filter.h" #include "drivers/sensor.h" -#include "drivers/gyro_sync.h" #include "drivers/accgyro.h" #include "sensors/sensors.h" @@ -74,7 +73,7 @@ pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we void setTargetPidLooptime(uint8_t pidProcessDenom) { - targetPidLooptime = targetLooptime * pidProcessDenom; + targetPidLooptime = gyro.targetLooptime * pidProcessDenom; } uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index db4c8ad4e..69f3f5691 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -52,7 +52,6 @@ #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" #include "drivers/sonar_hcsr04.h" -#include "drivers/gyro_sync.h" #include "drivers/usb_io.h" #include "drivers/transponder_ir.h" #include "drivers/sdcard.h" diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 23a87e764..7eec0de89 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -34,7 +34,6 @@ #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" #include "sensors/barometer.h" #include "sensors/battery.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 237a5310f..87df64634 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -48,7 +48,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" -#include "drivers/gyro_sync.h" #include "drivers/buf_writer.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3bd8d1441..9f2609c0a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -40,7 +40,6 @@ #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/buf_writer.h" #include "drivers/max7456.h" @@ -109,8 +108,8 @@ void setGyroSamplingSpeed(uint16_t looptime) { uint16_t gyroSampleRate = 1000; uint8_t maxDivider = 1; - if (looptime != targetLooptime || looptime == 0) { - if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes + if (looptime != gyro.targetLooptime || looptime == 0) { + if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes #ifdef STM32F303xC if (looptime < 1000) { masterConfig.gyro_lpf = 0; @@ -854,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LOOP_TIME: headSerialReply(2); - serialize16((uint16_t)targetLooptime); + serialize16((uint16_t)gyro.targetLooptime); break; case MSP_RC_TUNING: headSerialReply(11); diff --git a/src/main/main.c b/src/main/main.c index dba36d3f4..686e39504 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -49,7 +49,6 @@ #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" #include "drivers/sonar_hcsr04.h" -#include "drivers/gyro_sync.h" #include "drivers/sdcard.h" #include "drivers/usb_io.h" #include "drivers/transponder_ir.h" @@ -694,12 +693,12 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, targetLooptime); + rescheduleTask(TASK_GYROPID, gyro.targetLooptime); setTaskEnabled(TASK_GYROPID, true); if(sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); - switch(targetLooptime) { // Switch statement kept in place to change acc rates in the future + switch(gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future case(500): case(375): case(250): diff --git a/src/main/mw.c b/src/main/mw.c index 5b282c3e8..31d6e22c7 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -800,7 +800,7 @@ void taskMainPidLoopCheck(void) const uint32_t startTime = micros(); while (true) { - if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) { + if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) { static uint8_t pidUpdateCountdown; if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5d36956d4..255fb547e 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -41,7 +41,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" -#include "drivers/gyro_sync.h" #include "rx/pwm.h" #include "rx/sbus.h" #include "rx/spektrum.h" diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 320d00ddd..e4471fe30 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -28,7 +28,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/system.h" -#include "drivers/gyro_sync.h" #include "sensors/battery.h" #include "sensors/sensors.h" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index bce60a1fc..5cd741526 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -28,7 +28,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" #include "sensors/sensors.h" #include "io/beeper.h" #include "io/statusindicator.h" @@ -36,29 +35,30 @@ #include "sensors/gyro.h" -uint16_t calibratingG = 0; +gyro_t gyro; // gyro access functions +sensor_align_e gyroAlign = 0; + int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; +static uint16_t calibratingG = 0; +static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; static biquad_t gyroFilterState[3]; static bool gyroFilterStateIsSet; static float gyroLpfCutFreq; -gyro_t gyro; // gyro access functions -sensor_align_e gyroAlign = 0; - void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) { gyroConfig = gyroConfigToUse; gyroLpfCutFreq = gyro_lpf_hz; } -void initGyroFilterCoefficients(void) { +void initGyroFilterCoefficients(void) +{ int axis; - if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime); + if (gyroLpfCutFreq && gyro.targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], gyro.targetLooptime); gyroFilterStateIsSet = true; } } @@ -79,7 +79,7 @@ bool isOnFinalGyroCalibrationCycle(void) } uint16_t calculateCalibratingCycles(void) { - return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES; + return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } bool isOnFirstGyroCalibrationCycle(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 04a5188a0..075cccbe6 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -31,11 +31,9 @@ typedef enum { } gyroSensor_e; extern gyro_t gyro; -extern sensor_align_e gyroAlign; extern int32_t gyroADC[XYZ_AXIS_COUNT]; extern float gyroADCf[XYZ_AXIS_COUNT]; -extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 00093d18e..df394c7a6 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -79,6 +79,7 @@ extern float magneticDeclination; extern gyro_t gyro; extern baro_t baro; extern acc_t acc; +extern sensor_align_e gyroAlign; uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; @@ -632,7 +633,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a acc.init(&acc); } // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation + gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation gyro.init(gyroLpf); detectMag(magHardwareToUse); From 75237dd2099351512368617ae4b9df9be78080a1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 16:14:17 +0100 Subject: [PATCH 2/3] Fixed up gyro init. --- src/main/config/config.c | 2 +- src/main/io/rc_controls.c | 2 +- src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/gyro.c | 53 +++++++++++++------------------ src/main/sensors/gyro.h | 5 +-- src/main/sensors/initialisation.c | 1 + 7 files changed, 30 insertions(+), 37 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b7d404d3d..2c0c9c17d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -750,7 +750,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 7eec0de89..0848670db 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -209,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/main.c b/src/main/main.c index 686e39504..c6cf2ffb2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -625,7 +625,7 @@ void init(void) if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 31d6e22c7..65abe6b48 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -377,7 +377,7 @@ void mwArm(void) static bool firstArmingCalibrationWasCompleted; if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5cd741526..fc28705bf 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -45,21 +45,20 @@ static uint16_t calibratingG = 0; static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; static biquad_t gyroFilterState[3]; -static bool gyroFilterStateIsSet; -static float gyroLpfCutFreq; +static uint8_t gyroLpfHz; -void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) +void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) { gyroConfig = gyroConfigToUse; - gyroLpfCutFreq = gyro_lpf_hz; + gyroLpfHz = gyro_lpf_hz; } -void initGyroFilterCoefficients(void) +void gyroInit(void) { - int axis; - if (gyroLpfCutFreq && gyro.targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], gyro.targetLooptime); - gyroFilterStateIsSet = true; + if (gyroLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known + for (int axis = 0; axis < 3; axis++) { + BiQuadNewLpf(gyroLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + } } } @@ -73,27 +72,27 @@ bool isGyroCalibrationComplete(void) return calibratingG == 0; } -bool isOnFinalGyroCalibrationCycle(void) +static bool isOnFinalGyroCalibrationCycle(void) { return calibratingG == 1; } -uint16_t calculateCalibratingCycles(void) { +uint16_t gyroCalculateCalibratingCycles(void) +{ return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } -bool isOnFirstGyroCalibrationCycle(void) +static bool isOnFirstGyroCalibrationCycle(void) { - return calibratingG == calculateCalibratingCycles(); + return calibratingG == gyroCalculateCalibratingCycles(); } static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) { - int8_t axis; static int32_t g[3]; static stdev_t var[3]; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { // Reset g[axis] at start of calibration if (isOnFirstGyroCalibrationCycle()) { @@ -113,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho float dev = devStandardDeviation(&var[axis]); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); return; } - gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles(); + gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); } } @@ -129,8 +128,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho static void applyGyroZero(void) { - int8_t axis; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { gyroADC[axis] -= gyroZero[axis]; } } @@ -138,14 +136,13 @@ static void applyGyroZero(void) void gyroUpdate(void) { int16_t gyroADCRaw[XYZ_AXIS_COUNT]; - int axis; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; gyroADC[axis] = gyroADCRaw[axis]; } @@ -158,16 +155,10 @@ void gyroUpdate(void) applyGyroZero(); - if (gyroLpfCutFreq) { - if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */ - - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - if (gyroFilterStateIsSet) { - gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]); - gyroADC[axis] = lrintf(gyroADCf[axis]); - } else { - gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled - } + if (gyroLpfHz) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); + gyroADC[axis] = lrintf(gyroADCf[axis]); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 075cccbe6..7a8214092 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -39,9 +39,10 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz); +void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void gyroInit(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); -uint16_t calculateCalibratingCycles(void); +uint16_t gyroCalculateCalibratingCycles(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index df394c7a6..80d616951 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -635,6 +635,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a // 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(); detectMag(magHardwareToUse); From d069945f894eb7ac2848eb19d8e6abf7bb529c3b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 21:43:04 +0100 Subject: [PATCH 3/3] Improved gyroSetCalibrationCycles parameters. --- src/main/io/rc_controls.c | 2 +- src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/gyro.c | 32 ++++++++++++++++---------------- src/main/sensors/gyro.h | 5 ++--- 5 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 0848670db..fdea81003 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -209,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/main.c b/src/main/main.c index c6cf2ffb2..743f77178 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -625,7 +625,7 @@ void init(void) if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 65abe6b48..bf97c56a2 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -377,7 +377,7 @@ void mwArm(void) static bool firstArmingCalibrationWasCompleted; if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index fc28705bf..48113e934 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,32 +41,27 @@ sensor_align_e gyroAlign = 0; int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -static uint16_t calibratingG = 0; static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; -static gyroConfig_t *gyroConfig; -static biquad_t gyroFilterState[3]; -static uint8_t gyroLpfHz; +static const gyroConfig_t *gyroConfig; +static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; +static uint8_t gyroSoftLpfHz; +static uint16_t calibratingG = 0; -void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz) { gyroConfig = gyroConfigToUse; - gyroLpfHz = gyro_lpf_hz; + gyroSoftLpfHz = gyro_soft_lpf_hz; } void gyroInit(void) { - if (gyroLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known + if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - BiQuadNewLpf(gyroLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime); } } } -void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired) -{ - calibratingG = calibrationCyclesRequired; -} - bool isGyroCalibrationComplete(void) { return calibratingG == 0; @@ -77,7 +72,7 @@ static bool isOnFinalGyroCalibrationCycle(void) return calibratingG == 1; } -uint16_t gyroCalculateCalibratingCycles(void) +static uint16_t gyroCalculateCalibratingCycles(void) { return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } @@ -87,6 +82,11 @@ static bool isOnFirstGyroCalibrationCycle(void) return calibratingG == gyroCalculateCalibratingCycles(); } +void gyroSetCalibrationCycles(void) +{ + calibratingG = gyroCalculateCalibratingCycles(); +} + static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) { static int32_t g[3]; @@ -112,7 +112,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho float dev = devStandardDeviation(&var[axis]); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); return; } gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); @@ -155,7 +155,7 @@ void gyroUpdate(void) applyGyroZero(); - if (gyroLpfHz) { + if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7a8214092..54069b8a6 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -39,10 +39,9 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); -void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); +void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); -uint16_t gyroCalculateCalibratingCycles(void);