API tools faq
paste
Guest User

Untitled

a guest
Jul 18th, 2022
80
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.30 KB | None | 0 0
raw download clone embed print report
  1. #include <Arduino_Helpers.h>
  2.  
  3. // Basic demo for accelerometer/gyro readings from Adafruit ISM330DHCX
  4.  
  5. #include <Adafruit_ISM330DHCX.h>
  6. #include "SensorFusion.h" //SF
  7. #include <Arduino_Helpers.h>
  8. #include <AH/Math/Quaternion.hpp>
  9. #include <AH/Math/Vector.hpp>
  10.  
  11. // For SPI mode, we need a CS pin
  12. #define LSM_CS 10
  13. // For software-SPI mode we need SCK/MOSI/MISO pins
  14. #define LSM_SCK 13
  15. #define LSM_MISO 12
  16. #define LSM_MOSI 11
  17.  
  18. Adafruit_ISM330DHCX ism330dhcx;
  19. Adafruit_ISM330DHCX ism330dhcx2;
  20.  
  21. SF fusion;
  22. float pitch, roll, yaw;
  23. float *quat;
  24. float deltat;
  25.  
  26. Quaternion quat1;
  27. Quaternion quat2;
  28.  
  29. void setup(void) {
  30. Serial.begin(9600);
  31. while (!Serial)
  32. delay(10); // will pause Zero, Leonardo, etc until serial console opens
  33.  
  34. Serial.println("Adafruit ISM330DHCX test!");
  35.  
  36. if (!ism330dhcx.begin_I2C(0x6A)) {
  37. // if (!ism330dhcx.begin_SPI(LSM_CS)) {
  38. // if (!ism330dhcx.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) {
  39. Serial.println("Failed to find ISM330DHCX 1 chip");
  40. while (1) {
  41. delay(10);
  42. }
  43. }
  44.  
  45. Serial.println("ISM330DHCX 1 Found!");
  46.  
  47. if (!ism330dhcx2.begin_I2C(0x6B)) {
  48. // if (!ism330dhcx.begin_SPI(LSM_CS)) {
  49. // if (!ism330dhcx.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) {
  50. Serial.println("Failed to find ISM330DHCX 2 chip");
  51. while (1) {
  52. delay(10);
  53. }
  54. }
  55.  
  56. Serial.println("ISM330DHCX 2 Found!");
  57.  
  58. // ism330dhcx.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
  59. Serial.print("Accelerometer range set to: ");
  60. switch (ism330dhcx.getAccelRange()) {
  61. case LSM6DS_ACCEL_RANGE_2_G:
  62. Serial.println("+-2G");
  63. break;
  64. case LSM6DS_ACCEL_RANGE_4_G:
  65. Serial.println("+-4G");
  66. break;
  67. case LSM6DS_ACCEL_RANGE_8_G:
  68. Serial.println("+-8G");
  69. break;
  70. case LSM6DS_ACCEL_RANGE_16_G:
  71. Serial.println("+-16G");
  72. break;
  73. }
  74.  
  75. // ism330dhcx.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
  76. Serial.print("Gyro range set to: ");
  77. switch (ism330dhcx.getGyroRange()) {
  78. case LSM6DS_GYRO_RANGE_125_DPS:
  79. Serial.println("125 degrees/s");
  80. break;
  81. case LSM6DS_GYRO_RANGE_250_DPS:
  82. Serial.println("250 degrees/s");
  83. break;
  84. case LSM6DS_GYRO_RANGE_500_DPS:
  85. Serial.println("500 degrees/s");
  86. break;
  87. case LSM6DS_GYRO_RANGE_1000_DPS:
  88. Serial.println("1000 degrees/s");
  89. break;
  90. case LSM6DS_GYRO_RANGE_2000_DPS:
  91. Serial.println("2000 degrees/s");
  92. break;
  93. case ISM330DHCX_GYRO_RANGE_4000_DPS:
  94. Serial.println("4000 degrees/s");
  95. break;
  96. }
  97.  
  98. // ism330dhcx.setAccelDataRate(LSM6DS_RATE_12_5_HZ);
  99. Serial.print("Accelerometer data rate set to: ");
  100. switch (ism330dhcx.getAccelDataRate()) {
  101. case LSM6DS_RATE_SHUTDOWN:
  102. Serial.println("0 Hz");
  103. break;
  104. case LSM6DS_RATE_12_5_HZ:
  105. Serial.println("12.5 Hz");
  106. break;
  107. case LSM6DS_RATE_26_HZ:
  108. Serial.println("26 Hz");
  109. break;
  110. case LSM6DS_RATE_52_HZ:
  111. Serial.println("52 Hz");
  112. break;
  113. case LSM6DS_RATE_104_HZ:
  114. Serial.println("104 Hz");
  115. break;
  116. case LSM6DS_RATE_208_HZ:
  117. Serial.println("208 Hz");
  118. break;
  119. case LSM6DS_RATE_416_HZ:
  120. Serial.println("416 Hz");
  121. break;
  122. case LSM6DS_RATE_833_HZ:
  123. Serial.println("833 Hz");
  124. break;
  125. case LSM6DS_RATE_1_66K_HZ:
  126. Serial.println("1.66 KHz");
  127. break;
  128. case LSM6DS_RATE_3_33K_HZ:
  129. Serial.println("3.33 KHz");
  130. break;
  131. case LSM6DS_RATE_6_66K_HZ:
  132. Serial.println("6.66 KHz");
  133. break;
  134. }
  135.  
  136. // ism330dhcx.setGyroDataRate(LSM6DS_RATE_12_5_HZ);
  137. Serial.print("Gyro data rate set to: ");
  138. switch (ism330dhcx.getGyroDataRate()) {
  139. case LSM6DS_RATE_SHUTDOWN:
  140. Serial.println("0 Hz");
  141. break;
  142. case LSM6DS_RATE_12_5_HZ:
  143. Serial.println("12.5 Hz");
  144. break;
  145. case LSM6DS_RATE_26_HZ:
  146. Serial.println("26 Hz");
  147. break;
  148. case LSM6DS_RATE_52_HZ:
  149. Serial.println("52 Hz");
  150. break;
  151. case LSM6DS_RATE_104_HZ:
  152. Serial.println("104 Hz");
  153. break;
  154. case LSM6DS_RATE_208_HZ:
  155. Serial.println("208 Hz");
  156. break;
  157. case LSM6DS_RATE_416_HZ:
  158. Serial.println("416 Hz");
  159. break;
  160. case LSM6DS_RATE_833_HZ:
  161. Serial.println("833 Hz");
  162. break;
  163. case LSM6DS_RATE_1_66K_HZ:
  164. Serial.println("1.66 KHz");
  165. break;
  166. case LSM6DS_RATE_3_33K_HZ:
  167. Serial.println("3.33 KHz");
  168. break;
  169. case LSM6DS_RATE_6_66K_HZ:
  170. Serial.println("6.66 KHz");
  171. break;
  172. }
  173.  
  174. ism330dhcx.configInt1(false, false, true); // accelerometer DRDY on INT1
  175. ism330dhcx.configInt2(false, true, false); // gyro DRDY on INT2
  176.  
  177. ism330dhcx2.configInt1(false, false, true); // accelerometer DRDY on INT1
  178. ism330dhcx2.configInt2(false, true, false); // gyro DRDY on INT2
  179. }
  180.  
  181. void rotationsFromQuat(Quaternion quat) {
  182. roll = atan2f(quat.w * quat.x + quat.y * quat.z, 0.5f - quat.x * quat.x - quat.y * quat.y);
  183. Serial.print("roll: "); Serial.println(roll * (180 / 3.1459));
  184. pitch = asinf(-2.0f * (quat.x * quat.z - quat.w * quat.y));
  185. Serial.print("pitch: "); Serial.println(pitch * (180 / 3.1459));
  186. yaw = atan2f(quat.x * quat.y + quat.w * quat.z, 0.5f - quat.y * quat.y - quat.z * quat.z);
  187. Serial.print("yaw: "); Serial.println(yaw * (180 / 3.1459));
  188. }
  189.  
  190.  
  191. void sensor1() {
  192. sensors_event_t accel;
  193. sensors_event_t gyro;
  194. sensors_event_t temp;
  195. ism330dhcx.getEvent(&accel, &gyro, &temp);
  196.  
  197. deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
  198. fusion.MahonyUpdate(gyro.gyro.x, gyro.gyro.y, gyro.gyro.z, accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
  199.  
  200.  
  201. /*
  202. Serial.print("Pitch:\t"); Serial.println(pitch);
  203. Serial.print("Roll:\t"); Serial.println(roll);
  204. Serial.print("Yaw:\t"); Serial.println(yaw);
  205. Serial.println();
  206. */
  207.  
  208. quat = fusion.getQuat();
  209. quat1 = Quaternion(quat[0], quat[1], quat[2], quat[3]);
  210.  
  211. Vec3f vec = Vec3f(accel.acceleration.x, accel.acceleration.y, accel.acceleration.z);
  212. Quaternion invertedQuat = quat1.conjugated();
  213.  
  214. Vec3f invertedVec = invertedQuat.rotate(vec);
  215.  
  216. Serial.println("---- 1 ----");
  217. Serial.print("x: "); Serial.println(vec.x);
  218. Serial.print("y: "); Serial.println(vec.y);
  219. Serial.print("z: "); Serial.println(vec.z);
  220.  
  221. Serial.println("rotated");
  222. Serial.print("x: "); Serial.println(invertedVec.x);
  223. Serial.print("y: "); Serial.println(invertedVec.y);
  224. Serial.print("z: "); Serial.println(invertedVec.z);
  225. Serial.println("");
  226. }
  227.  
  228. void sensor2() {
  229. sensors_event_t accel;
  230. sensors_event_t gyro;
  231. sensors_event_t temp;
  232. ism330dhcx2.getEvent(&accel, &gyro, &temp);
  233.  
  234. deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
  235. fusion.MahonyUpdate(gyro.gyro.x, gyro.gyro.y, gyro.gyro.z, accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
  236.  
  237. quat = fusion.getQuat();
  238. quat2 = Quaternion(quat[0], quat[1], quat[2], quat[3]);
  239.  
  240. Vec3f vec = Vec3f(accel.acceleration.x, accel.acceleration.y, accel.acceleration.z);
  241. Vec3f rotatedVector = quat2.conjugated().rotate(vec);
  242. rotatedVector = quat2.rotate(rotatedVector);
  243.  
  244. Serial.println("---- 1 ----");
  245. Serial.print("x: "); Serial.println(vec.x);
  246. Serial.print("y: "); Serial.println(vec.y);
  247. Serial.print("z: "); Serial.println(vec.z);
  248. Serial.println("rotated");
  249. Serial.print("x: "); Serial.println(rotatedVector.x);
  250. Serial.print("y: "); Serial.println(rotatedVector.y);
  251. Serial.print("z: "); Serial.println(rotatedVector.z);
  252. Serial.println("");
  253. }
  254.  
  255. void loop() {
  256. // /* Get a new normalized sensor event */
  257. Serial.println("-----------");
  258.  
  259. sensor1();
  260. //sensor2();
  261.  
  262. delay(25);
  263.  
  264. // // serial plotter friendly format
  265.  
  266. // Serial.print(temp.temperature);
  267. // Serial.print(",");
  268.  
  269. // Serial.print(accel.acceleration.x);
  270. // Serial.print(","); Serial.print(accel.acceleration.y);
  271. // Serial.print(","); Serial.print(accel.acceleration.z);
  272. // Serial.print(",");
  273.  
  274. // Serial.print(gyro.gyro.x);
  275. // Serial.print(","); Serial.print(gyro.gyro.y);
  276. // Serial.print(","); Serial.print(gyro.gyro.z);
  277. // Serial.println();
  278. // delayMicroseconds(10000);
  279. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!