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

Gyro Sync feature (Loop synced to Gyro)

Browse source 
Remove double MPU_RA_PWR_MGMT_1

Looptime to Gyro refresh rate

disable 2khz when acc used for F3

Gyro sync code improvement

doc change

MPU6500 INT_STATUS support // MPU3500 dummy handling

Enable full 1khz sampling on F1 boards (no acc mode)

Add MPU3050 INT_STATUS

F1 target decreased max refresh

Configurable optione (sync_gyro_to_loop)

correction watchdog

correct filter

Coding style and cleanup code

Cli.md

Bump EEPROM version

MPU6050 MPU DATA READY Pin instead of i2c

Add more targets and corrections

correction to CC3D

ident

MPU6500 correction

INT_ENABLE MPU6500

add interrupt mpu6500

MPU6500 corrections from rebase

MPU6500 correct

Final fix to compile // ready for testing and review

MPU6000 / MPU3050 Corrections

Add CC3D extiConfig

add mpu6500

Fix intStatus position

fix returns

Add COLIBRI_RACE MPU INT

Change CLI name to gyro_sync

This is shorter typing and speaks for itself

Fix MotoLab After rebase

Add motolab

Full gyro Sync rework

Idents etc

Doc update

more corrections

Fix serial_cli

refactor // ident

GPL

single line

runloop refactor
This commit is contained in:
borisbstyle 2015-08-05 00:16:52 +02:00
parent 6926c2057f
commit 10a96b0dfc
25 changed files with 238 additions and 75 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -235,6 +235,7 @@ COMMON_SRC = build_config.c \
drivers/serial.c \
drivers/sound_beeper.c \
drivers/system.c \
drivers/gyro_sync.c \
io/beeper.c \
io/rc_controls.c \
io/rc_curves.c \

6
docs/Cli.md
View file

@ -106,6 +106,8 @@ Re-apply any new defaults as desired.
| `looptime` | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). Default of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | 0 | 9000 | 3500 | Master | UINT16 |
| `emf_avoidance` | Default value is 0 for 72MHz processor speed. Setting this to 1 increases the processor speed, to move the 6th harmonic away from 432MHz. | OFF | ON | OFF | Master | UINT8 |
| `i2c_overclock` | Default value is 0 for disabled. Enabling this feature speeds up IMU speed significantly and faster looptimes are possible. | OFF | ON | OFF | Master | UINT8 |
| `gyro_sync` | Default value is Off. This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Use gyro_lpf and gyro_sync_denom determine the gyro refresh rate. Note that different targets have different limits. Setting too high refresh rate can mean that FC cannot keep up with the gyro and higher gyro_sync_denom is needed, | OFF | ON | OFF | Master | UINT8 |
| `gyro_sync_denom` | This option determines the sampling ratio. Denominator of 1 means full gyro sampling rate. Denominator 2 would mean 1/2 samples will be collected. Denominator and gyro_lpf will together determine the control loop speed. | 0 | 1 | 1 | Master | UINT8 |
| `mid_rc` | This is an important number to set in order to avoid trimming receiver/transmitter. Most standard receivers will have this at 1500, however Futaba transmitters will need this set to 1520. A way to find out if this needs to be changed, is to clear all trim/subtrim on transmitter, and connect to GUI. Note the value most channels idle at - this should be the number to choose. Once midrc is set, use subtrim on transmitter to make sure all channels (except throttle of course) are centered at midrc value. | 1200 | 1700 | 1500 | Master | UINT16 |
| `min_check` | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | 0 | 2000 | 1100 | Master | UINT16 |
| `max_check` | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | 0 | 2000 | 1900 | Master | UINT16 |
@ -165,7 +167,7 @@ Re-apply any new defaults as desired.
| `vbat_max_cell_voltage` | Maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) | 10 | 50 | 43 | Master | UINT8 |
| `vbat_min_cell_voltage` | Minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V) | 10 | 50 | 33 | Master | UINT8 |
| `vbat_warning_cell_voltage` | | 10 | 50 | 35 | Master | UINT8 |
| `current_meter_scale` | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the Überdistro with a 0.25mOhm shunt. | -10000 | 10000 | 400 | Master | INT16 |
| `current_meter_scale` | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the Überdistro with a 0.25mOhm shunt. | -10000 | 10000 | 400 | Master | INT16 |
| `current_meter_offset` | This sets the output offset voltage of the current sensor in millivolts. | 0 | 3300 | 0 | Master | UINT16 |
| `multiwii_current_meter_output` | Default current output via MSP is in 0.01A steps. Setting this to 1 causes output in default multiwii scaling (1mA steps). | OFF | ON | OFF | Master | UINT8 |
| `current_meter_type` | | 0 | 2 | 1 | Master | UINT8 |
@ -176,7 +178,7 @@ Re-apply any new defaults as desired.
| `align_board_pitch` | Arbitrary board rotation in degrees, to allow mounting it sideways / upside down / rotated etc | -180 | 360 | 0 | Master | INT16 |
| `align_board_yaw` | Arbitrary board rotation in degrees, to allow mounting it sideways / upside down / rotated etc | -180 | 360 | 0 | Master | INT16 |
| `max_angle_inclination` | This setting controls max inclination (tilt) allowed in angle (level) mode. default 500 (50 degrees). | 100 | 900 | 500 | Master | UINT16 |
| `gyro_lpf` | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 5,10,20,42,98,188,256Hz, while MPU3050 doesn't allow 5Hz. If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. Values outside of supported range will usually be ignored by drivers, and will configure lpf to default value of 42Hz. | 0 | 256 | 42 | Master | UINT16 |
| `gyro_lpf` | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. | 10HZ | 256HZ | 42HZ | Master | UINT16 |
| `moron_threshold` | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold (default of 32) means how much average gyro reading could differ before re-calibration is triggered. | 0 | 128 | 32 | Master | UINT8 |
| `gyro_cmpf_factor` | This setting controls the Gyro Weight for the Gyro/Acc complementary filter. Increasing this value reduces and delays Acc influence on the output of the filter. | 100 | 1000 | 600 | Master | UINT16 |
| `gyro_cmpfm_factor` | This setting controls the Gyro Weight for the Gyro/Magnetometer complementary filter. Increasing this value reduces and delays the Magnetometer influence on the output of the filter. | 100 | 1000 | 250 | Master | UINT16 |

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

