Advertisement
Guest User

NodeMCU_I2C_BNO055

a guest
Mar 15th, 2020
581
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 8.06 KB | None | 0 0
  1. #include <Wire.h>
  2.  
  3. #define BNO055_ADDR      (0x28)
  4. #define BNO055_CHIP_ID          0x00
  5. #define BNO055_CHIP_ID_VALUE    0xa0
  6. #define BNO055_AXIS_MAP_CONFIG  0x41
  7. #define BNO055_OPR_MODE         0x3d
  8. #define CONFIGMODE              0x00
  9. #define MODE_NDOF               0x0c
  10. #define ACCEL_OFFSET_X_LSB      0x55
  11. #define ACCEL_OFFSET_X_MSB      0x56
  12. #define ACCEL_OFFSET_Y_LSB      0x57
  13. #define ACCEL_OFFSET_Y_MSB      0x58
  14. #define ACCEL_OFFSET_Z_LSB      0x59
  15. #define ACCEL_OFFSET_Z_MSB      0x5a
  16. #define MAG_OFFSET_X_LSB        0x5b
  17. #define MAG_OFFSET_X_MSB        0x5c
  18. #define MAG_OFFSET_Y_LSB        0x5d
  19. #define MAG_OFFSET_Y_MSB        0x5e
  20. #define MAG_OFFSET_Z_LSB        0x5f
  21. #define MAG_OFFSET_Z_MSB        0x60
  22. #define GYRO_OFFSET_X_LSB       0x61
  23. #define GYRO_OFFSET_X_MSB       0x62
  24. #define GYRO_OFFSET_Y_LSB       0x63
  25. #define GYRO_OFFSET_Y_MSB       0x64
  26. #define GYRO_OFFSET_Z_LSB       0x65
  27. #define GYRO_OFFSET_Z_MSB       0x66
  28. #define ACCEL_RADIUS_LSB        0x67
  29. #define ACCEL_RADIUS_MSB        0x68
  30. #define MAG_RADIUS_LSB          0x69
  31. #define MAG_RADIUS_MSB          0x6a
  32. #define BNO055_EULER_H_LSB      0x1a
  33. #define BNO055_EULER_H_MSB      0x1b
  34. #define BNO055_EULER_R_LSB      0x1c
  35. #define BNO055_EULER_R_MSB      0x1d
  36. #define BNO055_EULER_P_LSB      0x1e
  37. #define BNO055_EULER_P_MSB      0x1f
  38.  
  39. void I2C_BNO055_INIT()
  40. {
  41.   char data[7];
  42.   char chip_id;
  43.    
  44.   char accel_offset_x_lsb_value = 8;
  45.   char accel_offset_x_msb_value = 0;
  46.   char accel_offset_y_lsb_value = 34;
  47.   char accel_offset_y_msb_value = 0;
  48.   char accel_offset_z_lsb_value = 7;
  49.   char accel_offset_z_msb_value = 0;
  50.  
  51.   char mag_offset_x_lsb_value = 45;
  52.   char mag_offset_x_msb_value = 255;
  53.   char mag_offset_y_lsb_value = 116;
  54.   char mag_offset_y_msb_value = 0;
  55.   char mag_offset_z_lsb_value = 90;
  56.   char mag_offset_z_msb_value = 1;
  57.  
  58.   char gyro_offset_x_lsb_value = 1;
  59.   char gyro_offset_x_msb_value = 0;
  60.   char gyro_offset_y_lsb_value = 1;
  61.   char gyro_offset_y_msb_value = 0;
  62.   char gyro_offset_z_lsb_value = 0;
  63.   char gyro_offset_z_msb_value = 0;
  64.  
  65.   char accel_radius_lsb_value = 0;
  66.   char accel_radius_msb_value = 3;
  67.   char mag_radius_lsb_value = 66;
  68.   char mag_radius_msb_value = 2;
  69.    
  70.   data[0] = BNO055_CHIP_ID;
  71.  
  72.   Wire.beginTransmission(BNO055_ADDR);
  73.   Wire.write(data, 1);
  74.   Wire.endTransmission(false);
  75.  
  76.   Wire.requestFrom(BNO055_ADDR, 7, true);
  77.   Serial.println(Wire.available());
  78.   data[0] = Wire.read();
  79.   data[1] = Wire.read();
  80.   data[2] = Wire.read();
  81.   data[3] = Wire.read();
  82.   data[4] = Wire.read();
  83.   data[5] = Wire.read();
  84.   data[6] = Wire.read();
  85.  
  86.   chip_id = data[0];
  87.  
  88.   while (chip_id != BNO055_CHIP_ID_VALUE)
  89.   {
  90.     data[0] = BNO055_CHIP_ID;
  91.  
  92.     Wire.beginTransmission(BNO055_ADDR);
  93.     Wire.write(data, 1);
  94.     Wire.endTransmission(false);
  95.  
  96.    
  97.     Wire.requestFrom(BNO055_ADDR, 7, true);
  98.        
  99.     Serial.println(Wire.available());
  100.     data[0] = Wire.read();
  101.     data[1] = Wire.read();
  102.     data[2] = Wire.read();
  103.     data[3] = Wire.read();
  104.     data[4] = Wire.read();
  105.     data[5] = Wire.read();
  106.     data[6] = Wire.read();
  107.  
  108.     chip_id = data[0];
  109.     yield();
  110.   }
  111.  
  112.   data[0] = BNO055_OPR_MODE;
  113.   data[1] = CONFIGMODE;
  114.  
  115.   Wire.beginTransmission(BNO055_ADDR);
  116.   Wire.write(data,2);
  117.   Wire.endTransmission(true);
  118.  
  119.   delay(50);
  120.  
  121.   data[0] = BNO055_AXIS_MAP_CONFIG;
  122.   data[1] = 0x24;
  123.   data[2] = 0x00;
  124.  
  125.   Wire.beginTransmission(BNO055_ADDR);
  126.   Wire.write(data,2);
  127.   Wire.endTransmission(true);
  128.    
  129.   data[0] = ACCEL_OFFSET_X_LSB; data[1] = accel_offset_x_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  130.   data[0] = ACCEL_OFFSET_X_MSB; data[1] = accel_offset_x_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  131.   data[0] = ACCEL_OFFSET_Y_LSB; data[1] = accel_offset_y_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  132.   data[0] = ACCEL_OFFSET_Y_LSB; data[1] = accel_offset_y_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  133.   data[0] = ACCEL_OFFSET_Z_LSB; data[1] = accel_offset_z_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  134.   data[0] = ACCEL_OFFSET_Z_LSB; data[1] = accel_offset_z_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  135.    
  136.   data[0] = MAG_OFFSET_X_LSB; data[1] = mag_offset_x_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  137.   data[0] = MAG_OFFSET_X_MSB; data[1] = mag_offset_x_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  138.   data[0] = MAG_OFFSET_Y_LSB; data[1] = mag_offset_y_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  139.   data[0] = MAG_OFFSET_Y_LSB; data[1] = mag_offset_y_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  140.   data[0] = MAG_OFFSET_Z_LSB; data[1] = mag_offset_z_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  141.   data[0] = MAG_OFFSET_Z_LSB; data[1] = mag_offset_z_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  142.    
  143.   data[0] = GYRO_OFFSET_X_LSB; data[1] = gyro_offset_x_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  144.   data[0] = GYRO_OFFSET_X_MSB; data[1] = gyro_offset_x_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  145.   data[0] = GYRO_OFFSET_Y_LSB; data[1] = gyro_offset_y_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  146.   data[0] = GYRO_OFFSET_Y_LSB; data[1] = gyro_offset_y_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  147.   data[0] = GYRO_OFFSET_Z_LSB; data[1] = gyro_offset_z_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  148.   data[0] = GYRO_OFFSET_Z_LSB; data[1] = gyro_offset_z_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  149.    
  150.   data[0] = ACCEL_RADIUS_LSB; data[1] = accel_radius_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  151.   data[0] = ACCEL_RADIUS_MSB; data[1] = accel_radius_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  152.   data[0] = MAG_RADIUS_LSB; data[1] = mag_radius_lsb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  153.   data[0] = MAG_RADIUS_MSB; data[1] = mag_radius_msb_value; Wire.beginTransmission(BNO055_ADDR); Wire.write(data,2); Wire.endTransmission(true); delay(20);
  154.    
  155.   data[0] = BNO055_OPR_MODE;
  156.   data[1] = MODE_NDOF;
  157.  
  158.   Wire.beginTransmission(BNO055_ADDR);
  159.   Wire.write(data,2);
  160.   Wire.endTransmission(true);
  161.  
  162.   delay(10);
  163. }
  164.  
  165. int I2C_BNO055_READ_YAW()
  166. {
  167.   int16_t wert;
  168.   char data[6];
  169.  
  170.   data[0] = BNO055_EULER_H_LSB;
  171.  
  172.   Wire.beginTransmission(BNO055_ADDR);
  173.   Wire.write(data, 1);
  174.   Wire.endTransmission(false);
  175.  
  176.   Wire.requestFrom(BNO055_ADDR, 2, true);
  177.   data[0] = ((Wire.available() > 0) ? Wire.read() : 0);
  178.   data[1] = ((Wire.available() > 0) ? Wire.read() : 0);
  179.   data[2] = ((Wire.available() > 0) ? Wire.read() : 0);
  180.   data[3] = ((Wire.available() > 0) ? Wire.read() : 0);
  181.   data[4] = ((Wire.available() > 0) ? Wire.read() : 0);
  182.   data[5] = ((Wire.available() > 0) ? Wire.read() : 0);
  183.  
  184.   wert = data[1] << 8 | data[0];
  185.   wert = (double)wert/16;
  186.   return wert;
  187. }
  188.  
  189. void setup() {
  190.   Serial.begin(9600); /* begin serial for debug */
  191.   Wire.begin(D1, D2); /* join i2c bus with SDA=D1 and SCL=D2 of NodeMCU */
  192.   I2C_BNO055_INIT();
  193. }
  194.  
  195. void loop() {
  196.   Serial.print("YAW:");
  197.   Serial.println(I2C_BNO055_READ_YAW());
  198.  
  199.   delay(200);
  200. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement