static int mpu6050_reset(struct mpu6050_data *data)
{
unsigned char val = 0;
int error;
error = MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_PWR_MGMT_1, 1, &val, "Reset");
if (error) {
dev_err(data->dev, "fail to read PWR_MGMT_1 reg\n");
return error;
}
/* Reset sequence */
val |= MPU6050_DEVICE_RESET;
error = MPU6050_WRITE(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_PWR_MGMT_1, val, "Reset");
if (error) {
dev_err(data->dev, "fail to set reset bit\n");
return error;
}
error = MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_PWR_MGMT_1, 1, &val, "Reset");
if (error) {
dev_err(data->dev, "fail to read PWR_MGMT_1 reg\n");
return error;
}
if (val & MPU6050_DEVICE_RESET) {
dev_err(data->dev, "Reset failed\n");
return val;
}
/*
* Reset of PWR_MGMT_1 reg puts the device in sleep mode,
* so disable the sleep mode
*/
mpu6050_enable_sleep(data, 0);
return 0;
}
static int mpu6050_enable_sleep(struct mpu6050_data *data, int action)
{
unsigned char val = 0;
MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_PWR_MGMT_1, 1, &val, "Enable Sleep");
if (action)
val |= MPU6050_DEVICE_SLEEP_MODE;
else
val &= ~MPU6050_DEVICE_SLEEP_MODE;
MPU6050_WRITE(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_PWR_MGMT_1, val, "Enable Sleep");
MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_PWR_MGMT_1, 1, &val, "Enable Sleep");
if (val & MPU6050_DEVICE_SLEEP_MODE)
dev_err(data->dev, "Enable Sleep failed\n");
return 0;
}
static ssize_t mpu6050_registers_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct platform_device *pdev = to_platform_device(dev);
struct mpu6050_data *data = platform_get_drvdata(pdev);
unsigned i, n, reg_count;
uint8_t value;
reg_count = sizeof(mpu6050_regs) / sizeof(mpu6050_regs[0]);
for (i = 0, n = 0; i < reg_count; i++) {
MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
mpu6050_regs[i].reg, 1, &value, mpu6050_regs[i].name);
n += scnprintf(buf + n, PAGE_SIZE - n,
"%-20s = 0x%02X\n",
mpu6050_regs[i].name, value);
}
return n;
}
//*****TIM2中断函数2ms*****//
void TIM2_IRQHandler(void)
{
if(TIM_GetITStatus(TIM2,TIM_IT_Update) == SET)//检测TIM2溢出中断是否发生
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
time2_tick++;
time2_tick = time2_tick%1000;//2HZ
//LED0(ON);
//START////////////////////数据读取+姿态解算+PID控制 1.4ms////////////////////////////
MPU6050_READ();//读取
acc[0] = MPU6050_ACC_LAST.X;
acc[1] = MPU6050_ACC_LAST.Y;
acc[2] = MPU6050_ACC_LAST.Z;
gyro[0] = MPU6050_GYRO_LAST.X - GYRO_OFFSET.X;
gyro[1] = -(MPU6050_GYRO_LAST.Y - GYRO_OFFSET.Y);
gyro[2] = MPU6050_GYRO_LAST.Z - GYRO_OFFSET.Z;
if(gyro[2]>=-5 &&gyro[2]<=5) gyro[2] = 0;//Z轴陀螺仪,漂移处理
//printf("%d,%d,%d",acc[0],acc[1],acc[2]);
IMUupdate(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2],Vaule_2_Gyro());//非全姿态角解算
//IMUupdate1(number_to_dps1(gyro[0]),number_to_dps1(gyro[1]),number_to_dps1(gyro[2]),acc[0],acc[1],acc[2]);
//IMUupdate2(number_to_dps1(gyro[0]),number_to_dps1(gyro[1]),number_to_dps1(gyro[2]),acc[0],acc[1],acc[2]);
CONTROL(Q_ANGLE.ROLL,Q_ANGLE.PITCH,Q_ANGLE.YAW,Value_2_Thr(),Value_2_Roll(),Value_2_Pitch(),Vaule_2_Gyro());//对电机进行PID控制
//CONTROL1(Q_ANGLE.ROLL,Q_ANGLE.PITCH,Q_ANGLE.YAW,Value_2_Thr(),Value_2_Roll(),Value_2_Pitch(),Vaule_2_Gyro(), acc, gyro);
//END///////////////////////////////////////////////////////////////////////
//LED0(OFF);
if(time2_tick%5 == 0)//10ms -- 100HZ进行一次
{
}
if(time2_tick%25 == 0)//50ms -- 20HZ进行一次
{
DMA_DATA_SEND_FLAG = 1;//上传数据
}
if(time2_tick%50 == 0)//100ms -- 10HZ进行一次
{
}
if(time2_tick%500 == 0)//1000ms -- 1HZ进行一次
{
if(ARMED == 1)
{
Is_DisArmed(RC_CH[3-1],RC_CH[4-1]);//是否加锁
}
else
{
Is_Armed(RC_CH[3-1],RC_CH[4-1]);//是否解锁
}
}
time2_led++;
if(time2_led>=1000) time2_led=time2_led-1000;
// time2_led = time2_led%1000;//2ms一个周期
switch(led_state)
{
case 0:Led_Flash0();break;
case 1:Led_Flash1();break;
case 2:Led_Flash2();break;
case 3:Led_Flash3();break;
}
}
}
void System_Time_Init(void)
{
MPU6050_READ();//读取
Get_Accel_Angle(MPU6050_ACC_LAST.X,MPU6050_ACC_LAST.Y,MPU6050_ACC_LAST.Z,&Q_ANGLE.ROLL,&Q_ANGLE.PITCH);//加速度算出的角度为初始角度
TIM2_Config();
}
static int mpu6050_set_bypass_mode(struct mpu6050_data *data)
{
int error;
unsigned char val = 0;
error = MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_I2C_MST_STATUS,
1, &val, "Pass Through");
if (error) {
dev_err(data->dev,
"MPU6050: fail to read MST_STATUS register\n");
return error;
}
val |= MPU6050_PASS_THROUGH;
error = MPU6050_WRITE(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_I2C_MST_STATUS,
val, "Pass Through");
if (error) {
dev_err(data->dev,
"MPU6050: fail to set pass through\n");
return error;
}
/* Bypass Enable Configuration */
error = MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_INT_PIN_CFG,
1, &val, "Bypass enabled");
if (error) {
dev_err(data->dev,
"MPU6050: fail to read INT_PIN_CFG register\n");
return error;
}
val |= MPU6050_I2C_BYPASS_EN;
error = MPU6050_WRITE(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_INT_PIN_CFG,
val, "Bypass enabled");
if (error) {
dev_err(data->dev,
"MPU6050: fail to set i2c bypass mode\n");
return error;
}
error = MPU6050_READ(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_USER_CTRL,
1, &val, "I2C MST mode");
if (error) {
dev_err(data->dev,
"MPU6050: fail to read USER_CTRL register\n");
return error;
}
val &= ~MPU6050_I2C_MST_EN;
error = MPU6050_WRITE(data, MPU6050_CHIP_I2C_ADDR,
MPU6050_REG_USER_CTRL,
val, "I2C MST mode");
if (error) {
dev_err(data->dev,
"MPU6050: fail to set i2c master enable bit\n");
return error;
}
return 0;
}