@ -39,6 +39,7 @@
#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"
@ -551,7 +552,7 @@ bool blackboxDeviceOpen(void)
* = floor((looptime_ns * 3) / 500.0)
* = (looptime_ns * 3) / 500
*/
blackboxMaxHeaderBytesPerIteration = constrain((masterConfig.looptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
blackboxMaxHeaderBytesPerIteration = constrain((targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
return blackboxPort != NULL;
}

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

@ -401,7 +401,7 @@ static void resetConf(void)
masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
masterConfig.gyro_lpf = 3; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
resetAccelerometerTrims(&masterConfig.accZero);
@ -478,6 +478,8 @@ static void resetConf(void)
masterConfig.looptime = 3500;
masterConfig.emf_avoidance = 0;
masterConfig.i2c_overclock = 0;
masterConfig.gyroSync = 0;
masterConfig.gyroSyncDenominator = 1;
resetPidProfile(&currentProfile->pidProfile);
@ -1058,4 +1060,3 @@ uint32_t featureMask(void)
{
return masterConfig.enabledFeatures;
}

5
src/main/config/config_master.h
View file

@ -28,6 +28,8 @@ typedef struct master_t {
uint16_t looptime; // imu loop time in us
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band
uint8_t i2c_overclock; // Overclock i2c Bus for faster IMU readings
uint8_t gyroSync; // Enable interrupt based loop
uint8_t gyroSyncDenominator; // Gyro sync Denominator
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
#ifdef USE_SERVOS
@ -47,9 +49,10 @@ typedef struct master_t {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
gyroConfig_t gyroConfig;

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

@ -23,6 +23,7 @@ typedef struct gyro_s {
sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
sensorInterruptFuncPtr intStatus;
float scale; // scalefactor
} gyro_t;

13
src/main/drivers/accgyro_l3g4200d.c
View file

@ -54,7 +54,7 @@
#define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0
static void l3g4200dInit(uint16_t lpf);
static void l3g4200dInit(uint8_t lpf);
static bool l3g4200dRead(int16_t *gyroADC);
bool l3g4200dDetect(gyro_t *gyro)
@ -76,24 +76,25 @@ bool l3g4200dDetect(gyro_t *gyro)
return true;
}
static void l3g4200dInit(uint16_t lpf)
static void l3g4200dInit(uint8_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
// Conversion from MPU6XXX LPF values
switch (lpf) {
default:
case 32:
case 3:
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
break;
case 54:
case 4:
mpuLowPassFilter = L3G4200D_DLPF_54HZ;
break;
case 78:
case 5:
mpuLowPassFilter = L3G4200D_DLPF_78HZ;
break;
case 93:
case 6:
mpuLowPassFilter = L3G4200D_DLPF_93HZ;
break;
}

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

@ -32,6 +32,7 @@
#include "gpio.h"
#include "exti.h"
#include "bus_i2c.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
@ -44,12 +45,13 @@
//#define DEBUG_MPU_DATA_READY_INTERRUPT
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
static void mpu6050FindRevision(void);
static volatile bool mpuDataReady;
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void);
#endif
@ -191,6 +193,8 @@ void MPU_DATA_READY_EXTI_Handler(void)
EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line);
mpuDataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
// Measure the delta in micro seconds between calls to the interrupt handler
static uint32_t lastCalledAt = 0;
@ -287,30 +291,6 @@ void mpuIntExtiInit(void)
mpuExtiInitDone = true;
}
uint8_t determineMPULPF(uint16_t lpf)
{
uint8_t mpuLowPassFilter;
if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = INV_FILTER_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else if (lpf > 0)
mpuLowPassFilter = INV_FILTER_5HZ;
else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return mpuLowPassFilter;
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
{
bool ack = i2cRead(MPU_ADDRESS, reg, length, data);
@ -354,3 +334,12 @@ bool mpuGyroRead(int16_t *gyroADC)
return true;
}
void checkMPUDataReady(bool *mpuDataReadyPtr) {
if (mpuDataReady) {
*mpuDataReadyPtr = true;
mpuDataReady= false;
} else {
*mpuDataReadyPtr = false;
}
}

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

@ -178,9 +178,9 @@ typedef struct mpuDetectionResult_s {
extern mpuDetectionResult_t mpuDetectionResult;
uint8_t determineMPULPF(uint16_t lpf);
void configureMPUDataReadyInterruptHandling(void);
void mpuIntExtiInit(void);
bool mpuAccRead(int16_t *accData);
bool mpuGyroRead(int16_t *gyroADC);
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
void checkMPUDataReady(bool *mpuDataReadyPtr);

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

@ -30,6 +30,7 @@
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu3050.h"
#include "gyro_sync.h"
// MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68
@ -46,7 +47,7 @@
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
static void mpu3050Init(uint16_t lpf);
static void mpu3050Init(uint8_t lpf);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro)
@ -57,6 +58,7 @@ bool mpu3050Detect(gyro_t *gyro)
gyro->init = mpu3050Init;
gyro->read = mpuGyroRead;
gyro->temperature = mpu3050ReadTemp;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
@ -64,19 +66,17 @@ bool mpu3050Detect(gyro_t *gyro)
return true;
}
static void mpu3050Init(uint16_t lpf)
static void mpu3050Init(uint8_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(FAILURE_ACC_INIT);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf);
mpuConfiguration.write(MPU3050_INT_CFG, 0);
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);

14
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
@ -31,6 +31,7 @@
#include "gpio.h"
#include "exti.h"
#include "bus_i2c.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
@ -51,7 +52,7 @@ extern uint8_t mpuLowPassFilter;
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
static void mpu6050AccInit(void);
static void mpu6050GyroInit(uint16_t lpf);
static void mpu6050GyroInit(uint8_t lpf);
bool mpu6050AccDetect(acc_t *acc)
{
@ -73,6 +74,7 @@ bool mpu6050GyroDetect(gyro_t *gyro)
}
gyro->init = mpu6050GyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
@ -94,20 +96,18 @@ static void mpu6050AccInit(void)
}
}
static void mpu6050GyroInit(uint16_t lpf)
static void mpu6050GyroInit(uint8_t lpf)
{
bool ack;
mpuIntExtiInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
ack = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.

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

@ -27,6 +27,7 @@
#include "system.h"
#include "exti.h"
#include "gpio.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
@ -55,6 +56,7 @@ bool mpu6500GyroDetect(gyro_t *gyro)
gyro->init = mpu6500GyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
@ -69,7 +71,7 @@ void mpu6500AccInit(void)
acc_1G = 512 * 8;
}
void mpu6500GyroInit(uint16_t lpf)
void mpu6500GyroInit(uint8_t lpf)
{
mpuIntExtiInit();
@ -86,8 +88,6 @@ void mpu6500GyroInit(uint16_t lpf)
}
#endif
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
@ -97,8 +97,8 @@ void mpu6500GyroInit(uint16_t lpf)
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0); // 1kHz S/R
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); // Get Divider
// Data ready interrupt configuration
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN

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

