Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/development' into osd-improvement

Browse source
This commit is contained in:
Evgeny Sychov 2016-06-27 15:39:11 -07:00
commit d6ce148ae6
83 changed files with 681 additions and 711 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/blackbox/blackbox.c
View file

@ -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);

1
src/main/blackbox/blackbox_io.c
View file

@ -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"

3
src/main/config/config.c
View file

@ -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)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

1
src/main/drivers/accgyro.h
View file

@ -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 {

10
src/main/drivers/accgyro_mpu.c
View file

@ -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;
}

2
src/main/drivers/accgyro_mpu.h
View file

@ -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);

1
src/main/drivers/accgyro_mpu3050.c
View file

@ -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

2
src/main/drivers/accgyro_mpu6050.c
View file

@ -1,4 +1,4 @@
/*
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify

4
src/main/drivers/bus_i2c_stm32f30x.c
View file

@ -83,8 +83,8 @@ void i2cInit(I2CDevice device)
RCC_ClockCmd(i2c->rcc, ENABLE);
RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK);
IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4);
IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4);
IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4);
IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4);
I2C_InitTypeDef i2cInit = {
.I2C_Mode = I2C_Mode_I2C,

42
src/main/drivers/gyro_sync.c
View file

@ -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 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;
}

7
src/main/drivers/gyro_sync.h
View file

@ -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);

2
src/main/drivers/sensor.h
View file

@ -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

2
src/main/drivers/timer.c
View file

@ -17,7 +17,6 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
@ -26,7 +25,6 @@
#include "nvic.h"
#include "gpio.h"
#include "gpio.h"
#include "rcc.h"
#include "system.h"

1
src/main/flight/mixer.c
View file

@ -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"

3
src/main/flight/pid.c
View file

@ -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)

1
src/main/io/osd.c
View file

@ -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"

3
src/main/io/rc_controls.c
View file

@ -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)) {

1
src/main/io/serial_cli.c
View file

@ -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"

7
src/main/io/serial_msp.c
View file

@ -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);

16
src/main/main.c
View file

@ -17,11 +17,11 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/maths.h"
@ -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"
@ -153,9 +152,6 @@ static uint8_t systemState = SYSTEM_STATE_INITIALISING;
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
@ -260,6 +256,7 @@ void init(void)
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
@ -333,6 +330,7 @@ void init(void)
#endif
pwmRxInit(masterConfig.inputFilteringMode);
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
@ -504,7 +502,7 @@ void init(void)
LED0_OFF;
LED2_OFF;
for (i = 0; i < 10; i++) {
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
@ -626,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
@ -694,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):

4
src/main/mw.c
View file

@ -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

1
src/main/rx/rx.c
View file

@ -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"

3
src/main/sensors/acceleration.c
View file

@ -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"
@ -45,7 +44,7 @@ acc_t acc; // acc access functions
sensor_align_e accAlign = 0;
uint32_t accTargetLooptime;
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 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 AccInflightCalibrationArmed;

7
src/main/sensors/barometer.c
View file

@ -20,6 +20,10 @@
#include <math.h>
#include "platform.h"
int32_t BaroAlt = 0;
#ifdef BARO
#include "common/maths.h"
#include "drivers/barometer.h"
@ -32,9 +36,6 @@ baro_t baro; // barometer access functions
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t BaroAlt = 0;
#ifdef BARO
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;

9
src/main/sensors/compass.c
View file

@ -40,13 +40,14 @@
#endif
mag_t mag; // mag access functions
int32_t magADC[XYZ_AXIS_COUNT];
sensor_align_e magAlign = 0;
#ifdef MAG
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
int16_t magADCRaw[XYZ_AXIS_COUNT];
int32_t magADC[XYZ_AXIS_COUNT];
sensor_align_e magAlign = 0;
#ifdef MAG
static int16_t magADCRaw[XYZ_AXIS_COUNT];
static uint8_t magInit = 0;
void compassInit(void)

79
src/main/sensors/gyro.c
View file

@ -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) {
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
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
}
}
}
}

8
src/main/sensors/gyro.h
View file

@ -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);

4
src/main/sensors/initialisation.c
View file

@ -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);

1
src/main/target/X_RACERSPI/target.c
View file

@ -84,7 +84,6 @@ const uint16_t airPWM[] = {
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N
// Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype.
{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)
{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7)
{ TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1

2
src/main/target/X_RACERSPI/target.h
View file

@ -17,7 +17,7 @@
#pragma once
#define TARGET_BOARD_IDENTIFIER "X_RACERSPI"
#define TARGET_BOARD_IDENTIFIER "XRC3"
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE