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

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
This commit is contained in:
borisbstyle 2015-08-05 00:16:52 +02:00
parent 07b9aceb7c
commit 26deeb8ff6
16 changed files with 162 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

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

6
docs/Cli.md
View file

@ -103,7 +103,7 @@ Re-apply any new defaults as desired.
| `Variable` | Description/Units | Min | Max | Default | Type | Datatype | | `Variable` | Description/Units | Min | Max | Default | Type | Datatype |
|---------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------|--------|---------------|--------------|----------| |---------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------|--------|---------------|--------------|----------|
| `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 | | `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. The looptime is also determing the gyro refresh rate together with gyro_lpf. | 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. | 0 | 1 | 0 | Master | UINT8 | | `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. | 0 | 1 | 0 | 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 | | `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 | | `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 |
@ -127,7 +127,7 @@ Re-apply any new defaults as desired.
| `retarded_arm` | Disabled by default, enabling (setting to 1) allows disarming by throttle low + roll. This could be useful for mode-1 users and non-acro tricopters, where default arming by yaw could move tail servo too much. | 0 | 1 | 0 | Master | UINT8 | | `retarded_arm` | Disabled by default, enabling (setting to 1) allows disarming by throttle low + roll. This could be useful for mode-1 users and non-acro tricopters, where default arming by yaw could move tail servo too much. | 0 | 1 | 0 | Master | UINT8 |
| `disarm_kill_switch` | Enabled by default. Disarms the motors independently of throttle value. Setting to 0 reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | 0 | 1 | 1 | Master | UINT8 | | `disarm_kill_switch` | Enabled by default. Disarms the motors independently of throttle value. Setting to 0 reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | 0 | 1 | 1 | Master | UINT8 |
| `auto_disarm_delay` | | 0 | 60 | 5 | Master | UINT8 | | `auto_disarm_delay` | | 0 | 60 | 5 | Master | UINT8 |
| `small_angle` | If the copter tilt angle exceed this value the copter will refuse to arm. default is 25°. | 0 | 180 | 25 | Master | UINT8 | | `small_angle` | If the copter tilt angle exceed this value the copter will refuse to arm. default is 25°. | 0 | 180 | 25 | Master | UINT8 |
| `pid_at_min_throttle` | If enabled, the copter will process the pid algorithm at minimum throttle. Cannot be used when `retarded_arm` is enabled. | 0 | 1 | 1 | Master | UINT8 | | `pid_at_min_throttle` | If enabled, the copter will process the pid algorithm at minimum throttle. Cannot be used when `retarded_arm` is enabled. | 0 | 1 | 1 | Master | UINT8 |
| `flaps_speed` | | 0 | 100 | 0 | Master | UINT8 | | `flaps_speed` | | 0 | 100 | 0 | Master | UINT8 |
| `fixedwing_althold_dir` | | -1 | 1 | 1 | Master | INT8 | | `fixedwing_althold_dir` | | -1 | 1 | 1 | Master | INT8 |
@ -163,7 +163,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_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_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 | | `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 | | `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). | 0 | 1 | None defined! | Master | UINT8 | | `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). | 0 | 1 | None defined! | Master | UINT8 |
| `current_meter_type` | | 0 | 2 | 1 | Master | UINT8 | | `current_meter_type` | | 0 | 2 | 1 | Master | UINT8 |

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