@ -25,4 +25,4 @@ bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro);
void mpu6500AccInit(void);
void mpu6500GyroInit(uint16_t lpf);
void mpu6500GyroInit(uint8_t lpf);

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

@ -35,6 +35,7 @@
#include "gpio.h"
#include "exti.h"
#include "bus_spi.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
@ -120,12 +121,10 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true;
}
void mpu6000SpiGyroInit(uint16_t lpf)
void mpu6000SpiGyroInit(uint8_t lpf)
{
mpuIntExtiInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
@ -133,7 +132,7 @@ void mpu6000SpiGyroInit(uint16_t lpf)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
mpu6000WriteRegister(MPU6000_CONFIG, lpf);
delayMicroseconds(1);
int16_t data[3];
@ -226,7 +225,7 @@ static void mpu6000AccAndGyroInit(void) {
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, 0x00);
mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider());
delayMicroseconds(1);
// Gyro +/- 1000 DPS Full Scale
@ -271,6 +270,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro)
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;

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

@ -133,10 +133,10 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro)
gyro->init = mpu6500GyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
return true;
}

72
src/main/drivers/gyro_sync.c Normal file
View file

@ -0,0 +1,72 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#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 "config/runtime_config.h"
#include "config/config.h"
extern gyro_t gyro;
uint32_t targetLooptime;
uint8_t mpuDividerDrops;
bool getMpuDataStatus(gyro_t *gyro)
{
bool mpuDataStatus;
gyro->intStatus(&mpuDataStatus);
return mpuDataStatus;
}
bool gyroSyncCheckUpdate(void) {
return getMpuDataStatus(&gyro);
}
void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator) {
int gyroSamplePeriod;
if (gyroSync) {
if (!lpf) {
gyroSamplePeriod = 125;
} else {
gyroSamplePeriod = 1000;
}
mpuDividerDrops = gyroSyncDenominator - 1;
targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
} else {
mpuDividerDrops = 0;
targetLooptime = looptime;
}
}
uint8_t gyroMPU6xxxCalculateDivider(void) {
return mpuDividerDrops;
}

