fifo_write0fzc.c

发现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----------------------------

#include "headfile.h" //========ICM20602寄存器地址======================== /******************************************** *复位后所有寄存器地址都为0，除了 *Register 26 CONFIG = 0x80 *Register 107 Power Management 1 = 0x41 *Register 117 WHO_AM_I = 0x12 *********************************************/ //陀螺仪温度补偿 #define ICM20_XG_OFFS_TC_H 0x04 #define ICM20_XG_OFFS_TC_L 0x05 #define ICM20_YG_OFFS_TC_H 0x07 #define ICM20_YG_OFFS_TC_L 0x08 #define ICM20_ZG_OFFS_TC_H 0x0A #define ICM20_ZG_OFFS_TC_L 0x0B //加速度自检输出(出产时设置，用于与用户的自检输出值比较） #define ICM20_SELF_TEST_X_ACCEL 0x0D #define ICM20_SELF_TEST_Y_ACCEL 0x0E #define ICM20_SELF_TEST_Z_ACCEL 0x0F //陀螺仪静态偏移 #define ICM20_XG_OFFS_USRH 0x13 #define ICM20_XG_OFFS_USRL 0x14 #define ICM20_YG_OFFS_USRH 0x15 #define ICM20_YG_OFFS_USRL 0x16 #define ICM20_ZG_OFFS_USRH 0x17 #define ICM20_ZG_OFFS_USRL 0x18 #define ICM20_SMPLRT_DIV 0x19 #define ICM20_CONFIG 0x1A #define ICM20_GYRO_CONFIG 0x1B #define ICM20_ACCEL_CONFIG 0x1C #define ICM20_ACCEL_CONFIG2 0x1D #define ICM20_LP_MODE_CFG 0x1E //运动唤醒加速度阈值 #define ICM20_ACCEL_WOM_X_THR 0x20 #define ICM20_ACCEL_WOM_Y_THR 0x21 #define ICM20_ACCEL_WOM_Z_THR 0x22 #define ICM20_FIFO_EN 0x23 #define ICM20_FSYNC_INT 0x36 #define ICM20_INT_PIN_CFG 0x37 //#define ICM20_INT_ENABLE 0x38 #define ICM20_FIFO_WM_INT_STATUS 0x39 #define ICM20_INT_STATUS 0x3A //加速度输出 #define ICM20_ACCEL_XOUT_H 0x3B #define ICM20_ACCEL_XOUT_L 0x3C #define ICM20_ACCEL_YOUT_H 0x3D #define ICM20_ACCEL_YOUT_L 0x3E #define ICM20_ACCEL_ZOUT_H 0x3F #define ICM20_ACCEL_ZOUT_L 0x40 //温度输出 #define ICM20_TEMP_OUT_H 0x41 #define ICM20_TEMP_OUT_L 0x42 //角速度输出 #define ICM20_GYRO_XOUT_H 0x43 #define ICM20_GYRO_XOUT_L 0x44 #define ICM20_GYRO_YOUT_H 0x45 #define ICM20_GYRO_YOUT_L 0x46 #define ICM20_GYRO_ZOUT_H 0x47 #define ICM20_GYRO_ZOUT_L 0x48 //陀螺仪自检输出 #define ICM20_SELF_TEST_X_GYRO 0x50 #define ICM20_SELF_TEST_Y_GYRO 0x51 #define ICM20_SELF_TEST_Z_GYRO 0x52 #define ICM20_FIFO_WM_TH1 0x60 #define ICM20_FIFO_WM_TH2 0x61 #define ICM20_SIGNAL_PATH_RESET 0x68 #define ICM20_ACCEL_INTEL_CTRL 0x69 #define ICM20_USER_CTRL 0x6A //电源控制 #define ICM20_PWR_MGMT_1 0x6B #define ICM20_PWR_MGMT_2 0x6C #define ICM20_I2C_IF 0x70 #define ICM20_FIFO_COUNTH 0x72 #define ICM20_FIFO_COUNTL 0x73 #define ICM20_FIFO_R_W 0x74 #define ICM20_WHO_AM_I 0x75 //加速度静态偏移 #define ICM20_XA_OFFSET_H 0x77 #define ICM20_XA_OFFSET_L 0x78 #define ICM20_YA_OFFSET_H 0x7A #define ICM20_YA_OFFSET_L 0x7B #define ICM20_ZA_OFFSET_H 0x7D #define ICM20_ZA_OFFSET_L 0x7E //片选 #define CS_ICM_GPIO_Port GPIOA #define CS_ICM_Pin GPIO_PIN_4 //=========================================================== extern SPI_HandleTypeDef hspi2; static float _accel_scale; static float _gyro_scale; static uint8_t tx, rx; static uint8_t tx_buff[14]; //static uint8_t mpu_buff[14]; #define ICM20602_ADDRESS 0xD2 uint8_t icm20602_read_buffer(uint8_t const regAddr, uint8_t *pData, uint8_t len) { HAL_GPIO_WritePin(CS_ICM_GPIO_Port, CS_ICM_Pin,0); tx = regAddr | 0x80; tx_buff[0] = tx; HAL_SPI_TransmitReceive(&hspi2, &tx, &rx, 1, 55); HAL_SPI_TransmitReceive(&hspi2, tx_buff, pData, len, 55); HAL_GPIO_WritePin(CS_ICM_GPIO_Port, CS_ICM_Pin,1); return 0; } uint8_t icm20602_write_reg(uint8_t reg,uint8_t val) { HAL_GPIO_WritePin(CS_ICM_GPIO_Port, CS_ICM_Pin,0); tx = reg & 0x7F; HAL_SPI_TransmitReceive(&hspi2, &tx, &rx, 1, 55); tx = val; HAL_SPI_TransmitReceive(&hspi2, &tx, &rx, 1, 55); HAL_GPIO_WritePin(CS_ICM_GPIO_Port, CS_ICM_Pin,1); return 0; } uint8_t icm20602_read_reg(uint8_t reg) { HAL_GPIO_WritePin(CS_ICM_GPIO_Port, CS_ICM_Pin,0); tx = reg | 0x80; HAL_SPI_TransmitReceive(&hspi2, &tx, &rx, 1, 55); HAL_SPI_TransmitReceive(&hspi2, &tx, &rx, 1, 55);//这个应该是随便发一个数就行 HAL_GPIO_WritePin(CS_ICM_GPIO_Port, CS_ICM_Pin,1);//源代码发送的是ff return rx; } uint8_t icm20602_init() { if(icm20602_write_reg(ICM20_PWR_MGMT_1,0x80)) //复位，复位后位0x41,睡眠模式， { puts("icm_20602 reset fail\r\n"); return 1; } HAL_Delay(50); icm20602_write_reg(ICM20_PWR_MGMT_1,0x01); //关闭睡眠，自动选择时钟 HAL_Delay(50); printf("icm_20602 id=%x\r\n",icm20602_read_reg(ICM20_WHO_AM_I));//读取ID icm20602_write_reg(ICM20_SMPLRT_DIV,0); //分频数=为0+1，数据输出速率为内部采样速率 icm20602_write_reg(ICM20_CONFIG,DLPF_BW_20); //GYRO低通滤波设置 icm20602_write_reg(ICM20_ACCEL_CONFIG2,ACCEL_AVER_4|ACCEL_DLPF_BW_21); //ACCEL低通滤波设置 icm20602_set_accel_fullscale(ICM20_ACCEL_FS_8G); icm20602_set_gyro_fullscale(ICM20_GYRO_FS_2000); HAL_Delay(100); printf("icm20602 init pass\r\n\r\n"); return 0; } uint8_t icm20602_set_gyro_fullscale(uint8_t fs) { switch(fs) { case ICM20_GYRO_FS_250: _gyro_scale = 1.0f/131.068f; //32767/250 break; case ICM20_GYRO_FS_500: _gyro_scale = 1.0f/65.534f; break; case ICM20_GYRO_FS_1000: _gyro_scale = 1.0f/32.767f; break; case ICM20_GYRO_FS_2000: _gyro_scale = 1.0f/16.4f; break; default: fs = ICM20_GYRO_FS_2000; _gyro_scale = 1.0f/16.3835f; break; } return icm20602_write_reg(ICM20_GYRO_CONFIG,fs); } uint8_t icm20602_set_accel_fullscale(uint8_t fs) { switch(fs) { case ICM20_ACCEL_FS_2G: _accel_scale = 1.0f/16348.0f; break; case ICM20_ACCEL_FS_4G: _accel_scale = 1.0f/8192.0f; break; case ICM20_ACCEL_FS_8G: _accel_scale = 1.0f/4096.0f; break; case ICM20_ACCEL_FS_16G: _accel_scale = 1.0f/2048.0f; break; default: fs = ICM20_ACCEL_FS_8G; _accel_scale = 1.0f/4096.0f; break; } return icm20602_write_reg(ICM20_ACCEL_CONFIG,fs); } uint8_t icm20602_get_accel_adc(int16_t *accel) { uint8_t buf[6]; if(icm20602_read_buffer(ICM20_ACCEL_XOUT_H,buf,6))return 1; accel[0] = ((int16_t)buf[0]<<8) + buf[1]; accel[1] = ((int16_t)buf[2]<<8) + buf[3]; accel[2] = ((int16_t)buf[4]<<8) + buf[5]; return 0; } uint8_t icm20602_get_gyro_adc(int16_t *gyro) { uint8_t buf[6]; if(icm20602_read_buffer(ICM20_GYRO_XOUT_H,buf,6))return 1; gyro[0] = (buf[0]<<8) + buf[1]; gyro[1] = (buf[2]<<8) + buf[3]; gyro[2] = (buf[4]<<8) + buf[5]; return 0; } uint8_t icm20602_get_gyro(float *gyro) { int16_t gyro_adc[3]; if(icm20602_get_gyro_adc(gyro_adc))return 1; gyro[0] = _gyro_scale * gyro_adc[0]; gyro[1] = _gyro_scale * gyro_adc[1]; gyro[2] = _gyro_scale * gyro_adc[2]; return 0; } uint8_t icm20602_get_accel(float *accel) { int16_t accel_adc[3]; if(icm20602_get_accel_adc(accel_adc))return 1; accel[0] = _accel_scale * accel_adc[0]; accel[1] = _accel_scale * accel_adc[1]; accel[2] = _accel_scale * accel_adc[2]; return 0; } float icm20602_get_temp() { int16_t temp_adc; uint8_t buf[2]; if(icm20602_read_buffer(ICM20_TEMP_OUT_H,buf,2))return 0.0f; temp_adc = (buf[0]<<8)+buf[1]; return (25.0f + (float)temp_adc/326.8f); } 解释代码