@ -22,6 +22,7 @@
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "drivers/sound_beeper.h" #include "drivers/sound_beeper.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
@ -470,7 +471,7 @@ bool blackboxDeviceOpen(void)
* *
* 9 / 1250 = 7200 / 1000000 * 9 / 1250 = 7200 / 1000000
*/ */
blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); blackboxWriteChunkSize = MAX((targetLooptime * 9) / 1250, 4);
switch (masterConfig.blackbox_device) { switch (masterConfig.blackbox_device) {
case BLACKBOX_DEVICE_SERIAL: case BLACKBOX_DEVICE_SERIAL:

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

@ -988,4 +988,3 @@ uint32_t featureMask(void)
{ {
return masterConfig.enabledFeatures; return masterConfig.enabledFeatures;
} }

1
src/main/config/config.h
View file

@ -21,7 +21,6 @@
#define MAX_CONTROL_RATE_PROFILE_COUNT 3 #define MAX_CONTROL_RATE_PROFILE_COUNT 3
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
typedef enum { typedef enum {
FEATURE_RX_PPM = 1 << 0, FEATURE_RX_PPM = 1 << 0,
FEATURE_VBAT = 1 << 1, FEATURE_VBAT = 1 << 1,

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

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

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

@ -35,6 +35,8 @@
#include "accgyro.h" #include "accgyro.h"
#include "accgyro_mpu6050.h" #include "accgyro_mpu6050.h"
#include "gyro_sync.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
// MPU6050, Standard address 0x68 // MPU6050, Standard address 0x68
@ -138,19 +140,6 @@
// RF = Register Flag // RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0) #define MPU_RF_DATA_RDY_EN (1 << 0)
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
enum gyro_fsr_e { enum gyro_fsr_e {
INV_FSR_250DPS = 0, INV_FSR_250DPS = 0,
INV_FSR_500DPS, INV_FSR_500DPS,
@ -176,6 +165,7 @@ static void mpu6050AccInit(void);
static void mpu6050AccRead(int16_t *accData); static void mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(void); static void mpu6050GyroInit(void);
static void mpu6050GyroRead(int16_t *gyroADC); static void mpu6050GyroRead(int16_t *gyroADC);
static void checkMPU6050Interrupt(bool *gyroIsUpdated);
typedef enum { typedef enum {
MPU_6050_HALF_RESOLUTION, MPU_6050_HALF_RESOLUTION,
@ -368,6 +358,7 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_
gyro->init = mpu6050GyroInit; gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead; gyro->read = mpu6050GyroRead;
gyro->intStatus = checkMPU6050Interrupt;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
@ -425,8 +416,9 @@ static void mpu6050GyroInit(void)
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100); delay(100);
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 7 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
i2cWrite(MPU6050_ADDRESS, 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) i2cWrite(MPU6050_ADDRESS, 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)
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
@ -455,3 +447,11 @@ static void mpu6050GyroRead(int16_t *gyroADC)
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]); gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]); gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
} }
void checkMPU6050Interrupt(bool *gyroIsUpdated) {
uint8_t mpuIntStatus;
i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &mpuIntStatus);
(mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false);
}

14
src/main/drivers/accgyro_mpu6050.h
View file

@ -33,7 +33,17 @@ typedef struct mpu6050Config_s {
IRQn_Type exti_irqn; IRQn_Type exti_irqn;
} mpu6050Config_t; } mpu6050Config_t;
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc); bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc);
bool mpu6050GyroDetect(const mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf); bool mpu6050GyroDetect(const mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf);
void mpu6050DmpLoop(void);
void mpu6050DmpResetFifo(void);

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

@ -39,6 +39,8 @@
#include "accgyro.h" #include "accgyro.h"
#include "accgyro_spi_mpu6000.h" #include "accgyro_spi_mpu6000.h"
#include "gyro_sync.h"
static bool mpuSpi6000InitDone = false; static bool mpuSpi6000InitDone = false;
// Registers // Registers
@ -127,6 +129,7 @@ static bool mpuSpi6000InitDone = false;
void mpu6000SpiGyroRead(int16_t *gyroADC); void mpu6000SpiGyroRead(int16_t *gyroADC);
void mpu6000SpiAccRead(int16_t *gyroADC); void mpu6000SpiAccRead(int16_t *gyroADC);
void checkMPU6000Interrupt(bool *gyroIsUpdated);
static void mpu6000WriteRegister(uint8_t reg, uint8_t data) static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
{ {
@ -202,7 +205,7 @@ bool mpu6000SpiDetect(void)
return false; return false;
} }
void mpu6000AccAndGyroInit() { void mpu6000AccAndGyroInit(void) {
if (mpuSpi6000InitDone) { if (mpuSpi6000InitDone) {
return; return;
@ -294,6 +297,10 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Determine the new sample divider
mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDivider());
delayMicroseconds(1);
// Accel and Gyro DLPF Setting // Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter); mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1); delayMicroseconds(1);
@ -306,6 +313,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
} }
gyro->init = mpu6000SpiGyroInit; gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead; gyro->read = mpu6000SpiGyroRead;
gyro->intStatus = checkMPU6000Interrupt;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
@ -338,3 +346,11 @@ void mpu6000SpiAccRead(int16_t *gyroData)
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]); gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
} }
void checkMPU6000Interrupt(bool *gyroIsUpdated) {
uint8_t mpuIntStatus;
mpu6000ReadRegister(MPU6000_INT_STATUS, &mpuIntStatus, 1);
(mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false);
}

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

