rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Changed gyro init and read functions to take a gyro_t* parameter. 

Scaled gyro values to degrees per second in gyroUpdate.
This commit is contained in:
Martin Budden 2016-11-19 14:11:03 +00:00
parent 55b32740d9
commit 590e569375
28 changed files with 136 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/blackbox/blackbox.c
View File

@ -18,6 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
@ -990,7 +991,7 @@ static void loadMainState(uint32_t currentTime)
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->gyroADC[i] = gyroADC[i];
blackboxCurrent->gyroADC[i] = lrintf(gyroADCf[i]);
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {

2
src/main/common/filter.h
View File

@ -15,6 +15,8 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef STM32F10X
#define MAX_FIR_DENOISE_WINDOW_SIZE 60
#else

9
src/main/drivers/accgyro.h
View File

@ -17,6 +17,8 @@
#pragma once
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#ifndef MPU_I2C_INSTANCE
@ -34,11 +36,14 @@
typedef struct gyro_s {
sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorGyroReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
sensorInterruptFuncPtr intStatus;
sensorGyroInterruptStatusFuncPtr intStatus;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
float scale; // scalefactor
uint32_t targetLooptime;
uint16_t lpf;
volatile bool dataReady;
} gyro_t;
typedef struct acc_s {

16
src/main/drivers/accgyro_l3g4200d.c
View File

@ -54,8 +54,8 @@
#define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0
static void l3g4200dInit(uint8_t lpf);
static bool l3g4200dRead(int16_t *gyroADC);
static void l3g4200dInit(gyro_t *gyro);
static bool l3g4200dRead(gyro_t *gyro);
bool l3g4200dDetect(gyro_t *gyro)
{
@ -76,13 +76,13 @@ bool l3g4200dDetect(gyro_t *gyro)
return true;
}
static void l3g4200dInit(uint8_t lpf)
static void l3g4200dInit(gyro_t *gyro)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
switch (lpf) {
switch (gyro->lpf) {
default:
case 32:
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
@ -109,7 +109,7 @@ static void l3g4200dInit(uint8_t lpf)
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static bool l3g4200dRead(int16_t *gyroADC)
static bool l3g4200dRead(gyro_t *gyro)
{
uint8_t buf[6];
@ -117,9 +117,9 @@ static bool l3g4200dRead(int16_t *gyroADC)
return false;
}
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}

12
src/main/drivers/accgyro_l3gd20.c
View File

@ -84,9 +84,9 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
}
void l3gd20GyroInit(uint8_t lpf)
void l3gd20GyroInit(gyro_t *gyro)
{
UNUSED(lpf); // FIXME use it!
UNUSED(gyro); // FIXME use it!
l3gd20SpiInit(L3GD20_SPI);
@ -120,7 +120,7 @@ void l3gd20GyroInit(uint8_t lpf)
delay(100);
}
static bool l3gd20GyroRead(int16_t *gyroADC)
static bool l3gd20GyroRead(gyro_t *gyro)
{
uint8_t buf[6];
@ -134,9 +134,9 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
DISABLE_L3GD20;
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
#if 0
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);

60
src/main/drivers/accgyro_mpu.c
View File

@ -25,7 +25,9 @@
#include "build/build_config.h"
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "nvic.h"
@ -52,8 +54,6 @@ 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
@ -221,12 +221,22 @@ static void mpu6050FindRevision(void)
}
}
extiCallbackRec_t mpuIntCallbackRec;
typedef struct mpuIntRec_s {
extiCallbackRec_t exti;
gyro_t *gyro;
} mpuIntRec_t;
void mpuIntExtiHandler(extiCallbackRec_t *cb)
mpuIntRec_t mpuIntRec;
/*
* Gyro interrupt service routine
*/
#if defined(MPU_INT_EXTI)
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
{
UNUSED(cb);
mpuDataReady = true;
mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti);
gyro_t *gyro = rec->gyro;
gyro->dataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
static uint32_t lastCalledAt = 0;
@ -236,17 +246,18 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
lastCalledAt = now;
#endif
}
#endif
void mpuIntExtiInit(void)
static void mpuIntExtiInit(gyro_t *gyro)
{
mpuIntRec.gyro = gyro;
#if defined(MPU_INT_EXTI)
static bool mpuExtiInitDone = false;
if (mpuExtiInitDone || !mpuIntExtiConfig) {
return;
}
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
@ -258,20 +269,20 @@ void mpuIntExtiInit(void)
#if defined (STM32F7)
IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
#else
IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);
#endif
#endif
mpuExtiInitDone = true;
#endif
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
@ -302,28 +313,33 @@ bool mpuAccRead(int16_t *accData)
return true;
}
bool mpuGyroRead(int16_t *gyroADC)
bool mpuGyroRead(gyro_t *gyro)
{
uint8_t data[6];
bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) {
return false;
}
gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
bool checkMPUDataReady(void)
void mpuGyroInit(gyro_t *gyro)
{
mpuIntExtiInit(gyro);
}
bool checkMPUDataReady(gyro_t* gyro)
{
bool ret;
if (mpuDataReady) {
if (gyro->dataReady) {
ret = true;
mpuDataReady= false;
gyro->dataReady= false;
} else {
ret = false;
}

7
src/main/drivers/accgyro_mpu.h
View File

@ -185,8 +185,9 @@ typedef struct mpuDetectionResult_s {
extern mpuDetectionResult_t mpuDetectionResult;
void configureMPUDataReadyInterruptHandling(void);
void mpuIntExtiInit(void);
struct gyro_s;
void mpuGyroInit(struct gyro_s *gyro);
bool mpuAccRead(int16_t *accData);
bool mpuGyroRead(int16_t *gyroADC);
bool mpuGyroRead(struct gyro_s *gyro);
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
bool checkMPUDataReady(void);
bool checkMPUDataReady(struct gyro_s *gyro);

6
src/main/drivers/accgyro_mpu3050.c
View File

@ -46,7 +46,7 @@
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
static void mpu3050Init(uint8_t lpf);
static void mpu3050Init(gyro_t *gyro);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro)
@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
return true;
}
static void mpu3050Init(uint8_t lpf)
static void mpu3050Init(gyro_t *gyro)
{
bool ack;
@ -75,7 +75,7 @@ static void mpu3050Init(uint8_t lpf)
if (!ack)
failureMode(FAILURE_ACC_INIT);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->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);

10
src/main/drivers/accgyro_mpu6050.c
View File

@ -51,7 +51,7 @@
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
static void mpu6050AccInit(acc_t *acc);
static void mpu6050GyroInit(uint8_t lpf);
static void mpu6050GyroInit(gyro_t *gyro);
bool mpu6050AccDetect(acc_t *acc)
{
@ -83,8 +83,6 @@ bool mpu6050GyroDetect(gyro_t *gyro)
static void mpu6050AccInit(acc_t *acc)
{
mpuIntExtiInit();
switch (mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION:
acc->acc_1G = 256 * 4;
@ -95,16 +93,16 @@ static void mpu6050AccInit(acc_t *acc)
}
}
static void mpu6050GyroInit(uint8_t lpf)
static void mpu6050GyroInit(gyro_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
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)
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //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
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)
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
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.

8
src/main/drivers/accgyro_mpu6500.c
View File

@ -64,14 +64,12 @@ bool mpu6500GyroDetect(gyro_t *gyro)
void mpu6500AccInit(acc_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 4;
}
void mpu6500GyroInit(uint8_t lpf)
void mpu6500GyroInit(gyro_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
@ -85,7 +83,7 @@ void mpu6500GyroInit(uint8_t lpf)
delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100);

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

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

8
src/main/drivers/accgyro_spi_icm20689.c
View File

@ -137,14 +137,12 @@ bool icm20689SpiGyroDetect(gyro_t *gyro)
void icm20689AccInit(acc_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 4;
}
void icm20689GyroInit(uint8_t lpf)
void icm20689GyroInit(gyro_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
@ -160,7 +158,7 @@ void icm20689GyroInit(uint8_t lpf)
delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100);

2
src/main/drivers/accgyro_spi_icm20689.h
View File

@ -23,7 +23,7 @@ bool icm20689AccDetect(acc_t *acc);
bool icm20689GyroDetect(gyro_t *gyro);
void icm20689AccInit(acc_t *acc);
void icm20689GyroInit(uint8_t lpf);
void icm20689GyroInit(gyro_t *gyro);
bool icm20689SpiDetect(void);

13
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -124,32 +124,29 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true;
}
void mpu6000SpiGyroInit(uint8_t lpf)
void mpu6000SpiGyroInit(gyro_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
mpu6000AccAndGyroInit();
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, lpf);
mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
int16_t data[3];
mpuGyroRead(data);
mpuGyroRead(gyro);
if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu6000SpiAccInit(acc_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 4;
}

