turnDetection(2002).c 2.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102
  1. #include "turnDetection.h"
  2. //--------------------------define
  3. // 定义转弯阈值
  4. #define TURNING_THRESHOLD 1.0 // 以角度为单位,这个值需要根据实际情况调整
  5. // 低通滤波参数
  6. #define ALPHA 0.9 // 这个值需要根据实际情况调整
  7. //--------------------------param
  8. // 初始化姿态角度和角速度
  9. float previousPitch = 0.0; // Pitch(俯仰角)
  10. float previousRoll = 0.0; // Roll(翻滚角)
  11. float previousYaw = 0.0; // Yaw(偏航角)
  12. float previousAngularVelocity = 0.0;
  13. // 转弯标志
  14. uint8_t turnFlag = 0;
  15. // 陀螺仪数据异常标志
  16. uint8_t gyroscopeDataAnomaly = 0; // 1:数据异常 0;数据正常
  17. /**
  18. * @brief 将陀螺仪值转换为角速度值
  19. * @param none
  20. * @note
  21. * @retval angle:角速度值
  22. */
  23. float getAngularVelocity(void)
  24. {
  25. int16_t gyroscopeData[3] = {0};
  26. // // 读取陀螺仪数据
  27. atk_ms6050_get_gyroscope(&gyroscopeData[0], &gyroscopeData[1], &gyroscopeData[2]);
  28. // 计算陀螺仪数据的模长(即角速度)
  29. float gyroMagnitude = sqrt(pow(gyroscopeData[0], 2) + pow(gyroscopeData[1], 2) + pow(gyroscopeData[2], 2));
  30. return gyroMagnitude;
  31. }
  32. // 低通滤波函数
  33. float lowPassFilter(float previousValue, float rawValue) {
  34. return ALPHA * previousValue + (1 - ALPHA) * rawValue;
  35. }
  36. // 判断车辆是否正在转弯的函数
  37. int isTurning(void) {
  38. // 获取当前的 pitch、roll 和 yaw 角度
  39. float roll = 0.0, pitch = 0.0, yaw = 0.0;
  40. // 错误计数
  41. global_par *p_global_par = &s_global_par;
  42. // 获取姿态角度数据
  43. while(atk_ms6050_dmp_get_data(&pitch,&roll,&yaw)!=0){}
  44. printf("pitch:%.1f\n", pitch);
  45. printf("roll:%.1f\n", roll);
  46. printf("yaw:%.1f\n", yaw);
  47. // 故障判断
  48. if((pitch == 0) && (roll == 0) && (yaw == 0)){
  49. p_global_par->gyroDataFaultFlag = 1; // 置位故障标志
  50. return 0;
  51. }else{
  52. p_global_par->gyroDataFaultFlag = 0; // 清零故障标志
  53. }
  54. // 获取当前的角速度
  55. float angularVelocity = getAngularVelocity();
  56. // 应用低通滤波
  57. pitch = lowPassFilter(previousPitch, pitch);
  58. roll = lowPassFilter(previousRoll, roll);
  59. yaw = lowPassFilter(previousYaw, yaw);
  60. angularVelocity = lowPassFilter(previousAngularVelocity, angularVelocity);
  61. // 计算姿态角度的变化速度
  62. float pitchChangeRate = fabs(pitch - previousPitch);
  63. float rollChangeRate = fabs(roll - previousRoll);
  64. float yawChangeRate = fabs(yaw - previousYaw);
  65. printf("pitchChangeRate:%.1f\n", pitchChangeRate);
  66. printf("rollChangeRate:%.1f\n", rollChangeRate);
  67. printf("yawChangeRate:%.1f\n", yawChangeRate);
  68. // 判断车辆是否正在转弯
  69. if (pitchChangeRate > TURNING_THRESHOLD || rollChangeRate > TURNING_THRESHOLD || yawChangeRate > TURNING_THRESHOLD) {
  70. turnFlag = 1; // 表示车辆正在转弯
  71. } else {
  72. turnFlag = 0; // 表示车辆未转弯
  73. }
  74. // 更新上一次的姿态角度和角速度
  75. previousPitch = pitch;
  76. previousRoll = roll;
  77. previousYaw = yaw;
  78. previousAngularVelocity = angularVelocity;
  79. if(turnFlag == 1){
  80. return 1;
  81. }else{
  82. return 0;
  83. }
  84. }