22
src/main/drivers/gyro_sync.h Normal file
View file

@ -0,0 +1,22 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxCalculateDivider(void);
void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);

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

@ -19,4 +19,5 @@
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf); // gyro sensor init prototype
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor read and align prototype

9
src/main/drivers/system.c
View file

@ -83,10 +83,19 @@ void EXTI15_10_IRQHandler(void)
extiHandler(EXTI15_10_IRQn);
}
#if defined(CC3D)
void EXTI3_IRQHandler(void)
{
extiHandler(EXTI3_IRQn);
}
#endif
#if defined (COLIBRI_RACE)
void EXTI9_5_IRQHandler(void)
{
extiHandler(EXTI9_5_IRQn);
}
#endif
// cycles per microsecond
static uint32_t usTicks = 0;

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

@ -153,7 +153,7 @@ static char cliBuffer[48];
static uint32_t bufferIndex = 0;
#ifndef USE_QUAD_MIXER_ONLY
// sync this with mixerMode_e
// this with mixerMode_e
static const char * const mixerNames[] = {
"TRI", "QUADP", "QUADX", "BI",
"GIMBAL", "Y6", "HEX6",
@ -351,6 +351,15 @@ static const char * const lookupTableGyroFilter[] = {
"OFF", "LOW", "MEDIUM", "HIGH"
};
static const char * const lookupTableGyroLpf[] = {
"256HZ",
"188HZ",
"98HZ",
"42HZ",
"20HZ",
"10HZ"
};
typedef struct lookupTableEntry_s {
const char * const *values;
@ -373,6 +382,7 @@ typedef enum {
TABLE_PID_CONTROLLER,
TABLE_SERIAL_RX,
TABLE_GYRO_FILTER,
TABLE_GYRO_LPF,
} lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = {
@ -390,7 +400,8 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) },
{ lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }
};
#define VALUE_TYPE_OFFSET 0
@ -446,6 +457,8 @@ const clivalue_t valueTable[] = {
{ "looptime", VAR_UINT16 | MASTER_VALUE, &masterConfig.looptime, .config.minmax = {0, 9000} },
{ "emf_avoidance", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.emf_avoidance, .config.lookup = { TABLE_OFF_ON } },
{ "i2c_overclock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.i2c_overclock, .config.lookup = { TABLE_OFF_ON } },
{ "gyro_sync", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroSync, .config.lookup = { TABLE_OFF_ON } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroSyncDenominator, .config.minmax = { 1, 32 } },
{ "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, .config.minmax = { 1200, 1700 } },
{ "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
@ -532,7 +545,7 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_lpf", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_lpf, .config.minmax = { 0, 256 } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 20000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 20000 } },

5
src/main/main.c
View file

@ -372,7 +372,10 @@ void init(void)
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf,
masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination,
masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}

21
src/main/mw.c
View file

@ -38,6 +38,7 @@
#include "drivers/serial.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
@ -92,6 +93,7 @@ enum {
#define VBATINTERVAL (6 * 3500)
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
#define IBATINTERVAL (6 * 3500)
#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro
uint32_t currentTime = 0;
uint32_t previousTime = 0;
@ -667,6 +669,21 @@ void processRx(void)
}
// Function for loop trigger
bool shouldRunLoop(uint32_t loopTime) {
bool loopTrigger = false;
if (masterConfig.gyroSync) {
if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
loopTrigger = true;
}
} else if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0){
loopTrigger = true;
}
return loopTrigger;
}
void filterRc(void){
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
@ -748,8 +765,8 @@ void loop(void)
}
currentTime = micros();
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime;
if (shouldRunLoop(loopTime)) {
loopTime = currentTime + targetLooptime;
imuUpdate(&currentProfile->accelerometerTrims);

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

@ -45,6 +45,7 @@
#include "drivers/bus_spi.h"
#include "drivers/accgyro_spi_mpu6000.h"
#include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/gyro_sync.h"
#include "drivers/barometer.h"
#include "drivers/barometer_bmp085.h"
@ -150,6 +151,19 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
return &MotolabF3MPUIntExtiConfig;
#endif
#if defined(COLIBRI_RACE)
static const extiConfig_t colibriRaceMPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_5,
.exti_port_source = EXTI_PortSourceGPIOA,
.exti_pin_source = EXTI_PinSource5,
.exti_line = EXTI_Line5,
.exti_irqn = EXTI9_5_IRQn
};
return &colibriRaceMPUIntExtiConfig;
#endif
return NULL;
}
@ -630,8 +644,10 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
{
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig,
uint32_t looptime, uint8_t gyroSync, uint8_t gyroSyncDenominator) {
int16_t deg, min;
memset(&acc, 0, sizeof(acc));
@ -656,6 +672,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyroUpdateSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator); // Set gyro sampling rate divider before initialization
gyro.init(gyroLpf);
detectMag(magHardwareToUse);

5
src/main/sensors/initialisation.h
View file

@ -17,4 +17,7 @@
#pragma once
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf,
uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig,
uint32_t looptime, uint8_t gyroSync, uint8_t gyroSyncDenominator);

7
src/main/target/COLIBRI_RACE/target.h
View file

@ -55,6 +55,8 @@
#define USABLE_TIMER_CHANNEL_COUNT 11
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define GYRO
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500
@ -158,6 +160,11 @@
#define WS2811_DMA_CHANNEL DMA1_Channel3
#define WS2811_IRQ DMA1_Channel3_IRQn
// MPU6500 interrupt
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
#define TELEMETRY
#define SERIAL_RX
#define USE_SERVOS