4
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -99,12 +99,12 @@ void mpu6500SpiAccInit(acc_t *acc)
mpu6500AccInit(acc);
}
void mpu6500SpiGyroInit(uint8_t lpf)
void mpu6500SpiGyroInit(gyro_t *gyro)
{
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
delayMicroseconds(1);
mpu6500GyroInit(lpf);
mpu6500GyroInit(gyro);
// Disable Primary I2C Interface
mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);

2
src/main/drivers/accgyro_spi_mpu6500.h
View File

@ -20,7 +20,7 @@
bool mpu6500SpiDetect(void);
void mpu6500SpiAccInit(acc_t *acc);
void mpu6500SpiGyroInit(uint8_t lpf);
void mpu6500SpiGyroInit(gyro_t *gyro);
bool mpu6500SpiAccDetect(acc_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro);

15
src/main/drivers/accgyro_spi_mpu9250.c
View File

@ -96,22 +96,19 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true;
}
void mpu9250SpiGyroInit(uint8_t lpf)
void mpu9250SpiGyroInit(gyro_t *gyro)
{
(void)(lpf);
mpuGyroInit(gyro);
mpuIntExtiInit();
mpu9250AccAndGyroInit(lpf);
mpu9250AccAndGyroInit(gyro->lpf);
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
int16_t data[3];
mpuGyroRead(data);
mpuGyroRead(gyro);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
@ -119,8 +116,6 @@ void mpu9250SpiGyroInit(uint8_t lpf)
void mpu9250SpiAccInit(acc_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 8;
}

4
src/main/drivers/gyro_sync.c
View File

@ -16,11 +16,11 @@
static uint8_t mpuDividerDrops;
bool gyroSyncCheckUpdate(const gyro_t *gyro)
bool gyroSyncCheckUpdate(gyro_t *gyro)
{
if (!gyro->intStatus)
return false;
return gyro->intStatus();
return gyro->intStatus(gyro);
}
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)

