mirror of
https://github.com/ADElectronics/RTL00_WEB_VS.git
synced 2026-07-02 05:55:39 +00:00
update
This commit is contained in:
ADElectronics
parent
6a1f93f17b
commit
764b020238
1201 changed files with 527271 additions and 1 deletions
223
RTLGDB/Project/MPU6050/MPU6050.c
Normal file
223
RTLGDB/Project/MPU6050/MPU6050.c
Normal file
|
|
@ -0,0 +1,223 @@
|
|||
#include "MPU6050.h"
|
||||
#include "kalman.h"
|
||||
|
||||
MPU6050_DataPack dataPack1[MPU6050_PACKSIZE], dataPack2[MPU6050_PACKSIZE];
|
||||
uint16_t currentDataPackPointer = 0;
|
||||
uint8_t currentDataPack = 0;
|
||||
uint8_t previousDataPackReady = 0;
|
||||
|
||||
MPU6050_Data mpu =
|
||||
{
|
||||
.i2c.idx = 3, // I2C3
|
||||
.i2c.io_sel = S0, // PB_2, PB_3
|
||||
.i2c.mode = DRV_I2C_FS_MODE,
|
||||
};
|
||||
|
||||
void MPU6050T_irq_handler(uint32_t *par)
|
||||
{
|
||||
MPU6050_Data_p p = &mpu;
|
||||
|
||||
// MPU6050_ReadGyroscope
|
||||
uint8_t data[2];
|
||||
data[0] = MPU6050_GYRO_XOUT_H; // MPU6050_ACCEL_XOUT_H;// MPU6050_GYRO_XOUT_H;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &data[0], 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &data, 2, 1);
|
||||
|
||||
p->Gyro_X = (int16_t)(data[0] << 8 | data[1]);
|
||||
|
||||
data[0] = MPU6050_ACCEL_XOUT_H;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &data[0], 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &data, 2, 1);
|
||||
|
||||
p->Accel_X = (int16_t)(data[0] << 8 | data[1]);
|
||||
|
||||
if (currentDataPack)
|
||||
{
|
||||
dataPack2[currentDataPackPointer].X = Kalman_GetAngle(p->Accel_X, p->Gyro_X, 0.001);
|
||||
}
|
||||
else
|
||||
{
|
||||
dataPack1[currentDataPackPointer].X = Kalman_GetAngle(p->Accel_X, p->Gyro_X, 0.001);
|
||||
}
|
||||
|
||||
currentDataPackPointer++;
|
||||
if (currentDataPackPointer >= MPU6050_PACKSIZE)
|
||||
{
|
||||
currentDataPackPointer = 0;
|
||||
currentDataPack = !currentDataPack;
|
||||
previousDataPackReady = 1;
|
||||
}
|
||||
}
|
||||
|
||||
int16_t MPU6050_Init()
|
||||
{
|
||||
MPU6050_Data_p p = &mpu;
|
||||
uint8_t temp;
|
||||
uint8_t d[2];
|
||||
|
||||
Kalman_Init();
|
||||
|
||||
_i2c_init(&p->i2c);
|
||||
|
||||
d[0] = MPU6050_PWR_MGMT_1;
|
||||
d[1] = 0x00;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &d, 2, 1);
|
||||
|
||||
|
||||
temp = MPU6050_WHO_AM_I;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 1);
|
||||
|
||||
if (temp != MPU6050_I_AM)
|
||||
{
|
||||
DiagPrintf("MPU6050. Init Error. ID = %d (default:%d)\n", temp, MPU6050_I_AM);
|
||||
return MPU6050_RESULT_ERROR;
|
||||
}
|
||||
else
|
||||
{
|
||||
DiagPrintf("MPU6050. Init Done. ID = %d\n", temp);
|
||||
}
|
||||
|
||||
MPU6050_SetDataRate(MPU6050_DataRate_8KHz);
|
||||
MPU6050_SetAccelerometer(MPU6050_Accelerometer_2G);
|
||||
MPU6050_SetGyroscope(MPU6050_Gyroscope_250s);
|
||||
MPU6050_SetDLPF(MPU6050_Bandwidth_Infinity);
|
||||
|
||||
MPU6050_OffsetCalibrate(2000);
|
||||
|
||||
gtimer_init(&p->timer, MPU6050_IRQTIMER);
|
||||
gtimer_start_periodical(&p->timer, MPU6050_PERIOD_US, (void*)MPU6050T_irq_handler, (uint32_t)&mpu);
|
||||
|
||||
return MPU6050_RESULT_OK;
|
||||
}
|
||||
|
||||
void MPU6050_SetGyroscope(MPU6050_Gyroscope gs)
|
||||
{
|
||||
MPU6050_Data_p p = &mpu;
|
||||
uint8_t temp;
|
||||
temp = MPU6050_GYRO_CONFIG;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 1);
|
||||
|
||||
temp = (temp & 0xE7) | (uint8_t)gs << 3;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 1);
|
||||
}
|
||||
|
||||
void MPU6050_SetAccelerometer(MPU6050_Accelerometer as)
|
||||
{
|
||||
MPU6050_Data_p p = &mpu;
|
||||
uint8_t temp;
|
||||
temp = MPU6050_ACCEL_CONFIG;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 1);
|
||||
|
||||
temp = (temp & 0xE7) | (uint8_t)as << 3;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &temp, 1, 1);
|
||||
}
|
||||
|
||||
void MPU6050_SetDataRate(uint8_t rate)
|
||||
{
|
||||
MPU6050_Data_p p = &mpu;
|
||||
uint8_t d[2];
|
||||
|
||||
d[0] = MPU6050_SMPLRT_DIV;
|
||||
d[1] = rate;
|
||||
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &d, 2, 1);
|
||||
}
|
||||
int16_t MPU6050_IsDataPacketReady(MPU6050_DataPack *dp)
|
||||
{
|
||||
if (previousDataPackReady)
|
||||
{
|
||||
previousDataPackReady = 0;
|
||||
|
||||
if (currentDataPack)
|
||||
{
|
||||
memcpy(dp, &dataPack1, sizeof(MPU6050_DataPack) * MPU6050_PACKSIZE);
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(dp, &dataPack2, sizeof(MPU6050_DataPack) * MPU6050_PACKSIZE);
|
||||
}
|
||||
return MPU6050_RESULT_OK;
|
||||
}
|
||||
|
||||
return MPU6050_RESULT_NO;
|
||||
}
|
||||
|
||||
void MPU6050_OffsetCalibrate(int32_t num)
|
||||
{
|
||||
MPU6050_Data_p p = &mpu;
|
||||
int32_t x = 0, y = 0, z = 0, i;
|
||||
uint8_t data[6];
|
||||
|
||||
DiagPrintf("MPU6050. Calibrating gyroscope, don't move the hardware!\n");
|
||||
//vTaskDelay(500);
|
||||
|
||||
data[0] = MPU6050_GYRO_XOUT_H;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &data[0], 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &data, 6, 1);
|
||||
|
||||
x = (int16_t)(data[0] << 8 | data[1]);
|
||||
y = (int16_t)(data[2] << 8 | data[3]);
|
||||
z = (int16_t)(data[4] << 8 | data[5]);
|
||||
|
||||
for (i = 0; i < num; i++)
|
||||
{
|
||||
data[0] = MPU6050_GYRO_XOUT_H;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &data[0], 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &data, 6, 1);
|
||||
|
||||
x = (x + (int16_t)(data[0] << 8 | data[1])) / 2;
|
||||
y = (y + (int16_t)(data[2] << 8 | data[3])) / 2;
|
||||
z = (z + (int16_t)(data[4] << 8 | data[5])) / 2;
|
||||
}
|
||||
|
||||
//gyro_x_OC = x;
|
||||
//gyro_y_OC = y;
|
||||
//gyro_z_OC = z;
|
||||
|
||||
DiagPrintf("MPU6050. Gyroscope offsets: X = %d Y = %d Z = %d\n", x, y, z);
|
||||
|
||||
data[0] = MPU6050_ACCEL_XOUT_H;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &data[0], 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &data, 6, 1);
|
||||
|
||||
DiagPrintf("MPU6050. Calibrating accelrometer, don't move the hardware!\n");
|
||||
|
||||
x = (int16_t)(data[0] << 8 | data[1]);
|
||||
y = (int16_t)(data[2] << 8 | data[3]);
|
||||
z = (int16_t)(data[4] << 8 | data[5]);
|
||||
|
||||
for (i = 0; i < num; i++)
|
||||
{
|
||||
data[0] = MPU6050_ACCEL_XOUT_H;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &data[0], 1, 0);
|
||||
_i2c_read(&p->i2c, MPU6050_I2C_ADDR, &data, 6, 1);
|
||||
|
||||
x = (x + (int16_t)(data[0] << 8 | data[1])) / 2;
|
||||
y = (y + (int16_t)(data[2] << 8 | data[3])) / 2;
|
||||
z = (z + (int16_t)(data[4] << 8 | data[5])) / 2;
|
||||
}
|
||||
|
||||
//accel_x_OC = x;
|
||||
//accel_y_OC = y;
|
||||
//accel_z_OC = z - (float)g * 1000 / accel_scale_fact;
|
||||
|
||||
DiagPrintf("MPU6050. Accelrometer offsets: X = %d Y = %d Z = %d\n", x, y, z);
|
||||
}
|
||||
|
||||
void MPU6050_SetDLPF(uint8_t Bandwidth)
|
||||
{
|
||||
MPU6050_Data_p p = &mpu;
|
||||
uint8_t data[2];
|
||||
|
||||
if (Bandwidth < MPU6050_Bandwidth_Infinity || Bandwidth > MPU6050_Bandwidth_5Hz)
|
||||
{
|
||||
Bandwidth = MPU6050_Bandwidth_Infinity;
|
||||
}
|
||||
|
||||
data[0] = MPU6050_CONFIG;
|
||||
data[1] = Bandwidth;
|
||||
_i2c_write(&p->i2c, MPU6050_I2C_ADDR, &data[0], 2, 1);
|
||||
}
|
||||
140
RTLGDB/Project/MPU6050/MPU6050.h
Normal file
140
RTLGDB/Project/MPU6050/MPU6050.h
Normal file
|
|
@ -0,0 +1,140 @@
|
|||
#ifndef _MPU6050_H_
|
||||
#define _MPU6050_H_
|
||||
|
||||
#include "FreeRTOS.h"
|
||||
#include "device.h"
|
||||
#include "rtl8195a/rtl_libc.h"
|
||||
#include "platform_stdlib.h"
|
||||
|
||||
#include "timer_api.h"
|
||||
#include "driver/i2c_drv.h"
|
||||
#include "pinmap.h"
|
||||
|
||||
#define MPU6050_IRQTIMER TIMER0
|
||||
#define MPU6050_I2C_ADDR 0x68
|
||||
#define MPU6050_PACKSIZE 4096
|
||||
#define MPU6050_PERIOD_US 1000
|
||||
|
||||
typedef struct _MPU6050_Data
|
||||
{
|
||||
// Ïðèâàòíûå
|
||||
i2c_drv_t i2c;
|
||||
gtimer_t timer;
|
||||
|
||||
// Ïóáëè÷íûå
|
||||
int16_t Accel_X;
|
||||
int16_t Accel_Y;
|
||||
int16_t Accel_Z;
|
||||
int16_t Gyro_X;
|
||||
int16_t Gyro_Y;
|
||||
int16_t Gyro_Z;
|
||||
float Temp_Deg;
|
||||
} MPU6050_Data, *MPU6050_Data_p;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double X;
|
||||
//int16_t Y;
|
||||
//int16_t Z;
|
||||
} MPU6050_DataPack;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MPU6050_Accelerometer_2G = 0x00,
|
||||
MPU6050_Accelerometer_4G = 0x01,
|
||||
MPU6050_Accelerometer_8G = 0x02,
|
||||
MPU6050_Accelerometer_16G = 0x03
|
||||
} MPU6050_Accelerometer;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MPU6050_Bandwidth_Infinity = 0x00,
|
||||
MPU6050_Bandwidth_184Hz = 0x01,
|
||||
MPU6050_Bandwidth_94Hz = 0x02,
|
||||
MPU6050_Bandwidth_44Hz = 0x03,
|
||||
MPU6050_Bandwidth_21Hz = 0x04,
|
||||
MPU6050_Bandwidth_10Hz = 0x05,
|
||||
MPU6050_Bandwidth_5Hz = 0x06,
|
||||
} MPU6050_DLFP;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MPU6050_Gyroscope_250s = 0x00,
|
||||
MPU6050_Gyroscope_500s = 0x01,
|
||||
MPU6050_Gyroscope_1000s = 0x02,
|
||||
MPU6050_Gyroscope_2000s = 0x03
|
||||
} MPU6050_Gyroscope;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MPU6050_RESULT_OK = 1,
|
||||
MPU6050_RESULT_NO = 0,
|
||||
MPU6050_RESULT_ERROR = -1,
|
||||
MPU6050_RESULT_DEVNOTCONN = -2
|
||||
} MPU6050_Result;
|
||||
|
||||
#define MPU6050_DataRate_8KHz 0
|
||||
#define MPU6050_DataRate_4KHz 1
|
||||
#define MPU6050_DataRate_2KHz 3
|
||||
#define MPU6050_DataRate_1KHz 7
|
||||
#define MPU6050_DataRate_500Hz 15
|
||||
#define MPU6050_DataRate_250Hz 31
|
||||
#define MPU6050_DataRate_125Hz 63
|
||||
#define MPU6050_DataRate_100Hz 79
|
||||
|
||||
#define MPU6050_AUX_VDDIO 0x01
|
||||
#define MPU6050_SMPLRT_DIV 0x19
|
||||
#define MPU6050_CONFIG 0x1A
|
||||
#define MPU6050_GYRO_CONFIG 0x1B
|
||||
#define MPU6050_ACCEL_CONFIG 0x1C
|
||||
#define MPU6050_MOTION_THRESH 0x1F
|
||||
#define MPU6050_INT_PIN_CFG 0x37
|
||||
#define MPU6050_INT_ENABLE 0x38
|
||||
#define MPU6050_INT_STATUS 0x3A
|
||||
#define MPU6050_ACCEL_XOUT_H 0x3B
|
||||
#define MPU6050_ACCEL_XOUT_L 0x3C
|
||||
#define MPU6050_ACCEL_YOUT_H 0x3D
|
||||
#define MPU6050_ACCEL_YOUT_L 0x3E
|
||||
#define MPU6050_ACCEL_ZOUT_H 0x3F
|
||||
#define MPU6050_ACCEL_ZOUT_L 0x40
|
||||
#define MPU6050_TEMP_OUT_H 0x41
|
||||
#define MPU6050_TEMP_OUT_L 0x42
|
||||
#define MPU6050_GYRO_XOUT_H 0x43
|
||||
#define MPU6050_GYRO_XOUT_L 0x44
|
||||
#define MPU6050_GYRO_YOUT_H 0x45
|
||||
#define MPU6050_GYRO_YOUT_L 0x46
|
||||
#define MPU6050_GYRO_ZOUT_H 0x47
|
||||
#define MPU6050_GYRO_ZOUT_L 0x48
|
||||
#define MPU6050_MOT_DETECT_STATUS 0x61
|
||||
#define MPU6050_SIGNAL_PATH_RESET 0x68
|
||||
#define MPU6050_MOT_DETECT_CTRL 0x69
|
||||
#define MPU6050_USER_CTRL 0x6A
|
||||
#define MPU6050_PWR_MGMT_1 0x6B
|
||||
#define MPU6050_PWR_MGMT_2 0x6C
|
||||
#define MPU6050_FIFO_COUNTH 0x72
|
||||
#define MPU6050_FIFO_COUNTL 0x73
|
||||
#define MPU6050_FIFO_R_W 0x74
|
||||
#define MPU6050_WHO_AM_I 0x75
|
||||
#define MPU6050_I_AM 0x68
|
||||
|
||||
// Gyro sensitivities in degrees/s
|
||||
#define MPU6050_GYRO_SENS_250 ((double) 131.0)
|
||||
#define MPU6050_GYRO_SENS_500 ((double) 65.5)
|
||||
#define MPU6050_GYRO_SENS_1000 ((double) 32.8)
|
||||
#define MPU6050_GYRO_SENS_2000 ((double) 16.4)
|
||||
|
||||
// Acce sensitivities in g/s
|
||||
#define MPU6050_ACCE_SENS_2 ((double) 16384.0)
|
||||
#define MPU6050_ACCE_SENS_4 ((double) 8192.0)
|
||||
#define MPU6050_ACCE_SENS_8 ((double) 4096.0)
|
||||
#define MPU6050_ACCE_SENS_16 ((double) 2048.0)
|
||||
|
||||
int16_t MPU6050_Init();
|
||||
void MPU6050_SetGyroscope(MPU6050_Gyroscope gs);
|
||||
void MPU6050_SetAccelerometer(MPU6050_Accelerometer as);
|
||||
void MPU6050_SetDataRate(uint8_t rate);
|
||||
int16_t MPU6050_IsDataPacketReady(MPU6050_DataPack *dp);
|
||||
void MPU6050_OffsetCalibrate(int32_t num);
|
||||
void MPU6050_SetDLPF(uint8_t Bandwidth);
|
||||
|
||||
#endif // _MPU6050_H_
|
||||
65
RTLGDB/Project/MPU6050/kalman.c
Normal file
65
RTLGDB/Project/MPU6050/kalman.c
Normal file
|
|
@ -0,0 +1,65 @@
|
|||
#include "kalman.h"
|
||||
|
||||
const double Q_angle = 0.01;
|
||||
const double Q_bias = 0.003;
|
||||
const double R_measure = 0.03;
|
||||
|
||||
static double angle;
|
||||
static double bias;
|
||||
static double rate;
|
||||
|
||||
double P[2][2];
|
||||
double K[2];
|
||||
double y;
|
||||
double S;
|
||||
|
||||
void Kalman_Init()
|
||||
{
|
||||
bias = 0;
|
||||
rate = 0;
|
||||
angle = 180;
|
||||
|
||||
P[0][0] = 0;
|
||||
P[0][1] = 0;
|
||||
P[1][0] = 0;
|
||||
P[1][1] = 0;
|
||||
}
|
||||
|
||||
// Óãîë â ãðàäóñàõ è óãëîâàÿ ñêîðîñòü â ãðàäóñ\ñåê, ïåðèîä â ñåêóíäàõ.
|
||||
double Kalman_GetAngle(double newAngle, double newRate, double dt)
|
||||
{
|
||||
// Step 1
|
||||
rate = newRate - bias;
|
||||
angle += dt * rate;
|
||||
|
||||
// Update estimation error covariance - Project the error covariance ahead
|
||||
// Step 2
|
||||
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
|
||||
P[0][1] -= dt * P[1][1];
|
||||
P[1][0] -= dt * P[1][1];
|
||||
P[1][1] += Q_bias * dt;
|
||||
|
||||
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
|
||||
// Calculate Kalman gain - Compute the Kalman gain
|
||||
// Step 4
|
||||
S = P[0][0] + R_measure;
|
||||
// Step 5
|
||||
K[0] = P[0][0] / S;
|
||||
K[1] = P[1][0] / S;
|
||||
|
||||
// Calculate angle and bias - Update estimate with measurement zk(newAngle)
|
||||
// Step 3
|
||||
y = newAngle - angle;
|
||||
// Step 6
|
||||
angle += K[0] * y;
|
||||
bias += K[1] * y;
|
||||
|
||||
// Calculate estimation error covariance - Update the error covariance
|
||||
// Step 7
|
||||
P[0][0] -= K[0] * P[0][0];
|
||||
P[0][1] -= K[0] * P[0][1];
|
||||
P[1][0] -= K[1] * P[0][0];
|
||||
P[1][1] -= K[1] * P[0][1];
|
||||
|
||||
return angle;
|
||||
}
|
||||
12
RTLGDB/Project/MPU6050/kalman.h
Normal file
12
RTLGDB/Project/MPU6050/kalman.h
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
#ifndef _KALMAN_H_
|
||||
#define _KALMAN_H_
|
||||
|
||||
#include "FreeRTOS.h"
|
||||
#include "device.h"
|
||||
#include "rtl8195a/rtl_libc.h"
|
||||
#include "platform_stdlib.h"
|
||||
|
||||
void Kalman_Init();
|
||||
double Kalman_GetAngle(double newAngle, double newRate, double dt);
|
||||
|
||||
#endif // _KALMAN_H_
|
||||
Loading…
Add table
Add a link
Reference in a new issue