为什么我采不到电压#include “stm32f4xx.h” #include “delay.h” #include “oled.h” #include “stdio.h” #include “stdlib.h” #include “arm_math.h” #include “pid.h” #include “./adc/bsp_adc.h” #include “tim.h” float pid_out; volatile uint8_t adc_data_ready = 0; volatile uint8_t tim_update_flag ; volatile uint32_t last_adc_value = 0; float Vout_actual = 0.0f; float Target = 20.0f; // 目标输出电压 float voltage1; // 全局PID控制器 PID_Controller pid; extern __IO uint16_t ADC_ConvertedValue[RHEOSTAT_NOFCHANEL]; extern uint16_t TIM1_Impluse ;//高级定时器占空比 int main(void) { OLED_Init(); Adc_Init(); TIM_Init(); // 输入20V→输出15V：kp=0.3-0.6, ki=0.05-0.2, kd=0.01-0.05 //输入35V→输出20V：kp=0.2-0.4, ki=0.02-0.1, kd=0.005-0.02 pid.kp = 0.5f; // 从较小值开始调试 pid.ki = 0.1f; pid.kd = 0.01f; pid.max_output = 100.0f; pid.min_output = 0.0f; pid.integral = 0; pid.prev_error = 0; while(1) { if (adc_data_ready) { // 计算电压 voltage1 = last_adc_value * 3.3f*0.000244140625; Vout_actual = voltage1; adc_data_ready = 0; } if (tim_update_flag) { // 使用PID计算 pid_out = pid_control(&pid, Target, Vout_actual); // 安全更新PWM (限制在0-8400) TIM1_Impluse = pid_out * 84; // 0-100% -> 0-8400 TIM1->CCR1 = TIM1_Impluse; // // 重置标志 tim_update_flag = 0; } // a=pid_control (5 , 0.25, 0 ,Target ,Vout_actual); // ADC_Read(); // float six = 6; // char str[40]; // sprintf(str,“Vout_actual = %.3f”,Vout_actual); // OLED_ShowString(WORD_WIDTH0,WORD_HIGH1,(u8 *)str,WORD_SIZE); // OLED_Refresh_Gram(); // delay_us(100); static char display_buffer[2][40]; snprintf(display_buffer[0], 40, “Vout: %.2fV”, Vout_actual); delay_ms(50); snprintf(display_buffer[1], 40, “Duty: %d”, TIM1->CCR1); OLED_ShowString(0, 1, (u8*)display_buffer[0], 12); OLED_ShowString(0, 18, (u8*)display_buffer[1], 12); OLED_Refresh_Gram(); } } #include “stm32f4xx_it.h” #include “oled.h” #include <math.h> #include “./adc/bsp_adc.h” #include “pid.h” extern uint16_t ADC_ConvertedValue[RHEOSTAT_NOFCHANEL]; extern float voltage1; extern float pid_out; extern float Vout_actual; extern uint16_t TIM1_Impluse ;//高级定时器占空比 extern volatile uint8_t adc_data_ready ; extern volatile uint8_t tim_update_flag ; extern volatile uint32_t last_adc_value; void TIM1_UP_TIM10_IRQHandler(void) { if(TIM_GetITStatus(TIM1,TIM_IT_Update) == SET) { // Vout_actual = voltage1; // pid_out = pid_control (5 , 0.25, 0 ,Vout_set ,Vout_actual); // TIM1->CCR1 = pid_out; // tim_update_flag = 1; // 设置标志，表示发生了一次更新中断 TIM_ClearITPendingBit(TIM1, TIM_IT_Update); } } void DMA2_Stream0_IRQHandler(void) { // 处理传输完成中断 if (DMA_GetITStatus(DMA2_Stream0, DMA_IT_TCIF0)) { DMA_ClearITPendingBit(DMA2_Stream0, DMA_IT_TCIF0); last_adc_value = ADC_ConvertedValue[0]; adc_data_ready = 1; } // 处理半传输中断 if (DMA_GetITStatus(DMA2_Stream0, DMA_IT_HTIF0)) { DMA_ClearITPendingBit(DMA2_Stream0, DMA_IT_HTIF0); } // 处理传输错误中断 if (DMA_GetITStatus(DMA2_Stream0, DMA_IT_TEIF0)) { DMA_ClearITPendingBit(DMA2_Stream0, DMA_IT_TEIF0); // 这里可以添加错误处理代码 } } void NMI_Handler(void) { } void HardFault_Handler(void) { /* Go to infinite loop when Hard Fault exception occurs */ while (1) {} } void MemManage_Handler(void) { /* Go to infinite loop when Memory Manage exception occurs / while (1) {} } void BusFault_Handler(void) { / Go to infinite loop when Bus Fault exception occurs / while (1) {} } void UsageFault_Handler(void) { / Go to infinite loop when Usage Fault exception occurs */ while (1) {} } void DebugMon_Handler(void) { } void SVC_Handler(void) { } void PendSV_Handler(void) { } void SysTick_Handler(void) { } #include “tim.h” uint16_t TIM1_Impluse = 4200;//预设占空比 float z = 0; const uint32_t spwm[400] = { 4200,4265,4331,4397,4463,4529,4595,4660,4726,4791,4857,4922,4987,5051,5116,5180, 5244,5308,5371,5434,5497,5560,5622,5684,5746,5807,5868,5928,5988,6047,6106,6165, 6223,6280,6337,6394,6450,6505,6560,6615,6668,6721,6774,6826,6877,6927,6977,7026, 7075,7122,7169,7216,7261,7306,7350,7393,7436,7477,7518,7558,7597,7636,7673,7710, 7746,7781,7815,7848,7880,7911,7942,7971,8000,8027,8054,8080,8105,8128,8151,8173, 8194,8214,8233,8251,8268,8283,8298,8312,8325,8337,8348,8358,8366,8374,8381,8387, 8391,8395,8397,8399,8400,8399,8397,8395,8391,8387,8381,8374,8366,8358,8348,8337, 8325,8312,8298,8283,8268,8251,8233,8214,8194,8173,8151,8128,8105,8080,8054,8027, 8000,7971,7942,7911,7880,7848,7815,7781,7746,7710,7673,7636,7597,7558,7518,7477, 7436,7393,7350,7306,7261,7216,7169,7122,7075,7026,6977,6927,6877,6826,6774,6721, 6668,6615,6560,6505,6450,6394,6337,6280,6223,6165,6106,6047,5988,5928,5868,5807, 5746,5684,5622,5560,5497,5434,5371,5308,5244,5180,5116,5051,4987,4922,4857,4791, 4726,4660,4595,4529,4463,4397,4331,4265,4200,4134,4068,4002,3936,3870,3804,3739, 3673,3608,3542,3477,3412,3348,3283,3219,3155,3091,3028,2965,2902,2839,2777,2715, 2653,2592,2531,2471,2411,2352,2293,2234,2176,2119,2062,2005,1949,1894,1839,1784, 1731,1678,1625,1573,1522,1472,1422,1373,1324,1277,1230,1183,1138,1093,1049,1006, 963,922,881,841,802,763,726,689,653,618,584,551,519,488,457,428, 399,372,345,319,294,271,248,226,205,185,166,148,131,116,101,87, 74,62,51,41,33,25,18,12,8,4,2,0,0,0,2,4, 8,12,18,25,33,41,51,62,74,87,101,116,131,148,166,185, 205,226,248,271,294,319,345,372,399,428,457,488,519,551,584,618, 653,689,726,763,802,841,881,922,963,1006,1049,1093,1138,1183,1230,1277, 1324,1373,1422,1472,1522,1573,1625,1678,1731,1784,1839,1894,1949,2005,2062,2119, 2176,2234,2293,2352,2411,2471,2531,2592,2653,2715,2777,2839,2902,2965,3028,3091, 3155,3219,3283,3348,3412,3477,3542,3608,3673,3739,3804,3870,3936,4002,4068,4134 }; //TIM1的GPIO static void TIM_GPIO_Config(void) { GPIO_InitTypeDef TIM_GPIO_InitStruct; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB, ENABLE);//开钟 /-----------------------------PA8,PA7------------------------------------/ GPIO_PinAFConfig(GPIOA,GPIO_PinSource8,GPIO_AF_TIM1);//引脚复用 主 PA8,PA7 GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM1);//引脚复用 补 TIM_GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; //模拟模式GPIO_Mode_AN/F TIM_GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8; //引脚 TIM_GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; //高速 TIM_GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; //推挽 TIM_GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_DOWN; GPIO_Init(GPIOA, &TIM_GPIO_InitStruct); //写入 TIM_GPIO_InitStruct.GPIO_Pin = GPIO_Pin_7; GPIO_Init(GPIOA, &TIM_GPIO_InitStruct); /-----------------------------------------------------------------------/ /-----------------------------PA9,PB14------------------------------------/ GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_TIM1);//引脚复用 主 GPIO_PinAFConfig(GPIOB,GPIO_PinSource14,GPIO_AF_TIM1);//引脚复用 补 TIM_GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; //模拟模式GPIO_Mode_AN/F TIM_GPIO_InitStruct.GPIO_Pin = GPIO_Pin_9; //引脚 TIM_GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; //高速 TIM_GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; //推挽 TIM_GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_DOWN; GPIO_Init(GPIOA, &TIM_GPIO_InitStruct); //写入 TIM_GPIO_InitStruct.GPIO_Pin = GPIO_Pin_14; GPIO_Init(GPIOB, &TIM_GPIO_InitStruct); /-----------------------------------------------------------------------/ /-----------------------------PA10,PB1------------------------------------/ GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_TIM1);//引脚复用 主 GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM1);//引脚复用 补 TIM_GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; //模拟模式GPIO_Mode_AN/F TIM_GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10; //引脚 TIM_GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; //高速 TIM_GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; //推挽 TIM_GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_DOWN; GPIO_Init(GPIOA, &TIM_GPIO_InitStruct); //写入 TIM_GPIO_InitStruct.GPIO_Pin = GPIO_Pin_1; GPIO_Init(GPIOB, &TIM_GPIO_InitStruct); /-----------------------------------------------------------------------/ // TIM_GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AN; //模拟模式 pa6死刹 // TIM_GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6; //引脚 // TIM_GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; //高速 // TIM_GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; //推挽 // TIM_GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; //浮空 // GPIO_Init(GPIOA, &TIM_GPIO_InitStruct); //写入 } //TIM1 static void TIM_A1_Mode_Config(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; TIM_OCInitTypeDef TIM_OCInitStruct; TIM_BDTRInitTypeDef TIM_BDTRInitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);///使能时钟 //168MHZ->20kHZ 主频/（计数+1）*（预分频系数+1） //168MHz/8 * 1050 = 20khz /-----------------------------基本结构体------------------------------------/ TIM_TimeBaseInitStructure.TIM_Period = (8400-1); //自动重装载值 TIM_TimeBaseInitStructure.TIM_Prescaler=(1-1); //定时器分频 TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式 TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; //1分频 TIM_TimeBaseInitStructure.TIM_RepetitionCounter=0; //不需要重复计数 TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStructure); //初始化TIM /-----------------------------基本结构体------------------------------------/ /-----------------------------输出比较------------------------------------/ TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1; //pwm模式选择 TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable; ///使能输出通道 TIM_OCInitStruct.TIM_OutputNState = TIM_OutputNState_Enable; //使能互补通道 TIM_OCInitStruct.TIM_Pulse = TIM1_Impluse; //预设占空比 TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; //PWM1和2中的CH和CCR之间值的大小（多用pwm1的模式1） TIM_OCInitStruct.TIM_OCNPolarity = TIM_OCNPolarity_High; //当使用了刹车功能时，两路PWM1和2都会被强制禁止，进而输出我们配置的的空闲先状态 TIM_OCInitStruct.TIM_OCIdleState = TIM_OCIdleState_Set; //刹车时输出通道的状态 Set = high TIM_OCInitStruct.TIM_OCNIdleState = TIM_OCNIdleState_Reset; //刹车时互补通道的状态 Reset = low TIM_OC1Init(TIM1, &TIM_OCInitStruct); //使能通道1 TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable); /* 使能通道1重载 */ TIM_OCInitStruct.TIM_Pulse = TIM1_Impluse; TIM_OC2Init(TIM1, &TIM_OCInitStruct); TIM_OC2PreloadConfig(TIM1,TIM_OCPreload_Enable); TIM_OCInitStruct.TIM_Pulse = TIM1_Impluse; TIM_OC3Init(TIM1, &TIM_OCInitStruct); TIM_OC3PreloadConfig(TIM1,TIM_OCPreload_Enable); /-----------------------------输出比较------------------------------------/ /-----------------------------死区刹车------------------------------------/ TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable; //开启死区 TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable; //开启1空闲状态 TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_1; //不同的锁定级别 （看BDTR寄存器） TIM_BDTRInitStructure.TIM_DeadTime = 20; //刹车时间，（看BDTR寄存器中的DTG[7：0]） //11转换成二进制为0000 1011 死区时间看[7；5]位，此处为000 TIM_BDTRInitStructure.TIM_Break = TIM_Break_Enable; //允许刹车 //BKIN 测到低电平 比较信号禁止 TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High; //高极性 TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Enable; //自动输出使能(刹车输入无效) TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure); //写入 /-----------------------------死区刹车------------------------------------/ TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE); //允许定时器更新中断 | TIM_IT_Trigger TIM_Cmd(TIM1,ENABLE); //使能定时器 TIM_CtrlPWMOutputs(TIM1, ENABLE); //主动输出使能 } static void TIM_A1_NVIC_Config(void) { NVIC_InitTypeDef NVIC_InitStructure; /-----------------------------中断------------------------------------/ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //分组 NVIC_InitStructure.NVIC_IRQChannel=TIM1_UP_TIM10_IRQn; //定时器1中断 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2; NVIC_InitStructure.NVIC_IRQChannelSubPriority=0; NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE; //使能中断 NVIC_Init(&NVIC_InitStructure); //写入 /-----------------------------中断------------------------------------/ } void TIM_Init(void) { TIM_A1_NVIC_Config(); TIM_GPIO_Config(); TIM_A1_Mode_Config(); } #include “oled.h” #include “oledfont.h” #include “delay.h” //OLED的显存 //存放格式如下. //[0]0 1 2 3 … 127 //[1]0 1 2 3 … 127 //[2]0 1 2 3 … 127 //[3]0 1 2 3 … 127 //[4]0 1 2 3 … 127 //[5]0 1 2 3 … 127 //[6]0 1 2 3 … 127 //[7]0 1 2 3 … 127 u8 OLED_GRAM[128][8]; #if OLED_MODE==0 //向SSD1106写入一个字节。 //dat:要写入的数据/命令 //cmd:数据/命令标志 0,表示命令;1,表示数据; void OLED_WR_Byte(u8 dat,u8 cmd) { u8 i; OLED_DC=cmd; OLED_CS=0; for(i=0;i<8;i++) { OLED_SCL=0; if(dat&0x80) OLED_SDA=1; else OLED_SDA=0; OLED_SCL=1; dat<<=1; } OLED_CS=1; OLED_DC=1; } #endif #if OLED_MODE==1 //向SSD1106写入一个字节。 //dat:要写入的数据/命令 //cmd:数据/命令标志 0,表示命令;1,表示数据; void OLED_WR_Byte(u8 dat,u8 cmd) { OLED_DATA_OUT(dat); OLED_RST=cmd; OLED_CS=0; OLED_WR=0; OLED_WR=1; OLED_CS=1; OLED_DC=1; } #endif #if OLED_MODE==2 void OLED_WR_Byte(u8 dat,u8 cmd) { } #endif //更新显存到LCD void OLED_Refresh_Gram(void) { u8 i,n; for(i=0;i<8;i++) { OLED_WR_Byte (0xb0+i,OLED_CMD); //设置页地址（0~7） OLED_WR_Byte (0x00,OLED_CMD); //设置显示位置—列低地址 OLED_WR_Byte (0x10,OLED_CMD); //设置显示位置—列高地址 for(n=0;n<128;n++)OLED_WR_Byte(OLED_GRAM[n][i],OLED_DATA); } } void OLED_Set_Pos(unsigned char x, unsigned char y) { OLED_WR_Byte(0xb0+y,OLED_CMD); OLED_WR_Byte(((x&0xf0)>>4)|0x10,OLED_CMD); OLED_WR_Byte((x&0x0f)|0x01,OLED_CMD); } //开启OLED显示 void OLED_Display_On(void) { OLED_WR_Byte(0X8D,OLED_CMD); //SET DCDC命令 OLED_WR_Byte(0X14,OLED_CMD); //DCDC ON OLED_WR_Byte(0XAF,OLED_CMD); //DISPLAY ON } //关闭OLED显示 void OLED_Display_Off(void) { OLED_WR_Byte(0X8D,OLED_CMD); //SET DCDC命令 OLED_WR_Byte(0X10,OLED_CMD); //DCDC OFF OLED_WR_Byte(0XAE,OLED_CMD); //DISPLAY OFF } //清屏函数,清完屏,整个屏幕是黑色的!和没点亮一样!!! void OLED_Clear(void) { u8 i,n; for(i=0;i<8;i++) { for(n=0;n<128;n++) { OLED_GRAM[n][i]=0; } } OLED_Refresh_Gram();//更新显示 } //画点 //x:0~127 //y:0~63 //t:1 填充 0,清空 void OLED_DrawPoint(u8 x,u8 y,u8 t) { u8 pos,bx,temp=0; if(x>127||y>63)return;//超出范围了. pos=7-y/8; bx=y%8; temp=1<<(7-bx); if(t)OLED_GRAM[x][pos]|=temp; else OLED_GRAM[x][pos]&=~temp; } void OLED_DrawLine(u8 x1, u8 y1, u8 x2, u8 y2) { u16 t; int xerr=0,yerr=0,delta_x,delta_y,distance; int incx,incy,uRow,uCol; delta_x=x2-x1; //计算坐标增量 delta_y=y2-y1; uRow=x1; uCol=y1; if(delta_x>0)incx=1; //设置单步方向 else if(delta_x==0)incx=0;//垂直线 else {incx=-1;delta_x=-delta_x;} if(delta_y>0)incy=1; else if(delta_y==0)incy=0;//水平线 else{incy=-1;delta_y=-delta_y;} if( delta_x>delta_y)distance=delta_x; //选取基本增量坐标轴 else distance=delta_y; for(t=0;t<=distance+1;t++ )//画线输出 { OLED_DrawPoint(uRow,uCol,1);//画点 xerr+=delta_x ; yerr+=delta_y ; if(xerr>distance) { xerr-=distance; uRow+=incx; } if(yerr>distance) { yerr-=distance; uCol+=incy; } } } void OLED_DrawRectangle(u8 x1, u8 y1, u8 x2, u8 y2) { OLED_DrawLine(x1,y1,x2,y1); OLED_DrawLine(x1,y1,x1,y2); OLED_DrawLine(x1,y2,x2,y2); OLED_DrawLine(x2,y1,x2,y2); } //x1,y1,x2,y2 填充区域的对角坐标 //确保x1<=x2;y1<=y2 0<=x1<=127 0<=y1<=63 //dot:0,清空;1,填充 void OLED_Fill(u8 x1,u8 y1,u8 x2,u8 y2,u8 dot) { u8 x,y; for(x=x1;x<=x2;x++) { for(y=y1;y<=y2;y++) { OLED_DrawPoint(x,y,dot); } } OLED_Refresh_Gram();//更新显示 } //在指定位置显示一个字符,包括部分字符 //x:0~127 //y:0~63 //mode:0,反白显示;1,正常显示 //size:选择字体 12/16/24 void OLED_ShowChar(u8 x,u8 y,u8 chr,u8 size,u8 mode) { u8 temp,t,t1; u8 y0=y; u8 csize=(size/8+((size%8)?1:0)) * (size/2); //得到字体一个字符对应点阵集所占的字节数 chr=chr-’ ';//得到偏移后的值 for(t=0;t<csize;t++) { if(size12)temp=ascii_1206[chr][t]; //调用1206字体 else if(size16)temp=ascii_1608[chr][t]; //调用1608字体 else if(size==24)temp=ascii_2412[chr][t]; //调用2412字体 else return; //没有的字库 for(t1=0;t1<8;t1++) { if(temp&0x80)OLED_DrawPoint(x,y,mode); else OLED_DrawPoint(x,y,!mode); temp<<=1; y++; if((y-y0)==size) { y=y0; x++; break; } } } } //m^n函数 u32 oled_pow(u8 m,u8 n) { u32 result=1; while(n–)result*=m; return result; } //显示2个数字 //x,y :起点坐标 //len :数字的位数 //size:字体大小 //num:数值(0~4294967295); void OLED_ShowNum(u8 x,u8 y,u32 num,u8 len,u8 size) { u8 t,temp; u8 enshow=0; for(t=0;t<len;t++) { temp=(num/oled_pow(10,len-t-1))%10; if(enshow0&&t<(len-1)) { if(temp0) { OLED_ShowChar(x+(size/2)*t,y,’ ',size,1); continue; }else enshow=1; } OLED_ShowChar(x+(size/2)*t,y,temp+'0',size,1); } } //显示字符串 //x,y:起点坐标 //size:字体大小 //*p:字符串起始地址 void OLED_ShowString(u8 x,u8 y,const u8 *p,u8 size) { while((*p<=‘~’)&&(*p>=’ '))//判断是不是非法字符! { if(x>(128-(size/2))){x=0;y+=size;} if(y>(64-size)){y=x=0;OLED_Clear();} OLED_ShowChar(x,y,*p,size,1); x+=size/2; p++; } } //显示汉字 //x,y:起点坐标 //pos:数组位置汉字显示 //size:字体大小 //mode:0,反白显示;1,正常显示 void OLED_ShowFontHZ(u8 x,u8 y,u8 pos,u8 size,u8 mode) { u8 temp,t,t1; u8 y0=y; u8 csize=(size/8+((size%8)?1:0))*(size);//得到字体一个字符对应点阵集所占的字节数 if(size!=12&&size!=16&&size!=24&&size!=32)return; //不支持的size for(t=0;t<csize;t++) { if(size==12)temp=FontHzk_12[pos][t]; //调用1206字体 else if(size==16)temp=FontHzk_16[pos][t]; //调用1608字体 else if(size==24)temp=FontHzk_24[pos][t]; //调用2412字体 else if(size==32)temp=FontHzk_32[pos][t]; //调用3216字体 else return; //没有的字库 for(t1=0;t1<8;t1++) { if(temp&0x80)OLED_DrawPoint(x,y,mode); else OLED_DrawPoint(x,y,!mode); temp<<=1; y++; if((y-y0)==size) { y=y0; x++; break; } } } } //显示BMP图片128×64 //起始点坐标(x,y),x的范围0～127，y为页的范围0～7 void OLED_DrawBMP(u8 x0, u8 y0,u8 x1, u8 y1,u8 BMP[]) { u16 j=0; u8 x,y; if(y1%8==0)y=y1/8; else y=y1/8+1; for(y=y0;y<y1;y++) { OLED_Set_Pos(x0,y); for(x=x0;x<x1;x++) { OLED_WR_Byte(BMP[j++],OLED_DATA); } } } //GND 接电源地 //VCC 接5V或3.3v电源 //D0 接PD6（SCL） //D1 接PD7（SDA） //RES 接PD4 //DC 接PD5 //CS 接PD3 void OLED_Init() { GPIO_InitTypeDef GPIO_InitStructure; #if OLED_MODE==0 //4线SPI模式 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE|RCC_AHB1Periph_GPIOC|RCC_AHB1Periph_GPIOA,ENABLE); GPIO_InitStructure.GPIO_Mode=GPIO_Mode_OUT; //输出模式 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_2|GPIO_Pin_6;//管脚设置 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz;//速度为100M GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;//推挽输出 GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL;//不拉 GPIO_Init(GPIOE,&GPIO_InitStructure); //初始化结构体 GPIO_SetBits(GPIOE,GPIO_Pin_2|GPIO_Pin_6); //拉高电平 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_OUT; //输出模式 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_13;//管脚设置 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz;//速度为100M GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;//推挽输出 GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL;//不拉 GPIO_Init(GPIOC,&GPIO_InitStructure); //初始化结构体 GPIO_SetBits(GPIOC,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_13); //拉高电平 OLED_Clear(); #endif #if OLED_MODE==1 //8080模式 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD|RCC_AHB1Periph_GPIOC,ENABLE); GPIO_InitStructure.GPIO_Mode=GPIO_Mode_OUT; //输出模式 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7;//管脚设置 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz;//速度为100M GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;//推挽输出 GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;//上拉 GPIO_Init(GPIOD,&GPIO_InitStructure); //初始化结构体 GPIO_SetBits(GPIOD,GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7); GPIO_InitStructure.GPIO_Pin = 0XFF; //PC0-7 GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_SetBits(GPIOC,0xFF); //PC0-7输出高 #endif #if OLED_MODE==2 //IIC模式 #endif OLED_RST=1; delay_ms(100); OLED_RST=0; delay_ms(100); OLED_RST=1; OLED_WR_Byte(0xAE,OLED_CMD); //关闭显示 OLED_WR_Byte(0xD5,OLED_CMD); //设置时钟分频因子,震荡频率 OLED_WR_Byte(80,OLED_CMD); //[3:0],分频因子;[7:4],震荡频率 OLED_WR_Byte(0xA8,OLED_CMD); //设置驱动路数 OLED_WR_Byte(0X3F,OLED_CMD); //默认0X3F(1/64) OLED_WR_Byte(0xD3,OLED_CMD); //设置显示偏移 OLED_WR_Byte(0X00,OLED_CMD); //默认为0 OLED_WR_Byte(0x40,OLED_CMD); //设置显示开始行 [5:0],行数. OLED_WR_Byte(0x8D,OLED_CMD); //电荷泵设置 OLED_WR_Byte(0x14,OLED_CMD); //bit2，开启/关闭 OLED_WR_Byte(0x20,OLED_CMD); //设置内存地址模式 OLED_WR_Byte(0x02,OLED_CMD); //[1:0],00，列地址模式;01，行地址模式;10,页地址模式;默认10; OLED_WR_Byte(0xA1,OLED_CMD); //段重定义设置,bit0:0,0->0;1,0->127; OLED_WR_Byte(0xC0,OLED_CMD); //设置COM扫描方向;bit3:0,普通模式;1,重定义模式 COM[N-1]->COM0;N:驱动路数 OLED_WR_Byte(0xDA,OLED_CMD); //设置COM硬件引脚配置 OLED_WR_Byte(0x12,OLED_CMD); //[5:4]配置 OLED_WR_Byte(0x81,OLED_CMD); //对比度设置 OLED_WR_Byte(0xEF,OLED_CMD); //1~255;默认0X7F (亮度设置,越大越亮) OLED_WR_Byte(0xD9,OLED_CMD); //设置预充电周期 OLED_WR_Byte(0xf1,OLED_CMD); //[3:0],PHASE 1;[7:4],PHASE 2; OLED_WR_Byte(0xDB,OLED_CMD); //设置VCOMH 电压倍率 OLED_WR_Byte(0x30,OLED_CMD); //[6:4] 000,0.65*vcc;001,0.77*vcc;011,0.83*vcc; OLED_WR_Byte(0xA4,OLED_CMD); //全局显示开启;bit0:1,开启;0,关闭;(白屏/黑屏) OLED_WR_Byte(0xA6,OLED_CMD); //设置显示方式;bit0:1,反相显示;0,正常显示 OLED_WR_Byte(0xAF,OLED_CMD); //开启显示 OLED_Clear(); } #ifndef _oled_H #define _oled_H #include “sys/system.h” #include “stdlib.h” #define WORD_SIZE 12 #define X_OFFSET_WORD 0 #define Y_OFFSET_WORD 0 #define X_OFFSTE_PIXEL 0 #define Y_OFFSTE_PIXEL 0 #if WORD_SIZE != 12 && WORD_SIZE != 16 && WORD_SIZE != 24 #define WORD_SIZE 12 #endif #if WORD_SIZE == 24 #define WORD_WIDTH 12 #define WORD_HIGH 24 #endif #if WORD_SIZE == 16 #define WORD_WIDTH 8 #define WORD_HIGH 16 #endif #if WORD_SIZE == 12 #define WORD_WIDTH 6 #define WORD_HIGH 12 #endif //OLED模式设置 //0:4线串行SPI模式 //1:并行8080模式 //2:IIC模式 #define OLED_MODE 0 #define SIZE 16 #define XLevelL 0x00 #define XLevelH 0x10 #define Max_Column 128 #define Max_Row 64 #define Brightness 0xFF #define X_WIDTH 128 #define Y_WIDTH 64 #if OLED_MODE==0 //OLDE-SPI4线控制管脚定义 #define OLED_SCL PCout(1) #define OLED_SDA PCout(0) #define OLED_RST PCout(13) #define OLED_DC PEout(6) #define OLED_CS PEout(2) #endif #if OLED_MODE==1 //OLDE-8080总线控制管脚定义 #define OLED_CS PDout(3) #define OLED_RST PDout(4) #define OLED_DC PDout(5) #define OLED_WR PDout(6) #define OLED_RD PDout(7) #define OLED_DATA_OUT(x) GPIO_Write(GPIOC,x);//输出 #endif #if OLED_MODE==2 //OLDE-IIC总线控制管脚定义 #endif #define OLED_CMD 0 //写命令 #define OLED_DATA 1 //写数据 //OLED控制用函数 void OLED_WR_Byte(u8 dat,u8 cmd); void OLED_Display_On(void); void OLED_Display_Off(void); void OLED_Set_Pos(unsigned char x, unsigned char y); void OLED_Init(void); void OLED_Refresh_Gram(void); void OLED_Clear(void); void OLED_DrawPoint(u8 x,u8 y,u8 t); void OLED_DrawLine(u8 x1, u8 y1, u8 x2, u8 y2); void OLED_DrawRectangle(u8 x1, u8 y1, u8 x2, u8 y2); void OLED_Fill(u8 x1,u8 y1,u8 x2,u8 y2,u8 dot); void OLED_ShowChar(u8 x,u8 y,u8 chr,u8 size,u8 mode); void OLED_ShowNum(u8 x,u8 y,u32 num,u8 len,u8 size); void OLED_ShowString(u8 x,u8 y,const u8 *p,u8 size); void OLED_ShowFontHZ(u8 x,u8 y,u8 pos,u8 size,u8 mode); void OLED_DrawBMP(u8 x0, u8 y0,u8 x1, u8 y1,u8 BMP[]); #endif #include “./adc/bsp_adc.h” __IO uint16_t ADC_ConvertedValue[RHEOSTAT_NOFCHANEL]={0}; static void ADC_GPIO_Config(void) { GPIO_InitTypeDef GPIO_InitStructure; /=通道1==/ // 使能 GPIO 时钟 RCC_AHB1PeriphClockCmd(ADC_GPIO_CLK1,ENABLE); // 配置 IO GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN1; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; //不上拉不下拉 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ; GPIO_Init(ADC_GPIO_PORT1, &GPIO_InitStructure); } void ADC_DMA_Config(void) { DMA_InitTypeDef DMA_InitStructure; // 1. 使能 DMA 时钟 RCC_AHB1PeriphClockCmd(ADC_DMA_CLK, ENABLE); // 2. 配置 DMA 参数 DMA_InitStructure.DMA_Channel = ADC_DMA_CHANNEL; // DMA 通道 0 DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)ADC_ConvertedValue ; // ADC 数据寄存器地址 DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ADC_ConvertedValue; // 内存缓冲区地址 DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; // 外设到内存 DMA_InitStructure.DMA_BufferSize = RHEOSTAT_NOFCHANEL; // 缓冲区大小 DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; // 外设地址不递增 DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; // 内存地址递增 DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; // 外设数据大小：半字(16位) DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; // 内存数据大小：半字(16位) DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; // 循环模式 DMA_InitStructure.DMA_Priority = DMA_Priority_High; // 高优先级 DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; // 禁用 FIFO 模式 DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; // FIFO 阈值 DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; // 内存突发传输：单次 DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; // 外设突发传输：单次 // 3. 初始化 DMA DMA_Init(ADC_DMA_STREAM, &DMA_InitStructure); // 4. 使能 DMA 中断（传输完成、传输错误） DMA_ITConfig(ADC_DMA_STREAM, DMA_IT_TC | DMA_IT_TE | DMA_IT_HT, ENABLE); // 5. 使能 DMA 流 DMA_Cmd(ADC_DMA_STREAM, ENABLE); } void ADC_Config(void) { ADC_InitTypeDef ADC_InitStructure; ADC_CommonInitTypeDef ADC_CommonInitStructure; // 1. 使能 ADC 时钟 RCC_APB2PeriphClockCmd(ADC_CLK, ENABLE); // 2. 配置 ADC 通用参数 ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; // 独立模式 ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4; // ADC 时钟分频：PCLK2/4 ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; // DMA 访问模式 ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; // 采样延迟 ADC_CommonInit(&ADC_CommonInitStructure); // 3. 配置 ADC 参数 ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; // 12位分辨率 ADC_InitStructure.ADC_ScanConvMode = ENABLE; // 扫描模式使能 ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; // 连续转换模式 ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; // 无外部触发 ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; // 外部触发源 ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; // 数据右对齐 ADC_InitStructure.ADC_NbrOfConversion = RHEOSTAT_NOFCHANEL ; // 转换通道数 ADC_Init(ADC_, &ADC_InitStructure); // 4. 配置 ADC 通道（通道4，PA4） ADC_RegularChannelConfig(ADC_, ADC_Channel_4, 1, ADC_SampleTime_84Cycles); // 5. 使能 ADC DMA ADC_DMACmd(ADC_, ENABLE); // 6. 使能 ADC ADC_Cmd(ADC_, ENABLE); // 7. 启动 ADC 转换 ADC_SoftwareStartConv(ADC_); } static void ADC_NVIC_Config(void) { NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = DMA2_Stream0_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); } void Adc_Init(void) { ADC_GPIO_Config(); ADC_DMA_Config(); ADC_Config(); ADC_NVIC_Config(); } #ifndef __BSP_ADC_H #define __BSP_ADC_H #include “stm32f4xx.h” #define RHEOSTAT_NOFCHANEL 1 /=通道1 IO==/ // ADC IO宏定义 #define ADC_GPIO_PORT1 GPIOA #define ADC_GPIO_PIN1 GPIO_Pin_4 #define ADC_GPIO_CLK1 RCC_AHB1Periph_GPIOA #define ADC_CHANNEL1 ADC_Channel_4 // ADC 序号宏定义 #define ADC_ ADC1 #define ADC_CLK RCC_APB2Periph_ADC1 // ADC DR寄存器宏定义，ADC转换后的数字值则存放在这里 #define RHEOSTAT_ADC_DR_ADDR ((u32)ADC1+0x4c) // ADC DMA 通道宏定义，这里我们使用DMA传输 // DMA 配置 #define ADC_DMA_CLK RCC_AHB1Periph_DMA2 #define ADC_DMA_CHANNEL DMA_Channel_0 #define ADC_DMA_STREAM DMA2_Stream0 void Adc_Init(void); #endif /* __BSP_ADC_H */ #include “pid.h” float pid_control(PID_Controller* pid, float setpoint, float input) { // 计算当前误差 float error = setpoint - input; // 比例项 float p_term = pid->kp * error; // 积分项（带抗饱和） pid->integral += error; // 积分限幅 if(pid->integral > pid->max_output) pid->integral = pid->max_output; else if(pid->integral < pid->min_output) pid->integral = pid->min_output; float i_term = pid->ki * pid->integral; // 微分项（标准实现） float d_term = pid->kd * (error - pid->prev_error); // PID输出 float output = p_term + i_term + d_term; // 输出限幅 if(output > pid->max_output) output = pid->max_output; else if(output < pid->min_output) output = pid->min_output; // 更新误差历史 pid->prev_error = error; return output; } #ifndef _PID_H #define _PID_H typedef struct { float kp, ki, kd; float integral; float prev_error; float max_output; float min_output; } PID_Controller; float pid_control(PID_Controller* pid, float setpoint, float input); #endif，这代码哪有问题