2
src/main/drivers/gyro_sync.h
View File

@ -6,6 +6,6 @@
*/
struct gyro_s;
bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
bool gyroSyncCheckUpdate(struct gyro_s *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(void);
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

7
src/main/drivers/sensor.h
View File

@ -21,5 +21,8 @@ struct acc_s;
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
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 bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready
struct gyro_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyro_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyro_s *gyro);
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyro_s *gyro);
typedef bool (*sensorInterruptFuncPtr)(void);

2
src/main/fc/fc_msp.c
View File

@ -612,7 +612,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, accSmooth[i] / scale);
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroADC[i]);
sbufWriteU16(dst, lrintf(gyroADCf[i] / gyro.scale));
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, magADC[i]);

5
src/main/flight/imu.c
View File

@ -75,8 +75,6 @@ static float rMat[3][3];
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
static float gyroScale;
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
{
float q1q1 = sq(q1);
@ -122,7 +120,6 @@ void imuConfigure(
void imuInit(void)
{
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
accVelScale = 9.80665f / acc.acc_1G / 10000.0f;
imuComputeRotationMatrix();
@ -399,7 +396,7 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
DEGREES_TO_RADIANS(gyroADCf[X]), DEGREES_TO_RADIANS(gyroADCf[Y]), DEGREES_TO_RADIANS(gyroADCf[Z]),
useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
useMag, magADC[X], magADC[Y], magADC[Z],
useYaw, rawYawError);

2
src/main/flight/pid.c
View File

@ -244,7 +244,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
}
const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
const float PVRate = gyroADCf[axis]; // Process variable from gyro output in deg/sec
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------

3
src/main/io/dashboard.c
View File

@ -18,6 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
@ -495,7 +496,7 @@ void showSensorsPage(void)
}
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]);
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyroADCf[X]), lrintf(gyroADCf[Y]), lrintf(gyroADCf[Z]));
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);

