ziggi/stm32uav
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add Robert Mahony correlator

Browse Source
This commit is contained in:
Oleg Borodin
2022-09-06 14:45:07 +02:00
parent 390077cb1b
commit 8af4151c3b
3 changed files with 21873 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mculoop/mpu6050.c
+69 -36
View File
@@ -5,13 +5,17 @@
#include <libopencm3/stm32/i2c.h>
#include <stdint.h>
#include <math.h>
#include <mpu6050.h>
#define MPU_REG_SMPLRT_DIV 0x19
#define MPU_REG_CONFIG 0x1A
#define MPU_REG_GYRO_CONFIG 0x1B
#define MPU_REG_ACCEL_CONFIG 0x1C
#define MPU_REG_ACCEL_XOUT_H 0x3B
#define MPU_REG_ACCEL_XOUT_L 0x3C
#define MPU_REG_ACCEL_YOUT_H 0x3D
@@ -57,6 +61,38 @@
#define MPU_ACCEL_LSB_8 4096.0f
#define MPU_ACCEL_LSB_16 2048.0f
/* PWR_MGMT_1 0x6B */
#define MPU_PWR1_DEVICE_RESET_BIT 7
#define MPU_PWR1_SLEEP_BIT 6
#define MPU_PWR1_CYCLE_BIT 5
#define MPU_PWR1_TEMP_DIS_BIT 3
#define MPU_PWR1_CLKSEL_BASE 0
#define MPU_PWR1_CLKSEL_LEN 3
#define MPU_PWR1_CLKSEL_INTERNAL 0
#define MPU_PWR1_CLKSEL_PLL_XGYRO 1
#define MPU_PWR1_CLKSEL_PLL_YGYRO 2
#define MPU_PWR1_CLKSEL_PLL_ZGYRO 3
#define MPU_PWR1_CLKSEL_PLL_EXT32K 4
#define MPU_PWR1_CLKSEL_PLL_EXT19M 5
#define MPU_PWR1_CLKSEL_KEEP_RESET 7
/* PWR_MGMT_2 0x6C */
#define MPU_PWR2_LP_WAKE_CTRL_BASE 6
#define MPU_PWR2_LP_WAKE_CTRL_LEN 2
#define MPU_PWR2_WAKE_FREQ_1P25 0
#define MPU_PWR2_WAKE_FREQ_2P5 1
#define MPU_PWR2_WAKE_FREQ_5 2
#define MPU_PWR2_WAKE_FREQ_10 3
#define MPU_PWR2_STBY_XA_BIT 5
#define MPU_PWR2_STBY_YA_BIT 4
#define MPU_PWR2_STBY_ZA_BIT 3
#define MPU_PWR2_STBY_XG_BIT 2
#define MPU_PWR2_STBY_YG_BIT 1
#define MPU_PWR2_STBY_ZG_BIT 0
//static uint8_t i2cdev_read_reg8(uint32_t i2c, uint8_t addr, uint8_t reg);
static void i2cdev_write_reg8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t value);
@@ -90,6 +126,12 @@ static void i2cdev_read_seq8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t* b
}
#define MPU_GYRO_LSB MPU_GYRO_LSB_1000
#define MPU_GYRO_FS MPU_GYRO_FS_1000
#define MPU_ACCEL_LSB MPU_ACCEL_LSB_16
#define MPU_ACCEL_FS MPU_ACCEL_FS_16
void mpu_setup(mpu_t* mpu, uint32_t i2c, uint8_t addr) {
mpu->bus = i2c;
@@ -102,35 +144,42 @@ void mpu_setup(mpu_t* mpu, uint32_t i2c, uint8_t addr) {
mpu->err.gy = 0;
mpu->err.gz = 0;
//i2cdev_write_reg8(i2c, addr, MPU_REG_PWR_MGMT_1, 1 << MPU_PWR1_DEVICE_RESET_BIT);
for (int i = 0; i < 10000; i++) __asm__("nop");
i2cdev_write_reg8(i2c, addr, MPU_REG_PWR_MGMT_1, 0x00);
i2cdev_write_reg8(i2c, addr, MPU_REG_GYRO_CONFIG, MPU_GYRO_FS_250 << MPU_GYRO_FS_BASE);
i2cdev_write_reg8(i2c, addr, MPU_REG_ACCEL_CONFIG, MPU_ACCEL_FS_2 << MPU_ACCEL_FS_BASE);
i2cdev_write_reg8(i2c, addr, MPU_REG_GYRO_CONFIG, MPU_GYRO_FS << MPU_GYRO_FS_BASE);
i2cdev_write_reg8(i2c, addr, MPU_REG_ACCEL_CONFIG, MPU_ACCEL_FS << MPU_ACCEL_FS_BASE);
i2cdev_write_reg8(i2c, addr, MPU_REG_SMPLRT_DIV, 4);
}
static void mpu_rawread(mpu_t* mpu, mpu_value_t* val) {
uint8_t buffer[14];
i2cdev_read_seq8(mpu->bus, mpu->addr, MPU_REG_ACCEL_XOUT_H, (uint8_t*)buffer, 14);
int16_t ax = (((int16_t) buffer[0]) << 8) | buffer[1];
int16_t ay = (((int16_t) buffer[2]) << 8) | buffer[3];
int16_t az = (((int16_t) buffer[4]) << 8) | buffer[5];
int16_t ax = (((int16_t)buffer[0]) << 8) | buffer[1];
int16_t ay = (((int16_t)buffer[2]) << 8) | buffer[3];
int16_t az = (((int16_t)buffer[4]) << 8) | buffer[5];
int16_t gx = (((int16_t) buffer[8]) << 8) | buffer[9];
int16_t gy = (((int16_t) buffer[10]) << 8) | buffer[11];
int16_t gz = (((int16_t) buffer[12]) << 8) | buffer[13];
int16_t gx = (((int16_t)buffer[8]) << 8) | buffer[9];
int16_t gy = (((int16_t)buffer[10]) << 8) | buffer[11];
int16_t gz = (((int16_t)buffer[12]) << 8) | buffer[13];
val->ax = (float)ax / MPU_ACCEL_LSB_2;
val->ay = (float)ay / MPU_ACCEL_LSB_2;
val->az = (float)az / MPU_ACCEL_LSB_2;
val->ax = (float)ax / (float)MPU_ACCEL_LSB;
val->ay = (float)ay / (float)MPU_ACCEL_LSB;
val->az = (float)az / (float)MPU_ACCEL_LSB;
val->gx = (float)gx / MPU_GYRO_LSB_250;
val->gy = (float)gy / MPU_GYRO_LSB_250;
val->gz = (float)gz / MPU_GYRO_LSB_250;
val->gx = (float)gx / (float)MPU_GYRO_LSB;
val->gy = (float)gy / (float)MPU_GYRO_LSB;
val->gz = (float)gz / (float)MPU_GYRO_LSB;
//val->gx *= 0.0174533f;
//val->gy *= 0.0174533f;
//val->gz *= 0.0174533f;
val->gx *= M_PI / 180.0f;
val->gy *= M_PI / 180.0f;
val->gz *= M_PI / 180.0f;
}
@@ -146,32 +195,16 @@ void mpu_calibrate(mpu_t* mpu, int count) {
for (int i = 0; i < count; i++) {
mpu_rawread(mpu, &val);
mpu->err.ax += val.ax;
mpu->err.ay += val.ay;
mpu->err.az += val.az;
mpu->err.gx += val.gx;
mpu->err.gy += val.gy;
mpu->err.gz += val.gz;
mpu->err.gx += (float)val.gx / count;
mpu->err.gy += (float)val.gy / count;
mpu->err.gz += (float)val.gz / count;
}
mpu->err.ax /= count;
mpu->err.ay /= count;
mpu->err.az /= count;
mpu->err.gx /= count;
mpu->err.gy /= count;
mpu->err.gz /= count;
}
void mpu_read(mpu_t* mpu, mpu_value_t* val) {
mpu_rawread(mpu, val);
val->ax -= mpu->err.ax;
val->ay -= mpu->err.ay;
val->az -= mpu->err.az;
val->gx -= mpu->err.gx;
val->gy -= mpu->err.gy;
val->gz -= mpu->err.gz;