Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.

66 lines
1.6KB

  1. float pitch, roll;
  2. float gx, gy, gz;
  3. uint16_t xc, yc;
  4. uint8_t isTouch;
  5. float ax, ay, az;
  6. int16_t xc_old, yc_old;
  7. void printAngles(){
  8. //test function calls
  9. float gx, gy, gz;
  10. getAccel(ax, ay, az);
  11. Serial.printf("Accel-g's: %f, %f, %f\n", ax, ay, az);
  12. getGyro(gx, gy, gz);
  13. Serial.printf("Gyro-deg/sec: %f, %f, %f\n", gx, gy, gz);
  14. getAngles(pitch, roll);
  15. Serial.printf("Pitch/Roll: %f, %f\n", pitch, roll);
  16. getCoords(xc, yc, isTouch);
  17. }
  18. void getCoords(uint16_t &xc, uint16_t &yc, uint8_t &isTouch){
  19. //uint8_t finger = 0; //only getting finger 1
  20. uint8_t Id = 0;
  21. // Trackpad touch 1: id, active, x, y
  22. xc = ((psAxis[37] & 0x0f) << 8) | psAxis[36];
  23. yc = psAxis[38] << 4 | ((psAxis[37] & 0xf0) >> 4),
  24. isTouch = psAxis[35] >> 7;
  25. if(xc != xc_old || yc != yc_old){
  26. Serial.printf("Touch: %d, %d, %d, %d\n", psAxis[33], isTouch, xc, yc);
  27. xc_old = xc;
  28. yc_old = yc;
  29. }
  30. }
  31. void getAccel( float &ax, float &ay, float &az){
  32. int accelx = (int16_t)(psAxis[20]<<8) | psAxis[19];
  33. int accelz = (int16_t)(psAxis[22]<<8) | psAxis[21];
  34. int accely = (int16_t)(psAxis[24]<<8) | psAxis[23];
  35. ax = (float) accelx/8192;
  36. ay = (float) accely/8192;
  37. az = (float) accelz/8192;
  38. }
  39. void getAngles(float &p, float &r){
  40. getAccel( ax, ay, az);
  41. p = (atan2f(ay, az) + PI) * RAD_TO_DEG;
  42. r = (atan2f(ax, az) + PI) * RAD_TO_DEG;
  43. }
  44. void getGyro(float &gx, float &gy, float &gz){
  45. int gyroy = (int16_t)(psAxis[14]<<8) | psAxis[13];
  46. int gyroz = (int16_t)(psAxis[16]<<8) | psAxis[15];
  47. int gyrox = (int16_t)(psAxis[18]<<8) | psAxis[17];
  48. gx = (float) gyrox * RAD_TO_DEG/1024;
  49. gy = (float) gyroy * RAD_TO_DEG/1024;
  50. gz = (float) gyroz * RAD_TO_DEG/1024;
  51. }