36
src/main/sensors/gyro.c
View File

@ -39,7 +39,7 @@
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
static int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
@ -174,41 +174,39 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
if (!gyro.read(&gyro)) {
return;
}
gyroADC[X] = gyroADCRaw[X];
gyroADC[Y] = gyroADCRaw[Y];
gyroADC[Z] = gyroADCRaw[Z];
gyro.dataReady = false;
gyroADC[X] = gyro.gyroADCRaw[X];
gyroADC[Y] = gyro.gyroADCRaw[Y];
gyroADC[Z] = gyro.gyroADCRaw[Z];
alignSensors(gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
const bool calibrationComplete = isGyroCalibrationComplete();
if (!calibrationComplete) {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
gyroADC[X] -= gyroZero[X];
gyroADC[Y] -= gyroZero[Y];
gyroADC[Z] -= gyroZero[Z];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
// scale gyro output to degrees per second
gyroADCf[axis] = (float)gyroADC[axis] * gyro.scale;
if (debugMode == DEBUG_GYRO)
debug[axis] = gyroADC[axis];
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf[axis]));
gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]);
gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf[axis]));
gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]);
gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
if (!calibrationComplete) {
gyroADC[axis] = lrintf(gyroADCf[axis] / gyro.scale);
}
}
}

1
src/main/sensors/gyro.h
View File

@ -37,7 +37,6 @@ typedef enum {
extern gyro_t gyro;
extern int32_t gyroADC[XYZ_AXIS_COUNT];
extern float gyroADCf[XYZ_AXIS_COUNT];
typedef struct gyroConfig_s {

15
src/main/sensors/initialisation.c
View File

@ -102,15 +102,15 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
#ifdef USE_FAKE_GYRO
int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 };
static void fakeGyroInit(uint8_t lpf)
static void fakeGyroInit(gyro_t *gyro)
{
UNUSED(lpf);
UNUSED(gyro);
}
static bool fakeGyroRead(int16_t *gyroADC)
static bool fakeGyroRead(gyro_t *gyro)
{
for (int i = 0; i < XYZ_AXIS_COUNT; ++i) {
gyroADC[i] = fake_gyro_values[i];
gyro->gyroADCRaw[X] = fake_gyro_values[i];
}
return true;
@ -123,7 +123,9 @@ static bool fakeGyroReadTemp(int16_t *tempData)
}
static bool fakeGyroInitStatus(void) {
static bool fakeGyroInitStatus(gyro_t *gyro)
{
UNUSED(gyro);
return true;
}
@ -662,7 +664,8 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
// Now time to init things
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.init(gyroConfig->gyro_lpf); // driver initialisation
gyro.lpf = gyroConfig->gyro_lpf;
gyro.init(&gyro); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation
if (detectAcc(sensorSelectionConfig->acc_hardware)) {

2
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -613,7 +613,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
for (i = 0; i < 3; i++)
bstWrite16(accSmooth[i] / scale);
for (i = 0; i < 3; i++)
bstWrite16(gyroADC[i]);
bstWrite16(lrintf(gyroADCf[i]));
for (i = 0; i < 3; i++)
bstWrite16(magADC[i]);
}