#include <zephyr/kernel.h> #include <zephyr/device.h> #include <zephyr/devicetree.h> #include <zephyr/drivers/i2c.h> #include <zephyr/logging/log.h> #include <stdio.h> #include <math.h> #include "GVL.h" #include "data_preprocess.h" #define LOG_MODULE_NAME LSM6DSOWTR_thread LOG_MODULE_REGISTER(LOG_MODULE_NAME); // LSM6DSOWTR 寄存器定义 #define LSM6DSOWTR_REG_WHO_AM_I 0x0F #define LSM6DSOWTR_REG_CTRL1_XL 0x10 #define LSM6DSOWTR_REG_CTRL2_G 0x11 #define LSM6DSOWTR_REG_CTRL3_C 0x12 #define LSM6DSOWTR_REG_OUTX_L_G 0x22 #define LSM6DSOWTR_REG_OUTX_H_G 0x23 #define LSM6DSOWTR_REG_OUTY_L_G 0x24 #define LSM6DSOWTR_REG_OUTY_H_G 0x25 #define LSM6DSOWTR_REG_OUTZ_L_G 0x26 #define LSM6DSOWTR_REG_OUTZ_H_G 0x27 #define LSM6DSOWTR_REG_OUTX_L_XL 0x28 #define LSM6DSOWTR_REG_OUTX_H_XL 0x29 #define LSM6DSOWTR_REG_OUTY_L_XL 0x2A #define LSM6DSOWTR_REG_OUTY_H_XL 0x2B #define LSM6DSOWTR_REG_OUTZ_L_XL 0x2C #define LSM6DSOWTR_REG_OUTZ_H_XL 0x2D // 配置参数 #define LSM6DSOWTR_WHO_AM_I_VAL 0x6C #define LSM6DSOWTR_ODR_XL_104HZ 0x40 // 加速度计 104Hz #define LSM6DSOWTR_FS_XL_2G 0x00 // ±2g 量程 #define LSM6DSOWTR_ODR_G_104HZ 0x40 // 陀螺仪 104Hz #define LSM6DSOWTR_FS_G_245DPS 0x00 // ±245dps 量程 #define LSM6DSOWTR_REG_STATUS_REG 0x1E #define ACCEL_SENSITIVITY_2G 0.000061f #define GYRO_SENSITIVITY_245DPS 0.00875f // 8.75 mdps/LSB // 压力传感器LPS28DFW 寄存器定义 // LPS28DFW 寄存器定义 #define LPS28DFW_WHO_AM_I 0x0F #define LPS28DFW_CTRL_REG1 0x10 #define LPS28DFW_CTRL_REG2 0x11 #define LPS28DFW_CTRL_REG3 0x12 #define LPS28DFW_PRESS_OUT_XL 0x28 #define LPS28DFW_PRESS_OUT_L 0x29 #define LPS28DFW_PRESS_OUT_H 0x2A #define LPS28DFW_TEMP_OUT_L 0x2B #define LPS28DFW_TEMP_OUT_H 0x2C #define LPS28DFW_INTERRUPT_CFG 0x0B #define LPS28DFW_THS_P_L 0x0C #define LPS28DFW_THS_P_H 0x0D // 配置值 #define LPS28DFW_WHO_AM_I_VAL 0xB4 #define LPS28DFW_ODR_50HZ 0x50 // 50Hz输出数据率 #define LPS28DFW_LPF_ENABLE 0x08 // 启用低通滤波器 #define LPS28DFW_BDU_ENABLE 0x08 // 启用块数据更新 #define LPS28DFW_WATER_MODE 0x40 // 水压测量模式 #define SAMPLE_INTERVAL_MS 100 //MS5837压力传感器 #define MS5837_CMD_RESET 0x1E #define MS5837_CMD_CONVERT_D1_OSR1024 0x44 #define MS5837_CMD_CONVERT_D2_OSR1024 0x54 #define MS5837_CMD_ADC_READ 0x00 #define MS5837_PROM_READ_BASE 0xA0 struct ms5837_calib { uint16_t factory; // PROM word 0 (厂商数据) uint16_t c1; // 压力灵敏度 uint16_t c2; // 压力偏移 uint16_t c3; // 温度系数-压力灵敏度 uint16_t c4; // 温度系数-压力偏移 uint16_t c5; // 参考温度 uint16_t c6; // 温度系数 uint16_t crc; // CRC校验 }; // 等待数据就绪 static bool wait_for_data_ready(const struct i2c_dt_spec *dev_spec) { uint8_t status; int retries = 10; while (retries-- > 0) { if (i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_STATUS_REG, &status) == 0) { // 检查加速度计和陀螺仪数据就绪位 (bit0 & bit1) if ((status & 0x03) == 0x03) return true; } k_msleep(1); } return false; } static int init_LPS28DFW_water(const struct i2c_dt_spec *dev_spec) { const int max_retries = 5; uint8_t whoami; int ret; int retries = 0; LOG_INF("Starting LPS28DFW initialization..."); LOG_DBG("Verifying I2C connection to device at 0x%02X", dev_spec->addr); do { ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_WHO_AM_I, &whoami); if (ret == 0 && whoami == LPS28DFW_WHO_AM_I_VAL) break; if (ret != 0) { LOG_WRN("I2C read error: %d (attempt %d)", ret, retries+1); } else { LOG_WRN("Unexpected WHO_AM_I: 0x%02X (expected 0x%02X) attempt %d", whoami, LPS28DFW_WHO_AM_I_VAL, retries+1); } i2c_recover_bus(dev_spec->bus); k_msleep(10); } while (++retries < max_retries); if (retries >= max_retries) { LOG_ERR("Device detection failed after %d attempts", max_retries); return -ENODEV; } uint8_t ctrl_reset=0x04; ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, ctrl_reset); k_msleep(10); if (ret) { LOG_ERR("ctrl_reg2 reset failed: %d", ret); return ret; } uint8_t ctrl_reset_value; ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, &ctrl_reset_value); if (ret == 0) { // 检查关键位 const uint8_t critical_bits_mask_reset = 0x04; if (ctrl_reset_value == critical_bits_mask_reset) { LOG_DBG("CTRL_REG2 reset verified: 0x%02X", ctrl_reset_value); } else{ LOG_WRN("CTRL_REG2 reset failed: read 0x%02X", ctrl_reset_value); } } uint8_t ctrl_reg1=0x50; ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG1, ctrl_reg1); if (ret) { LOG_ERR("ctrl_reg1 set failed: %d", ret); return ret; } uint8_t ctrl_reg2 =0x18; ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, ctrl_reg2); if (ret) { LOG_ERR("ctrl_reg1 set failed: %d", ret); return ret; } uint8_t reg_value1; uint8_t reg_value2; uint8_t reg_value3; ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG1, &reg_value1); if (ret == 0) { // 检查关键位 const uint8_t critical_bits_mask1 = 0x50; if (reg_value1 == critical_bits_mask1) { LOG_DBG("CTRL_REG1 verified: 0x%02X", reg_value1); } else{ LOG_WRN("CTRL_REG1 failed: read 0x%02X", reg_value1); } } ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, &reg_value2); if (ret == 0) { // 检查关键位 const uint8_t critical_bits_mask2 = 0x18; if (reg_value2 == critical_bits_mask2) { LOG_DBG("CTRL_REG2 verified: 0x%02X", reg_value2); } else{ LOG_WRN("CTRL_REG2 failed: read 0x%02X", reg_value2); } } uint8_t ctrl_reg3 =0x01; ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG3, ctrl_reg3); if (ret) { LOG_ERR("ctrl_reg3 set failed: %d", ret); return ret; } ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG3, &reg_value3); if (ret == 0) { // 检查关键位 const uint8_t critical_bits_mask3 = 0x01; if (reg_value3 == critical_bits_mask3) { LOG_DBG("CTRL_REG3 verified: 0x%02X", reg_value3); } else{ LOG_WRN("CTRL_REG3 failed: read 0x%02X", reg_value3); } } LOG_INF("LPS28DFW initialized successfully"); return 0; } // 读取原始压力数据 (24位有符号整数) static int32_t read_raw_pressure(const struct i2c_dt_spec *dev_spec) { uint8_t press_data[3]; int ret; ret = i2c_burst_read_dt(dev_spec, LPS28DFW_PRESS_OUT_XL, press_data, sizeof(press_data)); if (ret != 0) { LOG_ERR("Pressure read failed: %d", ret); return 0; } int32_t raw_pressure = ((int32_t)press_data[2] << 16) | ((int32_t)press_data[1] << 8) | press_data[0]; if (raw_pressure & 0x00800000) { raw_pressure |= 0xFF000000; } return raw_pressure; } // 读取原始温度数据 (16位有符号整数) static int16_t read_raw_temperature(const struct i2c_dt_spec *dev_spec) { uint8_t temp_data[2]; int ret; ret = i2c_burst_read_dt(dev_spec, LPS28DFW_TEMP_OUT_L, temp_data, sizeof(temp_data)); if (ret != 0) { LOG_ERR("Temperature read failed: %d", ret); return 0; } // 组合16位有符号整数 (小端格式) return (int16_t)((temp_data[1] << 8) | temp_data[0]); } // 读取带温度补偿的水压 static double read_compensated_water_pressure(const struct i2c_dt_spec *dev_spec) { // 读取原始压力值 int32_t raw_pressure = read_raw_pressure(dev_spec); // 读取原始温度值 int16_t raw_temp = read_raw_temperature(dev_spec); // 转换为摄氏度 (100 LSB/°C) double temperature = (double)raw_temp / 100.0f; // 转换为hPa double pressure_hPa = (double)raw_pressure / 4096.0f; // 温度补偿因子 (根据数据手册的典型特性) // 实际应用中应根据传感器校准数据调整 double comp_factor = 1.0f + (0.0001f * temperature); // 应用温度补偿 pressure_hPa *= comp_factor; // 转换为米水柱 return pressure_hPa*100.0; } // 读取水深 (单位: 米) static double read_water_depth(const struct i2c_dt_spec *dev_spec) { // 读取补偿后的水压 double pressure_mH2O = read_compensated_water_pressure(dev_spec); // 水深 = 水压值 (忽略大气压影响) return pressure_mH2O; } static double read_compensated_water_depth(const struct i2c_dt_spec *dev_spec, double atmospheric_pressure) { // 读取补偿后的水压（单位：米水柱） //double absolute_pressure_mH2O = read_compensated_water_pressure(dev_spec); double raw_press = read_raw_pressure(dev_spec) / 0.4096f; double raw_temp = read_raw_temperature(dev_spec) / 100.0f; // double absolute_pressure_Pa = read_compensated_water_pressure(dev_spec); // 转换为Pa (1 mH2O = 9806.65 Pa) //double temp_depth = (absolute_pressure_Pa - atmospheric_pressure) / (1000.0 * 9.80665); //double temp_depth = (sensor_data_PWM.pressure/10.0f - atmospheric_pressure/10.0f) / (1000.0 * 9.80665); double temp_depth = (raw_press/10.0f - atmospheric_pressure/10.0f) / (1000.0 * 9.80665); LOG_INF("raw temp:%.2f raw pressure: %.2f atmospheric_pressure: %.2f kPa (%.4f mH2O)", raw_temp,raw_press/10.f,atmospheric_pressure/10.0f, temp_depth); // 水深 = (绝对压力 - 大气压) / (水密度 * 重力加速度) //return (absolute_pressure_Pa - atmospheric_pressure) / (1000.0f * 9.80665f); return (raw_press/10.0f - atmospheric_pressure/10.0f) / (1000.0f * 9.80665f); } // 初始化 LSM6DS0 传感器 static int init_lsm6dsowtr(const struct i2c_dt_spec *dev_spec) { uint8_t whoami; int ret; LOG_INF("Initializing LSM6DSOWTR sensor..."); // 读取 WHO_AM_I 寄存器 ret = i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_WHO_AM_I, &whoami); if (ret != 0) { LOG_ERR("Failed to read WHO_AM_I: %d", ret); return ret; } if (whoami != LSM6DSOWTR_WHO_AM_I_VAL) { LOG_ERR("Unexpected WHO_AM_I: 0x%02X (expected 0x%02X)", whoami, LSM6DSOWTR_WHO_AM_I_VAL); return -ENODEV; } LOG_INF("LSM6DSOWTR detected (WHO_AM_I=0x%02X)", whoami); // 配置加速度计 uint8_t ctrl1_xl = LSM6DSOWTR_ODR_XL_104HZ | LSM6DSOWTR_FS_XL_2G; ret = i2c_reg_write_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL1_XL, ctrl1_xl); if (ret != 0) { LOG_ERR("Failed to write CTRL1_XL: %d", ret); return ret; } // 配置陀螺仪 uint8_t ctrl2_g = LSM6DSOWTR_ODR_G_104HZ | LSM6DSOWTR_FS_G_245DPS; ret = i2c_reg_write_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL2_G, ctrl2_g); if (ret != 0) { LOG_ERR("Failed to write CTRL2_G: %d", ret); return ret; } uint8_t reg_val; ret = i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL1_XL, &reg_val); if (ret != 0 || reg_val != ctrl1_xl) { LOG_ERR("CTRL1_XL verification failed: wrote 0x%02X, read 0x%02X", ctrl1_xl, reg_val); return -EIO; } ret = i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL2_G, &reg_val); if (ret != 0 || reg_val != ctrl2_g) { LOG_ERR("CTRL2_G verification failed: wrote 0x%02X, read 0x%02X", ctrl2_g, reg_val); return -EIO; } // 配置控制寄存器3 ret = i2c_reg_write_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL3_C, 0x04); // BDU=1 (块数据更新) if (ret != 0) { LOG_ERR("Failed to write CTRL3_C: %d", ret); return ret; } LOG_INF("LSM6DSOWTR initialized successfully"); return 0; } // 读取加速度计数据 static void read_accelerometer(const struct i2c_dt_spec *dev_spec, double *x, double *y, double *z) { if (!wait_for_data_ready(dev_spec)) { LOG_WRN("Accel data not ready"); *x = *y = *z = NAN; return; } uint8_t data[6]; int ret = i2c_burst_read_dt(dev_spec, LSM6DSOWTR_REG_OUTX_L_XL, data, sizeof(data)); if (ret != 0) { LOG_ERR("Accelerometer read failed: %d", ret); *x = *y = *z = NAN; return; } // 组合16位数据（小端格式） int16_t raw_x = (int16_t)((data[1] << 8) | data[0]); int16_t raw_y = (int16_t)((data[3] << 8) | data[2]); int16_t raw_z = (int16_t)((data[5] << 8) | data[4]); // 转换为 g (重力加速度) - ±2g 量程，灵敏度 0.061 mg/LSB *x = raw_x * ACCEL_SENSITIVITY_2G; *y = raw_y * ACCEL_SENSITIVITY_2G; *z = raw_z * ACCEL_SENSITIVITY_2G; } // 读取陀螺仪数据 static void read_gyroscope(const struct i2c_dt_spec *dev_spec, double *x, double *y, double *z) { if (!wait_for_data_ready(dev_spec)) { LOG_WRN("Gyro data not ready"); *x = *y = *z = NAN; return; } uint8_t data[6]; int ret = i2c_burst_read_dt(dev_spec, LSM6DSOWTR_REG_OUTX_L_G, data, sizeof(data)); if (ret != 0) { LOG_ERR("Gyroscope read failed: %d", ret); *x = *y = *z = NAN; return; } // 组合16位数据（小端格式） int16_t raw_x = (int16_t)((data[1] << 8) | data[0]); int16_t raw_y = (int16_t)((data[3] << 8) | data[2]); int16_t raw_z = (int16_t)((data[5] << 8) | data[4]); // 转换为 dps (度/秒) - ±245dps 量程，灵敏度 8.75 mdps/LSB *x = raw_x * GYRO_SENSITIVITY_245DPS; *y = raw_y * GYRO_SENSITIVITY_245DPS; *z = raw_z * GYRO_SENSITIVITY_245DPS; } // 将浮点值转换为定点表示（用于存储） static int32_t sensor_value_to_int(double value, int scale_factor) { return (int32_t)(value * scale_factor); } // 主传感器线程 void lsm6dsowtr_thread(void) { LOG_INF("LSM6DSOWTR sensor thread started"); // 获取六轴传感器设备规范 static const struct i2c_dt_spec lsm6dsowtr_dev = I2C_DT_SPEC_GET(DT_NODELABEL(lsm6dsowtr)); if (!device_is_ready(lsm6dsowtr_dev.bus)) { LOG_ERR("I2C bus not ready: %s", lsm6dsowtr_dev.bus->name); return; } // 获取压力传感器设备规范 static const struct i2c_dt_spec pressure_dev = I2C_DT_SPEC_GET(DT_NODELABEL(mysensor)); if (!device_is_ready(pressure_dev.bus)) { LOG_ERR("I2C bus %s is not ready!", pressure_dev.bus->name); return; } // 初始化传感器 int init_result; // 初始化压力传感器 init_result = init_LPS28DFW_water(&pressure_dev); if (init_result != 0) { LOG_ERR("Pressure sensor initialization failed: %d", init_result); } else { LOG_INF("Pressure sensor initialized successfully"); } // struct ms5837_calib calib_data; // int ms5837_init_ret = init_MS5837(&pressure_dev, &calib_data); // if (ms5837_init_ret != 0) { // LOG_ERR("MS5837 initialization failed: %d", ms5837_init_ret); // // 非致命错误，继续运行其他传感器 // } else { // LOG_INF("MS5837 initialized successfully"); // } // 初始化六轴传感器 init_result = init_lsm6dsowtr(&lsm6dsowtr_dev); if (init_result != 0) { LOG_ERR("LSM6DSOWTR initialization failed: %d", init_result); } else { LOG_INF("LSM6DSOWTR initialized successfully"); } LOG_INF("Starting sensor data collection..."); // 主循环变量 int64_t last_sample_time = k_uptime_get(); int64_t last_batch_time = last_sample_time; static int datacount = 0; static double press_a = 0; static double depth_comp = 0; static int depth_count = 0; while (1) { double press = read_raw_pressure(&pressure_dev); double temp = read_raw_temperature(&pressure_dev); double compensated_depth; if(datacount < 20){ press_a = press_a + press/4096.0f; datacount++; // LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount); } else{ press_a = press_a/20.0f; // LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount); press_a = press_a*10000; // sensor_data_PWM.pressure = press_a*10000; compensated_depth = read_compensated_water_depth(&pressure_dev, press_a); if(depth_count < 20){ depth_comp = depth_comp + compensated_depth; depth_count++; // LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount); } else{ depth_comp = depth_comp/20.0f; LOG_INF("depth_comp: %.4f Pa,depth_count:%d", depth_comp,depth_count); depth_count = 0; depth_comp = 0; } datacount = 0; press_a = 0; } // if(depth_count < 20){ // depth_comp = depth_comp + compensated_depth/4096.0f; // datacount++; // // LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount); // } else{ // press_a = press_a/20.0f; // // LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount); // press_a = press_a*10000; // compensated_depth = read_compensated_water_depth(&pressure_dev, press_a); // datacount = 0; // press_a = 0; // } double accel_x, accel_y, accel_z; double gyro_x, gyro_y, gyro_z; read_accelerometer(&lsm6dsowtr_dev, &accel_x, &accel_y, &accel_z); read_gyroscope(&lsm6dsowtr_dev, &gyro_x, &gyro_y, &gyro_z); //float magnitude = calculate_acceleration_magnitude(accel_x, accel_y, accel_z); // LOG_INF("Accel: X=%.3f g, Y=%.3f g, Z=%.3f g", // accel_x, accel_y, accel_z); // LOG_INF("Gyro: X=%.2f dps, Y=%.2f dps, Z=%.2f dps", // gyro_x, gyro_y, gyro_z); // 获取当前时间戳 int64_t current_time = k_uptime_get(); // 准备数据点 // sensor_data_point_t data_point = { // .timestamp = current_time, // .pressure = sensor_value_to_int(press, 1000), // kPa * 1000 = milli-kPa // .temp = sensor_value_to_int(temp, 100), // °C * 100 = centi-°C // .acc_x = sensor_value_to_int(accel_x, 1000), // g * 1000 = milli-g // .acc_y = sensor_value_to_int(accel_y, 1000), // .acc_z = sensor_value_to_int(accel_z, 1000), // .gyro_x = sensor_value_to_int(gyro_x, 1000), // dps * 1000 = milli-dps // .gyro_y = sensor_value_to_int(gyro_y, 1000), // .gyro_z = sensor_value_to_int(gyro_z, 1000) // }; sensor_data_point_t data_point = { .timestamp = current_time, .pressure = sensor_value_to_int(press/4096.0f, 10000), // kPa * 1000 = milli-kPa .temp = sensor_value_to_int(temp, 100), // °C * 100 = centi-°C .acc_x = sensor_value_to_int(accel_x, 1000), // g * 1000 = milli-g .acc_y = sensor_value_to_int(accel_y, 1000), .acc_z = sensor_value_to_int(accel_z, 1000), .gyro_x = sensor_value_to_int(gyro_x, 1000), // dps * 1000 = milli-dps .gyro_y = sensor_value_to_int(gyro_y, 1000), .gyro_z = sensor_value_to_int(gyro_z, 1000) }; // LOG_INF("data filter"); data_point = filter_sensor_data(&data_point); atmospheric_pressure = data_point.pressure; //double compensated_depth = read_compensated_water_depth(&pressure_dev, atmospheric_pressure); // double compensated_depth = read_compensated_water_depth(&pressure_dev, press/4096.0f); // LOG_INF("Compensated Depth: %.4f m",compensated_depth); // // 安全地添加到批量缓冲区 // k_mutex_lock(&data_mutex, K_FOREVER); // // 检查当前批次缓冲区是否有效 // if (sensor_batch.count == 0) { // // 新批次，记录起始时间戳 // sensor_batch.start_timestamp = current_time; // } // // 添加数据点到批次 // if (sensor_batch.count < BATCH_SIZE) { // sensor_batch.data[sensor_batch.count] = data_point; // sensor_batch.count++; // // 检查批次是否已满 // if (sensor_batch.count >= BATCH_SIZE) { // // 批次已满，标记为就绪 // sensor_batch.ready = true; // LOG_DBG("Batch filled (%d points)", BATCH_SIZE); // } // } else { // LOG_WRN("Batch buffer full, discarding data point"); // } // // 检查是否超时（即使批次未满） // if ((current_time - sensor_batch.start_timestamp) >= (MAX_BATCH_TIMEOUT * 1000)) { // if (sensor_batch.count > 0) { // sensor_batch.ready = true; // LOG_DBG("Batch timeout with %d points", sensor_batch.count); // } // } // // 如果批次就绪，通知BLE线程 // if (sensor_batch.ready) { // k_sem_give(&batch_ready_sem); // LOG_DBG("Notified BLE thread of data ready"); // } // k_mutex_unlock(&data_mutex); // 计算下一个采样点的时间 int64_t elapsed = k_uptime_get() - last_sample_time; int32_t sleep_time = SAMPLE_INTERVAL_MS - elapsed; if (sleep_time > 0) { k_msleep(sleep_time); } else { LOG_WRN("Sampling behind schedule by %d ms", -sleep_time); } last_sample_time = k_uptime_get(); } } // 定义传感器线程 K_THREAD_DEFINE(lsm6dsowtr_thread_id, 4096, lsm6dsowtr_thread, NULL, NULL, NULL, 7, 0, 0);对原始压力值进行中值滤波