@ -0,0 +1,88 @@
/*
* gyro_sync.c
*
* 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/accgyro_mpu6050.h"
#include "drivers/accgyro_spi_mpu6000.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 mpuDivider;
bool getMpuInterrupt(gyro_t *gyro)
{
bool gyroIsUpdated;
gyro->intStatus(&gyroIsUpdated);
return gyroIsUpdated;
}
bool gyroSyncCheckUpdate(void) {
return getMpuInterrupt(&gyro);
}
void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf) {
int gyroSamplePeriod;
int minLooptime;
#ifdef STM32F303xC
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) {
minLooptime = 500; // Max refresh 2khz
}
else {
minLooptime = 625; // Max refresh 1,6khz
}
}
else {
gyroSamplePeriod = 1000;
minLooptime = 1000;
}
#elif STM32F10X
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) {
minLooptime = 500; // Max refresh 2khz
}
else {
minLooptime = 1625; // Max refresh 615hz
}
}
else {
gyroSamplePeriod = 1000;
minLooptime = 2000;
}
#endif
looptime = constrain(looptime, minLooptime, 4000);
mpuDivider = (looptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1;
targetLooptime = (mpuDivider + 1) * gyroSamplePeriod;
}
uint8_t gyroMPU6xxxGetDivider(void) {
return mpuDivider;
}

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

@ -0,0 +1,12 @@
/*
* gyro_sync.h
*
* Created on: 3 aug. 2015
* Author: borisb
*/
extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxGetDivider(void);
void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf);

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

@ -19,3 +19,4 @@
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor read and align prototype

2
src/main/main.c
View file

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

8
src/main/mw.c
View file

@ -37,6 +37,7 @@
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/pwm_rx.h" #include "drivers/pwm_rx.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
@ -90,7 +91,8 @@ enum {
/* VBAT monitoring interval (in microseconds) - 1s*/ /* VBAT monitoring interval (in microseconds) - 1s*/
#define VBATINTERVAL (6 * 3500) #define VBATINTERVAL (6 * 3500)
/* IBat monitoring interval (in microseconds) - 6 default looptimes */ /* IBat monitoring interval (in microseconds) - 6 default looptimes */
#define IBATINTERVAL (6 * 3500) #define IBATINTERVAL (6 * 3500)
#define GYRO_WATCHDOG_DELAY 500 // Watchdog for boards without interrupt for gyro
#define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes #define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
@ -857,8 +859,8 @@ void loop(void)
} }
currentTime = micros(); currentTime = micros();
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { if (gyroSyncCheckUpdate() || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime; loopTime = currentTime + targetLooptime + GYRO_WATCHDOG_DELAY;
imuUpdate(&currentProfile->accelerometerTrims); imuUpdate(&currentProfile->accelerometerTrims);

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

@ -39,6 +39,7 @@
#include "drivers/bus_spi.h" #include "drivers/bus_spi.h"
#include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6000.h"
#include "drivers/accgyro_spi_mpu6500.h" #include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/gyro_sync.h"
#include "drivers/barometer.h" #include "drivers/barometer.h"
#include "drivers/barometer_bmp085.h" #include "drivers/barometer_bmp085.h"
@ -596,7 +597,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
} }
} }
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig) bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig, uint32_t looptime)
{ {
int16_t deg, min; int16_t deg, min;
@ -614,6 +615,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
if (sensors(SENSOR_ACC)) if (sensors(SENSOR_ACC))
acc.init(); acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyroUpdateSampleRate(looptime, gyroLpf); // Set gyro refresh rate before initialisation
gyro.init(); gyro.init();
detectMag(magHardwareToUse); detectMag(magHardwareToUse);

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

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