Merge pull request #602 from martinbudden/bf_gyro_tidy
gyro sync and initialisation cleanup
This commit is contained in:
commit
9843d7ce2f
|
@ -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);
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
||||
|
@ -751,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);
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -1,4 +1,4 @@
|
|||
/*
|
||||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
|
|
|
@ -4,48 +4,40 @@
|
|||
* Created on: 3 aug. 2015
|
||||
* Author: borisb
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
@ -210,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();
|
||||
|
||||
#ifdef GPS
|
||||
if (feature(FEATURE_GPS)) {
|
||||
|
|
|
@ -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"
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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"
|
||||
|
@ -625,7 +624,7 @@ void init(void)
|
|||
if (masterConfig.mixerMode == MIXER_GIMBAL) {
|
||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||
}
|
||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
||||
gyroSetCalibrationCycles();
|
||||
#ifdef BARO
|
||||
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
||||
#endif
|
||||
|
@ -693,12 +692,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):
|
||||
|
|
|
@ -377,7 +377,7 @@ void mwArm(void)
|
|||
static bool firstArmingCalibrationWasCompleted;
|
||||
|
||||
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
|
||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
||||
gyroSetCalibrationCycles();
|
||||
armingCalibrationWasInitialised = true;
|
||||
firstArmingCalibrationWasCompleted = true;
|
||||
}
|
||||
|
@ -802,7 +802,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
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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,36 +35,31 @@
|
|||
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
uint16_t calibratingG = 0;
|
||||
int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||
float gyroADCf[XYZ_AXIS_COUNT];
|
||||
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)
|
||||
int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||
float gyroADCf[XYZ_AXIS_COUNT];
|
||||
|
||||
static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
static const gyroConfig_t *gyroConfig;
|
||||
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
|
||||
static uint8_t gyroSoftLpfHz;
|
||||
static uint16_t calibratingG = 0;
|
||||
|
||||
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
|
||||
{
|
||||
gyroConfig = gyroConfigToUse;
|
||||
gyroLpfCutFreq = gyro_lpf_hz;
|
||||
gyroSoftLpfHz = gyro_soft_lpf_hz;
|
||||
}
|
||||
|
||||
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);
|
||||
gyroFilterStateIsSet = true;
|
||||
}
|
||||
}
|
||||
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
void gyroInit(void)
|
||||
{
|
||||
calibratingG = calibrationCyclesRequired;
|
||||
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool isGyroCalibrationComplete(void)
|
||||
|
@ -73,27 +67,32 @@ bool isGyroCalibrationComplete(void)
|
|||
return calibratingG == 0;
|
||||
}
|
||||
|
||||
bool isOnFinalGyroCalibrationCycle(void)
|
||||
static bool isOnFinalGyroCalibrationCycle(void)
|
||||
{
|
||||
return calibratingG == 1;
|
||||
}
|
||||
|
||||
uint16_t calculateCalibratingCycles(void) {
|
||||
return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
|
||||
static 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();
|
||||
}
|
||||
|
||||
void gyroSetCalibrationCycles(void)
|
||||
{
|
||||
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();
|
||||
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 (gyroSoftLpfHz) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
|
||||
gyroADC[axis] = lrintf(gyroADCf[axis]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -31,19 +31,17 @@ 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.
|
||||
} gyroConfig_t;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float 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 calculateCalibratingCycles(void);
|
||||
|
||||
|
|
|
@ -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,8 +633,9 @@ 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);
|
||||
gyroInit();
|
||||
|
||||
detectMag(magHardwareToUse);
|
||||
|
||||
|
|
Loading…
Reference in New Issue