发现yaw的值在一定时间后会被锁死(oled屏幕上数字不发生改变),查找原因,查找原因修正
empty.c
#include <math.h> // 添加数学库用于fabs函数
#include "board.h"
#include "my_key.h"
#include "my_time.h"
#include "ti_msp_dl_config.h"
#include "oled.h"
void BUZZY_OFF(void) { DL_GPIO_setPins(BUZZY_PORT, BUZZY_PIN_PIN); }
void BUZZY_ON(void) { DL_GPIO_clearPins(BUZZY_PORT, BUZZY_PIN_PIN); }
void refresh_oled(void);
void key(void);
void go_straight(int dis);
void go_arc_ccd(hsu_time_t);
void go_brc_ccd(hsu_time_t);
void turn_in_place(float angle);
void sound_light_alert(void);
void show_task_now(void);
u8 Car_Mode = Diff_Car;
int Motor_Left, Motor_Right; // 电机PWM变量 应是Motor的
u8 PID_Send; // 延时和调参相关变量
float RC_Velocity = 200, RC_Turn_Velocity, Move_X, Move_Y, Move_Z, PS2_ON_Flag; // 遥控控制的速度
float Velocity_Left, Velocity_Right; // 车轮速度(mm/s)
u16 test_num, show_cnt;
float Voltage = 0;
extern float Yaw; // 声明外部YAW角度变量
int64_t left_encoder = 0, right_encoder = 0;
void SysTick_Handler(void) { hsu_time_systick_handler(); }
typedef enum { BEGIN, T1, T2, T3, T4 } TaskState;
typedef enum { STOP, GO_STRAIGHT, GO_CCD, TURN_IN_PLACE, WAIT_ALERT } DoingWhat;
typedef struct __TASK_NAMESPACE {
uint8_t is;
uint8_t is_running;
uint8_t finish;
uint8_t sub_finish;
uint8_t running_state; // 0: 停止, 1: 运行中
TaskState state;
DoingWhat doing_what;
float target;
float vx;
float vz;
// 用于复杂任务
uint8_t sub_task_stage; // 子任务阶段
uint8_t lap_count; // 圈数计数
int64_t start_encoder; // 起始编码器值
uint32_t alert_start_time; // 声光提示开始时间
float start_yaw; // 起始YAW角度
float target_yaw_diff; // 目标YAW角度差
hsu_time_t ccd_end_time;
} TaskNamespace;
void reset_task_namespace(TaskNamespace *t) {
t->is_running = 0;
t->finish = 0;
t->sub_finish = 0;
t->state = BEGIN;
t->doing_what = STOP;
t->vx = 0;
t->vz = 0;
t->sub_task_stage = 0;
t->lap_count = 0;
t->start_encoder = left_encoder;
t->alert_start_time = 0;
t->start_yaw = 0;
t->target_yaw_diff = 0;
t->running_state = 0; // 明确重置运行状态为0
t->ccd_end_time = 0;
t->is = 0;
}
void next_state(TaskNamespace *t) {
TaskState last_state = t->state;
reset_task_namespace(t);
if (last_state < T4) {
t->state = last_state + 1;
}
}
TaskNamespace task_namespace;
void show_task_now(void) {
//OLED_ShowString(0, 0, "Task Now:");
switch (task_namespace.state) {
case BEGIN:
OLED_ShowString(1, 10,"0");
break;
case T1:
OLED_ShowString(1, 10,"1");
break;
case T2:
OLED_ShowString(1, 10,"2");
break;
case T3:
OLED_ShowString(1, 10,"3");
break;
case T4:
OLED_ShowString(1, 10,"4");
break;
default:
break;
}
}
void main_task(void);
int main(void) {
// 系统初始化
SYSCFG_DL_init(); // 初始化系统配置
hsu_time_init(); // 时间
// 清除所有外设的中断挂起状态
NVIC_ClearPendingIRQ(ENCODERA_INT_IRQN); // 编码器A中断
NVIC_ClearPendingIRQ(ENCODERB_INT_IRQN); // 编码器B中断
NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN); // UART0串口中断
// 使能各外设的中断
NVIC_EnableIRQ(ENCODERA_INT_IRQN); // 开启编码器A中断
NVIC_EnableIRQ(ENCODERB_INT_IRQN); // 开启编码器B中断
NVIC_EnableIRQ(UART_0_INST_INT_IRQN); // 开启UART0中断
reset_task_namespace(&task_namespace);
task_namespace.state = BEGIN; // 明确设置初始状态
// 定时器和ADC相关中断配置
NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN); // 清除定时器0中断挂起
NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); // 开启定时器0中断
NVIC_EnableIRQ(ADC12_VOLTAGE_INST_INT_IRQN);
NVIC_EnableIRQ(ADC12_CCD_INST_INT_IRQN);
OLED_Init(); // 初始化OLED显示屏
OLED_ShowString(1, 1, "Task Now:");
OLED_ShowString(2, 1, "state:");
OLED_ShowString(3, 1, "yaw:");
MPU6050_initialize();
DMP_Init();
BUZZY_ON();
// 主循环
// printf("Test delay 500us\n");
// hsu_time_delay_us(500);
// printf("Test delay 500us end\n");
uint8_t main_task_timer = hsu_time_timer_create(10, true, main_task);
hsu_time_timer_start(main_task_timer);
uint8_t refresh_oled_timer = hsu_time_timer_create(5, true, refresh_oled);
hsu_time_timer_start(refresh_oled_timer);
uint8_t key_timer = hsu_time_timer_create(2, true, key);
hsu_time_timer_start(key_timer);
while (1) {
hsu_time_timer_process();
RD_TSL(); // 读取CCD数据
Find_CCD_Median(); // 计算CCD数据中值
Read_DMP();
show_task_now();
char yaw_str[10]; // 存储格式化后的字符串
snprintf(yaw_str, sizeof(yaw_str), "%.1f", Yaw); // 格式化为带一位小数的字符串
OLED_ShowString(3, 6, yaw_str); // 在指定位置显示YAW值
//DL_GPIO_togglePins(LED_PORT, LED_led_PIN);
// printf("L=%lld R=%lld YAW=%.1f\n", left_encoder, right_encoder, Yaw);
}
}
void task_no(void);
void task_1(void);
void task_2(void);
void task_3(void);
void task_4(void);
void main_task(void) {
if (!(task_namespace.is)) return;
printf("main task\n");
switch (task_namespace.state) {
case BEGIN:
task_no();
break;
case T1:
task_1();
break;
case T2:
task_2();
break;
case T3:
task_3();
break;
case T4:
task_4();
break;
default:
break;
}
switch (task_namespace.doing_what) {
case STOP:
Get_Target_Encoder(0, 0);
break;
case GO_STRAIGHT:
if ((left_encoder * 1.f) < task_namespace.target) {
Get_Target_Encoder(0.3, 0); // 提高速度到300mm/s
} else {
Get_Target_Encoder(0, 0);
task_namespace.doing_what = STOP;
task_namespace.finish = 1;
}
break;
case GO_CCD:
if (task_namespace.ccd_end_time < hsu_time_get_ms()) {
Get_Target_Encoder(0, 0);
task_namespace.doing_what = STOP;
task_namespace.finish = 1;
} else {
CCD_Mode();
}
break;
case TURN_IN_PLACE:
// 原地转向控制
if (task_namespace.target_yaw_diff != 0) {
float current_yaw_diff = Yaw - task_namespace.start_yaw;
// 处理角度跨越±180度的情况
if (current_yaw_diff > 180) {
current_yaw_diff -= 360;
} else if (current_yaw_diff < -180) {
current_yaw_diff += 360;
}
printf("Turn: Start=%.1f Current=%.1f Diff=%.1f Target=%.1f\n", task_namespace.start_yaw, Yaw, current_yaw_diff,
task_namespace.target_yaw_diff);
// 检查是否达到目标角度
if ((task_namespace.target_yaw_diff > 0 && current_yaw_diff >= task_namespace.target_yaw_diff) ||
(task_namespace.target_yaw_diff < 0 && current_yaw_diff <= task_namespace.target_yaw_diff)) {
Get_Target_Encoder(0, 0); // 停止转向
task_namespace.doing_what = STOP;
task_namespace.finish = 1;
} else {
// 继续转向
float turn_speed = (task_namespace.target_yaw_diff > 0) ? 0.1 : -0.1;
Get_Target_Encoder(0, turn_speed);
}
}
break;
case WAIT_ALERT:
Get_Target_Encoder(0, 0); // 停车
if (hsu_time_get_ms() - task_namespace.alert_start_time > 1000) { // 声光提示1秒
task_namespace.doing_what = STOP;
task_namespace.finish = 1;
}
break;
default:
break;
}
}
void task_no(void) { return; }
// 任务1:A点到B点直线行驶
void task_1(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
task_namespace.sub_task_stage = 0;
task_namespace.finish = 1;
return;
}
if (task_namespace.finish) {
switch (task_namespace.sub_task_stage) {
case 0: // 开始第一阶段:A到B
go_straight(300);
break;
case 1: // A到B完成,开始B到C
DL_GPIO_togglePins(LED_PORT, LED_led_PIN);
break;
case 2: //任务结束
reset_task_namespace(&task_namespace);
task_namespace.running_state = 0; // 重置为停止状态
break;
}
task_namespace.finish = 0;
task_namespace.sub_task_stage++;
}
return;
}
// 任务2:A->B->C->D->A循环
void task_2(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
task_namespace.sub_task_stage = 0;
task_namespace.finish = 1;
return;
}
if (task_namespace.finish) {
switch (task_namespace.sub_task_stage) {
case 0: // 开始第一阶段:A到B
go_straight(3300);
break;
case 1:
//DL_GPIO_togglePins(LED_PORT, LED_led_PIN);
go_straight(2000);
break;
case 2: // B到C弧线完成,开始C到D直线
sound_light_alert();
//turn_in_place(-17.0f);
go_straight(2980);
break;
case 3: // C到D完成,开始D到A弧线
sound_light_alert();
go_arc_ccd(3530);
break;
case 4: // D到A弧线完成,任务结束
sound_light_alert();
reset_task_namespace(&task_namespace);
break;
}
task_namespace.finish = 0;
task_namespace.sub_task_stage++;
}
return;
}
// 任务3:A->C->B->D->A循环
void task_3(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
task_namespace.sub_task_stage = 0;
task_namespace.finish = 1;
return;
}
if (task_namespace.finish) {
switch (task_namespace.sub_task_stage) {
case 0:
turn_in_place(-31.0f);
break;
case 1:
go_straight(4060);
break;
case 2:
turn_in_place(30.0f);
break;
case 3: // 开始C到B弧线
sound_light_alert();
//go_straight(40);
go_brc_ccd(3550);
break;
case 4:
turn_in_place(36.0f);
break;
case 5:
sound_light_alert();
go_straight(3985);
break;
case 6:
turn_in_place(-40.0f);
break;
case 7: // 开始C到B弧线
sound_light_alert();
go_arc_ccd(3600);
break;
case 8: // D到A弧线完成,任务结束
sound_light_alert();
reset_task_namespace(&task_namespace);
break;
}
task_namespace.finish = 0;
task_namespace.sub_task_stage++;
}
return;
}
// 任务4:重复任务3路径4圈
void task_4(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
task_namespace.sub_task_stage = 0;
task_namespace.finish = 1;
return;
}
if (task_namespace.finish) {
switch (task_namespace.sub_task_stage) {
case 0:
turn_in_place(-30.0f);
break;
case 1:
go_straight(4045);
break;
case 2:
turn_in_place(29.0f);
break;
case 3: // 开始C到B弧线
sound_light_alert();
//go_straight(40);
go_brc_ccd(5000);
break;
case 4:
turn_in_place(34.0f);
break;
case 5:
sound_light_alert();
go_straight(4035);
break;
case 6:
turn_in_place(-33.0f);
break;
case 7: // 开始C到B弧线
sound_light_alert();
go_arc_ccd(5000);
break;
case 8: // D到A弧线完成,任务结束
sound_light_alert();
reset_task_namespace(&task_namespace);
break;
}
task_namespace.finish = 0;
task_namespace.sub_task_stage++;
}
}
void TIMER_0_INST_IRQHandler(void) {
if (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) {
if (DL_TIMER_IIDX_ZERO) {
Get_Velocity_From_Encoder(Get_Encoder_countA, Get_Encoder_countB);
Get_Encoder_countA = Get_Encoder_countB = 0;
MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder);
MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder);
if (!Flag_Stop) {
Set_PWM(-MotorA.Motor_Pwm, -MotorB.Motor_Pwm);
} else {
Set_PWM(0, 0);
}
}
}
}
uint32_t gpio_interrup1, gpio_interrup2;
int64_t B1, B2, B3, B4;
int64_t A1, A2, A3, A4;
void GROUP1_IRQHandler(void) {
// 获取中断信号
gpio_interrup1 = DL_GPIO_getEnabledInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN);
gpio_interrup2 = DL_GPIO_getEnabledInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN);
// encoderB
if ((gpio_interrup1 & ENCODERA_E1A_PIN) == ENCODERA_E1A_PIN) {
if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1B_PIN)) {
right_encoder--;
Get_Encoder_countB--;
} else {
right_encoder++;
Get_Encoder_countB++;
}
} else if ((gpio_interrup1 & ENCODERA_E1B_PIN) == ENCODERA_E1B_PIN) {
if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1A_PIN)) {
right_encoder++;
Get_Encoder_countB++;
} else {
right_encoder--;
Get_Encoder_countB--;
}
}
// encoderA
if ((gpio_interrup2 & ENCODERB_E2A_PIN) == ENCODERB_E2A_PIN) {
if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2B_PIN)) {
left_encoder++;
Get_Encoder_countA--;
} else {
left_encoder--;
Get_Encoder_countA++;
}
} else if ((gpio_interrup2 & ENCODERB_E2B_PIN) == ENCODERB_E2B_PIN) {
if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2A_PIN)) {
left_encoder--;
Get_Encoder_countA++;
} else {
left_encoder++;
Get_Encoder_countA--;
}
}
DL_GPIO_clearInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN);
DL_GPIO_clearInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN);
}
// 直线行驶函数
void go_straight(int dis) {
task_namespace.doing_what = GO_STRAIGHT;
task_namespace.target = left_encoder + dis;
task_namespace.finish = 0;
}
// 原地转向函数
void turn_in_place(float angle) {
task_namespace.doing_what = TURN_IN_PLACE;
task_namespace.start_yaw = Yaw;
task_namespace.target_yaw_diff = angle; // 正值右转,负值左转
task_namespace.finish = 0;
}
// CCD巡线函数(需要外部条件结束)
void go_ccd_line(void) {
task_namespace.doing_what = GO_CCD;
task_namespace.start_encoder = left_encoder;
task_namespace.finish = 0;
// 设置一个安全的最大距离,防止无限巡线
// 可以根据实际场地调整这个值
static uint32_t ccd_end_time = 0;
if (ccd_end_time == 0) {
ccd_end_time = hsu_time_get_ms();
}
// 如果巡线时间超过10秒或距离超过2000mm,强制结束
if (hsu_time_get_ms() - ccd_end_time > 10000 || (left_encoder * 1.f - task_namespace.start_encoder) > 2000) {
task_namespace.finish = 1;
ccd_end_time = 0;
}
}
// 弧线CCD巡线函数
void go_arc_ccd(hsu_time_t time) {
task_namespace.doing_what = GO_CCD;
task_namespace.start_encoder = left_encoder;
task_namespace.ccd_end_time = hsu_time_get_ms() + time;
}
void go_brc_ccd(hsu_time_t time) {
task_namespace.doing_what = GO_CCD;
task_namespace.start_encoder = right_encoder;
task_namespace.ccd_end_time = hsu_time_get_ms() + time;
}
// 声光提示函数
void sound_light_alert(void) {
DL_GPIO_togglePins(LED_PORT, LED_led_PIN);
DL_GPIO_setPins(BUZZY_PORT, BUZZY_PIN_PIN);
uint32_t start_time = hsu_time_get_ms();
while (hsu_time_get_ms() - start_time < 1000) {
// 空循环等待1秒
}
//hsu_time_delay_ms(200);
DL_GPIO_togglePins(LED_PORT, LED_led_PIN);
DL_GPIO_clearPins(BUZZY_PORT, BUZZY_PIN_PIN);
}
// callback
void refresh_oled(void) {
show_task_now();
OLED_ShowString(2, 1, "state:");
if (task_namespace.running_state) {
OLED_ShowString(2, 7, "1"); // 运行中
} else {
OLED_ShowString(2, 7, "0"); // 停止
}
}
uint32_t key_get_tick_ms(void) { return hsu_time_get_ms(); }
void key(void) {
key_event_t event = key_scan();
//uint8_t key_value = key_read_pin(); // 获取按键状态
//S1
switch (event) {
case KEY_EVENT_SINGLE_CLICK:
next_state(&task_namespace);
break;
case KEY_EVENT_DOUBLE_CLICK:
task_namespace.is = 1;
task_namespace.running_state = 1;
task_namespace.is_running = 0; // 重置任务运行标志
break;
}
}
MPU6050.c
#include "MPU6050.h"
#include <stdio.h>
#include "inv_mpu.h"
// #include "IOI2C.h"
// #include "usart.h"
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define MOTION (0)
#define NO_MOTION (1)
#define DEFAULT_MPU_HZ (200)
#define FLASH_SIZE (512)
#define FLASH_MEM_START ((void *)0x1800)
#define q30 1073741824.0f
short gyro[3], accel[3], sensors;
float Roll, Pitch, Yaw;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
static signed char gyro_orientation[9] = {-1, 0, 0, 0, -1, 0, 0, 0, 1};
Imu_t mpu6050 = {0};
Imu_t RegOri_mpu6050 = {0};
// iic转接
#include "bsp_siic.h"
static pIICInterface_t siic = &User_sIICDev;
uint8_t IICwriteBits(uint8_t addr, uint8_t reg, uint8_t bitStart, uint8_t length, uint8_t data) {
uint8_t b;
if (siic->read_reg(addr << 1, reg, &b, 1, 200) == IIC_OK) {
uint8_t mask = (0xFF << (bitStart + 1)) | (0xFF >> ((8 - bitStart) + length - 1));
data <<= (8 - length);
data >>= (7 - bitStart);
b &= mask;
b |= data;
return siic->write_reg(addr << 1, reg, &b, 1, 200);
}
return 1;
}
uint8_t IICwriteBit(uint8_t dev, uint8_t reg, uint8_t bitNum, uint8_t data) {
uint8_t b;
siic->read_reg(dev << 1, reg, &b, 1, 200);
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
return siic->write_reg(dev << 1, reg, &b, 1, 200);
}
uint8_t IICreadBytes(uint8_t dev, uint8_t reg, uint8_t length, uint8_t *data) {
return siic->read_reg(dev << 1, reg, data, length, 200);
}
int i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) {
return siic->read_reg(addr << 1, reg, buf, len, 200);
}
unsigned char I2C_ReadOneByte(unsigned char I2C_Addr, unsigned char addr) {
uint8_t b = 0;
siic->read_reg(I2C_Addr << 1, addr, &b, 1, 200);
return b;
}
static unsigned short inv_row_2_scale(const signed char *row) {
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
static unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) {
unsigned short scalar;
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
static void run_self_test(void) {
int result;
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x7) {
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
// printf("setting bias succesfully ......\r\n");
}
}
uint8_t buffer[14];
int16_t MPU6050_FIFO[6][11];
int16_t Gx_offset = 0, Gy_offset = 0, Gz_offset = 0;
/**************************************************************************
Function: The new ADC data is updated to FIFO array for filtering
Input : ax,ay,az:x,y, z-axis acceleration data;gx,gy,gz:x. Y, z-axis angular acceleration data
Output : none
函数功能:将新的ADC数据更新到 FIFO数组,进行滤波处理
入口参数:ax,ay,az:x,y,z轴加速度数据;gx,gy,gz:x,y,z轴角加速度数据
返回 值:无
**************************************************************************/
void MPU6050_newValues(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz) {
unsigned char i;
int32_t sum = 0;
for (i = 1; i < 10; i++) { // FIFO 操作
MPU6050_FIFO[0][i - 1] = MPU6050_FIFO[0][i];
MPU6050_FIFO[1][i - 1] = MPU6050_FIFO[1][i];
MPU6050_FIFO[2][i - 1] = MPU6050_FIFO[2][i];
MPU6050_FIFO[3][i - 1] = MPU6050_FIFO[3][i];
MPU6050_FIFO[4][i - 1] = MPU6050_FIFO[4][i];
MPU6050_FIFO[5][i - 1] = MPU6050_FIFO[5][i];
}
MPU6050_FIFO[0][9] = ax; // 将新的数据放置到 数据的最后面
MPU6050_FIFO[1][9] = ay;
MPU6050_FIFO[2][9] = az;
MPU6050_FIFO[3][9] = gx;
MPU6050_FIFO[4][9] = gy;
MPU6050_FIFO[5][9] = gz;
sum = 0;
for (i = 0; i < 10; i++) { // 求当前数组的合,再取平均值
sum += MPU6050_FIFO[0][i];
}
MPU6050_FIFO[0][10] = sum / 10;
sum = 0;
for (i = 0; i < 10; i++) {
sum += MPU6050_FIFO[1][i];
}
MPU6050_FIFO[1][10] = sum / 10;
sum = 0;
for (i = 0; i < 10; i++) {
sum += MPU6050_FIFO[2][i];
}
MPU6050_FIFO[2][10] = sum / 10;
sum = 0;
for (i = 0; i < 10; i++) {
sum += MPU6050_FIFO[3][i];
}
MPU6050_FIFO[3][10] = sum / 10;
sum = 0;
for (i = 0; i < 10; i++) {
sum += MPU6050_FIFO[4][i];
}
MPU6050_FIFO[4][10] = sum / 10;
sum = 0;
for (i = 0; i < 10; i++) {
sum += MPU6050_FIFO[5][i];
}
MPU6050_FIFO[5][10] = sum / 10;
}
/**************************************************************************
Function: Setting the clock source of mpu6050
Input : source:Clock source number
Output : none
函数功能:设置 MPU6050 的时钟源
入口参数:source:时钟源编号
返回 值:无
* CLK_SEL | Clock Source
* --------+--------------------------------------
* 0 | Internal oscillator
* 1 | PLL with X Gyro reference
* 2 | PLL with Y Gyro reference
* 3 | PLL with Z Gyro reference
* 4 | PLL with external 32.768kHz reference
* 5 | PLL with external 19.2MHz reference
* 6 | Reserved
* 7 | Stops the clock and keeps the timing generator in reset
**************************************************************************/
void MPU6050_setClockSource(uint8_t source) {
IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}
/** Set full-scale gyroscope range.
* @param range New full-scale gyroscope range value
* @see getFullScaleRange()
* @see MPU6050_GYRO_FS_250
* @see MPU6050_RA_GYRO_CONFIG
* @see MPU6050_GCONFIG_FS_SEL_BIT
* @see MPU6050_GCONFIG_FS_SEL_LENGTH
*/
void MPU6050_setFullScaleGyroRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}
/**************************************************************************
Function: Setting the maximum range of mpu6050 accelerometer
Input : range:Acceleration maximum range number
Output : none
函数功能:设置 MPU6050 加速度计的最大量程
入口参数:range:加速度最大量程编号
返回 值:无
**************************************************************************/
// #define MPU6050_ACCEL_FS_2 0x00 //===最大量程+-2G
// #define MPU6050_ACCEL_FS_4 0x01 //===最大量程+-4G
// #define MPU6050_ACCEL_FS_8 0x02 //===最大量程+-8G
// #define MPU6050_ACCEL_FS_16 0x03 //===最大量程+-16G
void MPU6050_setFullScaleAccelRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}
/**************************************************************************
Function: Set mpu6050 to sleep mode or not
Input : enable:1,sleep;0,work;
Output : none
函数功能:设置 MPU6050 是否进入睡眠模式
入口参数:enable:1,睡觉;0,工作;
返回 值:无
**************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/**************************************************************************
Function: Read identity
Input : none
Output : 0x68
函数功能:读取 MPU6050 WHO_AM_I 标识
入口参数:无
返回 值:0x68
**************************************************************************/
uint8_t MPU6050_getDeviceID(void) {
IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
return buffer[0];
}
/**************************************************************************
Function: Check whether mpu6050 is connected
Input : none
Output : 1:Connected;0:Not connected
函数功能:检测MPU6050 是否已经连接
入口参数:无
返回 值:1:已连接;0:未连接
**************************************************************************/
uint8_t MPU6050_testConnection(void) {
if (MPU6050_getDeviceID() == 0x68) // 0b01101000;
return 1;
else
return 0;
}
/**************************************************************************
Function: Setting whether mpu6050 is the host of aux I2C cable
Input : enable:1,yes;0;not
Output : none
函数功能:设置 MPU6050 是否为AUX I2C线的主机
入口参数:enable:1,是;0:否
返回 值:无
**************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
/**************************************************************************
Function: Setting whether mpu6050 is the host of aux I2C cable
Input : enable:1,yes;0;not
Output : none
函数功能:设置 MPU6050 是否为AUX I2C线的主机
入口参数:enable:1,是;0:否
返回 值:无
**************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
/**************************************************************************
Function: initialization Mpu6050 to enter the available state
Input : none
Output : none
函数功能:初始化 MPU6050 以进入可用状态
入口参数:无
返回 值:无
**************************************************************************/
void MPU6050_initialize(void) {
// 未识别陀螺仪,复位
if (MPU6050_getDeviceID() != 0x68) DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR);
MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); // 设置时钟
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // 陀螺仪量程设置
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // 加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); // 进入工作状态
MPU6050_setI2CMasterModeEnabled(0); // 不让MPU6050 控制AUXI2C
MPU6050_setI2CBypassEnabled(0); // 主控制器的I2C与 MPU6050的AUXI2C 直通关闭
}
/**************************************************************************
Function: Initialization of DMP in mpu6050
Input : none
Output : none
函数功能:MPU6050内置DMP的初始化
入口参数:无
返回 值:无
**************************************************************************/
void DMP_Init(void) {
uint8_t resetflag = 0;
uint8_t temp[1] = {0};
i2cRead(0x68, 0x75, 1, temp);
printf("mpu_set_sensor complete ......\r\n");
if (temp[0] != 0x68) DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR);
if (!mpu_init()) {
if (!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_set_sensor complete ......\r\n");
else
resetflag = 1;
if (!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_configure_fifo complete ......\r\n");
else
resetflag = 1;
if (!mpu_set_sample_rate(DEFAULT_MPU_HZ))
printf("mpu_set_sample_rate complete ......\r\n");
else
resetflag = 1;
if (!dmp_load_motion_driver_firmware())
printf("dmp_load_motion_driver_firmware complete ......\r\n");
else
resetflag = 1;
if (!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
printf("dmp_set_orientation complete ......\r\n");
else
resetflag = 1;
if (!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT |
DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL))
printf("dmp_enable_feature complete ......\r\n");
else
resetflag = 1;
if (!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
printf("dmp_set_fifo_rate complete ......\r\n");
else
resetflag = 1;
run_self_test();
if (!mpu_set_dmp_state(1)) printf("mpu_set_dmp_state complete ......\r\n");
} else {
DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR);
}
if (resetflag) {
mpu6050_i2c_sda_unlock();
DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR);
}
}
/**************************************************************************
Function: Read the attitude information of DMP in mpu6050
Input : none
Output : none
函数功能:读取MPU6050内置DMP的姿态信息
入口参数:无
返回 值:无
**************************************************************************/
void Read_DMP(void) {
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); // 读取DMP数据
if (sensors & INV_WXYZ_QUAT) {
q0 = quat[0] / q30;
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30; // 四元数
Roll = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // 计算出横滚角
Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // 计算出俯仰角
Yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // 计算出偏航角
}
}
/**************************************************************************
Function: Read mpu6050 built-in temperature sensor data
Input : none
Output : Centigrade temperature
函数功能:读取MPU6050内置温度传感器数据
入口参数:无
返回 值:摄氏温度
**************************************************************************/
int Read_Temperature(void) {
float Temp;
Temp = (I2C_ReadOneByte(devAddr, MPU6050_RA_TEMP_OUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_TEMP_OUT_L);
if (Temp > 32768) Temp -= 65536; // 数据类型转换
Temp = (36.53 + Temp / 340) * 10; // 温度放大十倍存放
return (int)Temp;
}
//------------------End of File----------------------------
最新发布