API tools faq
paste
RichieHard

Untitled

RichieHard
Mar 23rd, 2020
84
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 27.29 KB | None | 0 0
raw download clone embed print report
  1. /*********
  2. RHS2D made by Riccardo Arciulo For Rainbow team @IRISA Rennes
  3. Nov 2019 --> On
  4. *********/
  5.  
  6.  
  7. #include <BLEDevice.h>
  8. #include <BLEServer.h>
  9. #include <BLEUtils.h>
  10. #include <BLE2902.h>
  11. #include "quaternionFilters.h"
  12. #include "MPU9250.h"
  13. #include <Arduino.h>
  14. #include <Sparkfun_DRV2605L.h> //SparkFun Haptic Motor Driver Library
  15. #include <Wire.h> //I2C library
  16.  
  17. //=================== MPU9250 ACC+GYRO+MAG ==========================
  18.  
  19. #define AHRS false // Set to false for basic data read
  20. #define SerialDebug true // Set to true to get Serial output for debugging
  21.  
  22. #define I2Cclock 400000
  23. #define I2Cport Wire1
  24. #define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using
  25. //#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1
  26.  
  27. MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);
  28.  
  29. #define SDA_1 21
  30. #define SCL_1 22
  31.  
  32. #define SDA_2 5
  33. #define SCL_2 17
  34.  
  35. //================ VIBRO ======================================
  36.  
  37. SFE_HMD_DRV2605L HMD; //Create haptic motor driver object
  38.  
  39. //=================== MULTITREAD ==============================
  40.  
  41. TaskHandle_t Task1;
  42. TaskHandle_t Task2;
  43.  
  44. //*********************** BLE *********************************************************************
  45.  
  46. BLECharacteristic *_pCharacteristic;
  47. bool deviceConnected = false;
  48. float txValue = 0;
  49.  
  50. std::string rxValue; // Could also make this a global var to access it in loop()
  51.  
  52. // See the following for generating UUIDs:
  53. // https://www.uuidgenerator.net/
  54.  
  55. #define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
  56. #define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
  57. #define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
  58.  
  59. class MyServerCallbacks: public BLEServerCallbacks {
  60. void onConnect(BLEServer* pServer) {
  61. deviceConnected = true;
  62.  
  63. //evolve();
  64.  
  65. };
  66.  
  67. void onDisconnect(BLEServer* pServer) {
  68. deviceConnected = false;
  69.  
  70. //dissolve();
  71.  
  72. }
  73. };
  74.  
  75. // pCharacteristic->setValue(&txValue, 1); // To send the integer value
  76. // pCharacteristic->setValue("Hello!"); // Sending a test message
  77. // pCharacteristic->setValue(txString);
  78.  
  79. //pCharacteristic->notify(); // Send the value to the app!
  80. // Serial.print("*** Sent Value: ");
  81. //Serial.print(txString);
  82. //Serial.println(" ***");
  83.  
  84. // You can add the rxValue checks down here instead
  85. // if you set "rxValue" as a global var at the top!
  86. // Note you will have to delete "std::string" declaration
  87. // of "rxValue" in the callback function.
  88.  
  89. /*if (rxValue.find("A") != -1) {
  90. Serial.println("Turning ON!");
  91. pCharacteristic->setValue("HIGH"); // Sending a test message
  92. pCharacteristic->notify(); // Send the value to the app!
  93. }
  94.  
  95. else if (rxValue.find("B") != -1) {
  96. Serial.println("Turning OFF!");
  97. pCharacteristic->setValue("LOW"); // Sending a test message
  98. pCharacteristic->notify(); // Send the value to the app!
  99. }
  100.  
  101. pCharacteristic->notify(); // Send the value to the app!
  102. }*/
  103.  
  104.  
  105. //*************ble
  106.  
  107. /* QUI E' DOVE RICEVI */
  108. class MyCallbacks: public BLECharacteristicCallbacks {
  109. void onWrite(BLECharacteristic *pCharacteristic) {
  110. std::string rxValue = pCharacteristic->getValue();
  111.  
  112. if (rxValue.length() > 0) {
  113. //Serial.println("*********");
  114. //Serial.print("Received Value: ");
  115.  
  116. for (int i = 0; i < rxValue.length(); i++) {
  117. //Serial.print(rxValue[i]);
  118.  
  119. }
  120.  
  121. Serial.println();
  122.  
  123. // Do stuff based on the command received from the app
  124. if (rxValue.find("<CMD:0xA0,0x01>") != -1) {
  125.  
  126. // digitalWrite(2, HIGH); // sets the digital pin 2 on
  127. // delay(500); // waits for 500 sec
  128. // digitalWrite(2, LOW); // sets the digital pin 2 off
  129. // delay(500); // waits for 500 sec
  130. // digitalWrite(2, HIGH); // sets the digital pin 2 on
  131. // delay(500); // waits for 500 sec
  132. // digitalWrite(2, LOW); // sets the digital pin 2 off
  133. // delay(500); // waits for 500 sec
  134.  
  135.  
  136. _pCharacteristic->setValue("<ACK:0xA0,0x11>"); // done
  137. _pCharacteristic->notify(); // Send the value to the app!
  138. }
  139.  
  140. else if (rxValue.find("<CMD:0xA1,0x01>") != -1) {
  141.  
  142. // digitalWrite(15, HIGH); // sets the digital pin 2 on
  143. // delay(500); // waits for 500 sec
  144. // digitalWrite(15, LOW); // sets the digital pin 2 off
  145. // delay(500); // waits for 500 sec
  146. // digitalWrite(15, HIGH); // sets the digital pin 2 on
  147. // delay(500); // waits for 500 sec
  148. // digitalWrite(15, LOW); // sets the digital pin 2 off
  149. // delay(500); // waits for 500 sec
  150.  
  151. _pCharacteristic->setValue("<ACK:0xA1,0x11>"); // done
  152. _pCharacteristic->notify(); // Send the value to the app!
  153.  
  154.  
  155. }
  156.  
  157. else if (rxValue.find("<CMD:0xA2,0x01>") != -1) {
  158.  
  159. // vibro=1;
  160. _pCharacteristic->setValue("<ACK:0xA2,0x11>"); // done
  161. _pCharacteristic->notify(); // Send the value to the app!
  162. }
  163.  
  164. else if (rxValue.find("<CMD:0xA2,0x02>") != -1) {
  165.  
  166. //vibro=0;
  167. _pCharacteristic->setValue("<ACK:0xA2,0x22>"); // done
  168. _pCharacteristic->notify(); // Send the value to the app!
  169. }
  170.  
  171. else if (rxValue.find("<CMD:0xA3,0x01>") != -1) {
  172.  
  173. // PulsingGreen();
  174.  
  175. _pCharacteristic->setValue("<ACK:0xA3,0x11>"); // done
  176. _pCharacteristic->notify(); // Send the value to the app!
  177.  
  178. // evolve();
  179.  
  180. }
  181.  
  182. else if (rxValue.find("<CMD:0xA3,0x02>") != -1) {
  183.  
  184. // PulsingRed();
  185.  
  186. _pCharacteristic->setValue("<ACK:0xA3,0x22>"); // done
  187. _pCharacteristic->notify(); // Send the value to the app!
  188.  
  189. // evolve();
  190.  
  191. }
  192. else if (rxValue.find("<CMD:0xA3,0x03>") != -1) {
  193.  
  194. //PulsingBlue();
  195.  
  196. _pCharacteristic->setValue("<ACK:0xA3,0x33>"); // done
  197. _pCharacteristic->notify(); // Send the value to the app!
  198.  
  199. //evolve();
  200.  
  201. }
  202. else if (rxValue.find("<CMD:0xA4,0x01>") != -1) {
  203.  
  204. //servoON=1;
  205. // ledcWriteTone(0,0); //stop
  206. // myservo.attach(19); // Attach the servo after it has been detatched
  207. //
  208. // myservo.write (0);
  209. // delay (500);
  210. // myservo.write (30);
  211. // delay (500);
  212. // myservo.write (60);
  213. // delay (500);
  214. // myservo.write (90);
  215. // delay (500);
  216. // myservo.write (120);
  217. // delay (500);
  218. // myservo.write (150);
  219. // delay (500);
  220. //
  221. // myservo.write (180);
  222. // delay (500);
  223. //
  224. // myservo.write (150);
  225. // delay (500);
  226. // myservo.write (120);
  227. // delay (500);
  228. // myservo.write (90);
  229. // delay (500);
  230. // myservo.write (60);
  231. // delay (500);
  232. // myservo.write (30);
  233. // delay (500);
  234. // myservo.write (0);
  235. // delay (500);
  236. //
  237. // myservo.detach(); // Turn the servo off for a while
  238. // ledcWriteTone(0,0); //stop
  239. _pCharacteristic->setValue("<ACK:0xA4,0x11>"); // done
  240. _pCharacteristic->notify(); // Send the value to the app!
  241.  
  242. }
  243. else if (rxValue.find("<CMD:0xA4,0x02>") != -1) {
  244.  
  245. //servoON=0;
  246. _pCharacteristic->setValue("<ACK:0xA4,0x22>"); // done
  247. _pCharacteristic->notify(); // Send the value to the app!
  248.  
  249. }
  250. else if (rxValue.find("<CMD:0xA5,0x01>") != -1) {
  251.  
  252. //*************if transducer is on ********************************************************
  253. // ledcWriteTone(0,800);
  254. // delay(1000);
  255. // uint8_t octave = 1;
  256. // ledcWriteNote(0,NOTE_C,octave);
  257. // delay(1000);
  258. // ledcWriteTone(0,0); //stop
  259.  
  260. _pCharacteristic->setValue("<ACK:0xA5,0x11>"); // done
  261. _pCharacteristic->notify(); // Send the value to the app!
  262.  
  263. }
  264. else if (rxValue.find("<CMD:0xA5,0x02>") != -1) {
  265.  
  266.  
  267. _pCharacteristic->setValue("<ACK:0xA5,0x22>"); // done
  268. _pCharacteristic->notify(); // Send the value to the app!
  269.  
  270. }
  271. else if (rxValue.find("<CMD:0xA6,0x01>") != -1) {
  272.  
  273. // none
  274.  
  275. _pCharacteristic->setValue("<ACK:0xA6,0x11>"); // done
  276. _pCharacteristic->notify(); // Send the value to the app!
  277.  
  278. }
  279. else if (rxValue.find("<CMD:0xA7,0x01>") != -1) {
  280.  
  281. //pilot = 1;
  282.  
  283. }
  284. else if (rxValue.find("<CMD:0xA7,0x02>") != -1) {
  285.  
  286. // pilot = 0;
  287.  
  288. _pCharacteristic->setValue("<ACK:0xA7,0x22>"); // done
  289. _pCharacteristic->notify(); // Send the value to the app!
  290.  
  291. }
  292. else if (rxValue.find("<CMD:0xA8,0x01>") != -1) {
  293.  
  294. //calib = true;
  295.  
  296. _pCharacteristic->setValue("<ACK:0xA8,0x11>"); // done
  297. _pCharacteristic->notify(); // Send the value to the app!
  298.  
  299. }
  300. else if (rxValue.find("<CMD:0xB8,0x02>") != -1) {
  301.  
  302. //calib = false;
  303.  
  304. _pCharacteristic->setValue("<ACK:0xB8,0x22>"); // done
  305. _pCharacteristic->notify(); // Send the value to the app!
  306.  
  307. }
  308. /*
  309. else if (rxValue.find("0x0") != -1) {
  310.  
  311. digitalWrite(15, HIGH); // sets the digital pin 2 on
  312. delay(500); // waits for 500 sec
  313. digitalWrite(15, LOW); // sets the digital pin 2 off
  314. delay(500); // waits for 500 sec
  315. digitalWrite(15, HIGH); // sets the digital pin 2 on
  316. delay(500); // waits for 500 sec
  317. digitalWrite(15, LOW); // sets the digital pin 2 off
  318. delay(500); // waits for 500 sec
  319.  
  320. _pCharacteristic->setValue("0xA1"); // done
  321. _pCharacteristic->notify(); // Send the value to the app!
  322.  
  323.  
  324. }
  325. */
  326.  
  327. }
  328. }
  329. };
  330.  
  331.  
  332. void setup() {
  333.  
  334. Serial.begin(115200); // Serial setup
  335. Wire1.begin(SDA_1, SCL_1);
  336. Wire.begin(SDA_2, SCL_2);
  337.  
  338. //=================== MPU9250 ACC+GYRO+MAG ==========================
  339.  
  340. while(!Serial){};
  341.  
  342. // Read the WHO_AM_I register, this is a good test of communication
  343. byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
  344. Serial.print(F("MPU9250 I AM 0x"));
  345. Serial.print(c, HEX);
  346. Serial.print(F(" I should be 0x"));
  347. Serial.println(0x71, HEX);
  348.  
  349. if (c == 0x71) // WHO_AM_I should always be 0x71
  350. {
  351. Serial.println(F("MPU9250 is online..."));
  352.  
  353. // Start by performing self test and reporting values
  354. myIMU.MPU9250SelfTest(myIMU.selfTest);
  355. Serial.print(F("x-axis self test: acceleration trim within : "));
  356. Serial.print(myIMU.selfTest[0],1); Serial.println("% of factory value");
  357. Serial.print(F("y-axis self test: acceleration trim within : "));
  358. Serial.print(myIMU.selfTest[1],1); Serial.println("% of factory value");
  359. Serial.print(F("z-axis self test: acceleration trim within : "));
  360. Serial.print(myIMU.selfTest[2],1); Serial.println("% of factory value");
  361. Serial.print(F("x-axis self test: gyration trim within : "));
  362. Serial.print(myIMU.selfTest[3],1); Serial.println("% of factory value");
  363. Serial.print(F("y-axis self test: gyration trim within : "));
  364. Serial.print(myIMU.selfTest[4],1); Serial.println("% of factory value");
  365. Serial.print(F("z-axis self test: gyration trim within : "));
  366. Serial.print(myIMU.selfTest[5],1); Serial.println("% of factory value");
  367.  
  368. // Calibrate gyro and accelerometers, load biases in bias registers
  369. myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
  370.  
  371. myIMU.initMPU9250();
  372. // Initialize device for active mode read of acclerometer, gyroscope, and
  373. // temperature
  374. Serial.println("MPU9250 initialized for active data mode....");
  375.  
  376. // Read the WHO_AM_I register of the magnetometer, this is a good test of
  377. // communication
  378. byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
  379. Serial.print("AK8963 ");
  380. Serial.print("I AM 0x");
  381. Serial.print(d, HEX);
  382. Serial.print(" I should be 0x");
  383. Serial.println(0x48, HEX);
  384.  
  385. if (d != 0x48)
  386. {
  387. // Communication failed, stop here
  388. Serial.println(F("Communication failed, abort!"));
  389. Serial.flush();
  390. abort();
  391. }
  392.  
  393. // Get magnetometer calibration from AK8963 ROM
  394. myIMU.initAK8963(myIMU.factoryMagCalibration);
  395. // Initialize device for active mode read of magnetometer
  396. Serial.println("AK8963 initialized for active data mode....");
  397.  
  398. if (SerialDebug)
  399. {
  400. // Serial.println("Calibration values: ");
  401. Serial.print("X-Axis factory sensitivity adjustment value ");
  402. Serial.println(myIMU.factoryMagCalibration[0], 2);
  403. Serial.print("Y-Axis factory sensitivity adjustment value ");
  404. Serial.println(myIMU.factoryMagCalibration[1], 2);
  405. Serial.print("Z-Axis factory sensitivity adjustment value ");
  406. Serial.println(myIMU.factoryMagCalibration[2], 2);
  407. }
  408.  
  409. // Get sensor resolutions, only need to do this once
  410. myIMU.getAres();
  411. myIMU.getGres();
  412. myIMU.getMres();
  413.  
  414. // The next call delays for 4 seconds, and then records about 15 seconds of
  415. // data to calculate bias and scale.
  416. // myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale);
  417. Serial.println("AK8963 mag biases (mG)");
  418. Serial.println(myIMU.magBias[0]);
  419. Serial.println(myIMU.magBias[1]);
  420. Serial.println(myIMU.magBias[2]);
  421.  
  422. Serial.println("AK8963 mag scale (mG)");
  423. Serial.println(myIMU.magScale[0]);
  424. Serial.println(myIMU.magScale[1]);
  425. Serial.println(myIMU.magScale[2]);
  426. // delay(2000); // Add delay to see results before serial spew of data
  427.  
  428. if(SerialDebug)
  429. {
  430. Serial.println("Magnetometer:");
  431. Serial.print("X-Axis sensitivity adjustment value ");
  432. Serial.println(myIMU.factoryMagCalibration[0], 2);
  433. Serial.print("Y-Axis sensitivity adjustment value ");
  434. Serial.println(myIMU.factoryMagCalibration[1], 2);
  435. Serial.print("Z-Axis sensitivity adjustment value ");
  436. Serial.println(myIMU.factoryMagCalibration[2], 2);
  437. }
  438.  
  439. } // if (c == 0x71)
  440. else
  441. {
  442. Serial.print("Could not connect to MPU9250: 0x");
  443. Serial.println(c, HEX);
  444.  
  445. // Communication failed, stop here
  446. Serial.println(F("Communication failed, abort!"));
  447. Serial.flush();
  448. abort();
  449. }
  450.  
  451. //================== VIBRO ==============================
  452. HMD.begin();
  453. HMD.Mode(0); // Internal trigger input mode -- Must use the GO() function to trigger playback.
  454. HMD.MotorSelect(0x36); // ERM motor, 4x Braking, Medium loop gain, 1.365x back EMF gain
  455. HMD.Library(2); //1-5 & 7 for ERM motors, 6 for LRA motors
  456.  
  457.  
  458. //create a task that will be executed in the Task1code() function, with priority 1 and executed on core 0
  459. xTaskCreatePinnedToCore(
  460. Task1code, /* Task function. */
  461. "Task1", /* name of task. */
  462. 100000, /* Stack size of task */
  463. NULL, /* parameter of the task */
  464. 2, /* priority of the task */
  465. &Task1, /* Task handle to keep track of created task */
  466. 0); /* pin task to core 0 */
  467. //delay(500);
  468.  
  469. //create a task that will be executed in the Task2code() function, with priority 1 and executed on core 1
  470. xTaskCreatePinnedToCore(
  471. Task2code, /* Task function. */
  472. "Task2", /* name of task. */
  473. 100000, /* Stack size of task */
  474. NULL, /* parameter of the task */
  475. 1, /* priority of the task */
  476. &Task2, /* Task handle to keep track of created task */
  477. 1); /* pin task to core 1 */
  478. //delay(500);
  479.  
  480. //******************** BLE DEVICE *************************************************
  481. // Create the BLE Device
  482. BLEDevice::init("RHS2D"); // Give it a name ex ESP32 UART Test
  483.  
  484. // Create the BLE Server
  485. BLEServer *pServer = BLEDevice::createServer();
  486. pServer->setCallbacks(new MyServerCallbacks());
  487.  
  488. // Create the BLE Service
  489. BLEService *pService = pServer->createService(SERVICE_UUID);
  490.  
  491. // Create a BLE Characteristic
  492. _pCharacteristic = pService->createCharacteristic(
  493. CHARACTERISTIC_UUID_TX,
  494. BLECharacteristic::PROPERTY_NOTIFY
  495. );
  496.  
  497. _pCharacteristic->addDescriptor(new BLE2902());
  498.  
  499. BLECharacteristic *pCharacteristic = pService->createCharacteristic(
  500. CHARACTERISTIC_UUID_RX,
  501. BLECharacteristic::PROPERTY_WRITE
  502. );
  503.  
  504. pCharacteristic->setCallbacks(new MyCallbacks());
  505.  
  506. // Start the service
  507. pService->start();
  508.  
  509. // Start advertising
  510. pServer->getAdvertising()->start();
  511. //Serial.println("Waiting a client connection to notify...");
  512. }
  513.  
  514.  
  515. //==========================================================
  516. //==================== TASK 1 CODE =========================
  517. //==========================================================
  518.  
  519. void Task1code( void * pvParameters ){
  520. //Serial.print("Task1 running on core ");
  521. //Serial.println(xPortGetCoreID());
  522.  
  523. for(;;){
  524.  
  525.  
  526. //==================== VIBRO =====================
  527. int seq = 0; //There are 8 sequence registers that can queue up to 8 waveforms
  528. for(int wave = 1; wave <=123; wave++) //There are 123 waveform effects
  529. {
  530. HMD.Waveform(seq, wave);
  531. HMD.go();
  532. delay(600); //give enough time to play effect
  533. Serial.print("Waveform Sequence: ");
  534. Serial.println(seq);
  535. Serial.print("Effect No.: ");
  536. Serial.println(wave);
  537.  
  538. if (wave%8==0) //Each Waveform register can queue 8 effects
  539. {
  540. seq=seq+1;
  541. }
  542. if (wave%64==0) // After the last register is used start over
  543. {
  544. seq=0;
  545. }
  546. }
  547. delay(1);
  548.  
  549.  
  550. }
  551.  
  552. }
  553.  
  554. //==========================================================
  555. //==================== TASK 2 CODE =========================
  556. //==========================================================
  557. void Task2code( void * pvParameters ){
  558. Serial.print("Task2 running on core ");
  559. Serial.println(xPortGetCoreID());
  560.  
  561. for(;;){
  562. Serial.print("Task2 running on core ");
  563. Serial.println(xPortGetCoreID());
  564.  
  565. //=================== MPU9250 ACC+GYRO+MAG ==========================
  566. // If intPin goes high, all data registers have new data
  567. // On interrupt, check if data ready interrupt
  568. if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
  569. {
  570. myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values
  571.  
  572. // Now we'll calculate the accleration value into actual g's
  573. // This depends on scale being set
  574. myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - myIMU.accelBias[0];
  575. myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1];
  576. myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2];
  577.  
  578. myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values
  579.  
  580. // Calculate the gyro value into actual degrees per second
  581. // This depends on scale being set
  582. myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
  583. myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
  584. myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
  585.  
  586. myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values
  587.  
  588. // Calculate the magnetometer values in milliGauss
  589. // Include factory calibration per data sheet and user environmental
  590. // corrections
  591. // Get actual magnetometer value, this depends on scale being set
  592. myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes
  593. * myIMU.factoryMagCalibration[0] - myIMU.magBias[0];
  594. myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes
  595. * myIMU.factoryMagCalibration[1] - myIMU.magBias[1];
  596. myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes
  597. * myIMU.factoryMagCalibration[2] - myIMU.magBias[2];
  598. } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
  599.  
  600. // Must be called before updating quaternions!
  601. myIMU.updateTime();
  602.  
  603. // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of
  604. // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis
  605. // (+ up) of accelerometer and gyro! We have to make some allowance for this
  606. // orientationmismatch in feeding the output to the quaternion filter. For the
  607. // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward
  608. // along the x-axis just like in the LSM9DS0 sensor. This rotation can be
  609. // modified to allow any convenient orientation convention. This is ok by
  610. // aircraft orientation standards! Pass gyro rate as rad/s
  611. //MadgwickQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
  612. // myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
  613. // myIMU.mx, myIMU.mz, myIMU.deltat);
  614.  
  615. MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
  616. myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
  617. myIMU.mx, myIMU.mz, myIMU.deltat);
  618.  
  619. if (!AHRS)
  620. {
  621. myIMU.delt_t = millis() - myIMU.count;
  622. if (myIMU.delt_t > 500)
  623. {
  624. if(SerialDebug)
  625. {
  626. // Print acceleration values in milligs!
  627. Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax);
  628. Serial.print(" mg ");
  629. Serial.print("Y-acceleration: "); Serial.print(1000 * myIMU.ay);
  630. Serial.print(" mg ");
  631. Serial.print("Z-acceleration: "); Serial.print(1000 * myIMU.az);
  632. Serial.println(" mg ");
  633.  
  634. // Print gyro values in degree/sec
  635. Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3);
  636. Serial.print(" degrees/sec ");
  637. Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3);
  638. Serial.print(" degrees/sec ");
  639. Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3);
  640. Serial.println(" degrees/sec");
  641.  
  642. // Print mag values in degree/sec
  643. Serial.print("X-mag field: "); Serial.print(myIMU.mx);
  644. Serial.print(" mG ");
  645. Serial.print("Y-mag field: "); Serial.print(myIMU.my);
  646. Serial.print(" mG ");
  647. Serial.print("Z-mag field: "); Serial.print(myIMU.mz);
  648. Serial.println(" mG");
  649.  
  650. myIMU.tempCount = myIMU.readTempData(); // Read the adc values
  651. // Temperature in degrees Centigrade
  652. myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0;
  653. // Print temperature in degrees Centigrade
  654. Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1);
  655. Serial.println(" degrees C");
  656. }
  657.  
  658. myIMU.count = millis();
  659. //digitalWrite(myLed, !digitalRead(myLed)); // toggle led
  660. } // if (myIMU.delt_t > 500)
  661. } // if (!AHRS)
  662. else
  663. {
  664. // Serial print and/or display at 0.5 s rate independent of data rates
  665. myIMU.delt_t = millis() - myIMU.count;
  666.  
  667. // update LCD once per half-second independent of read rate
  668. if (myIMU.delt_t > 500)
  669. {
  670. if(SerialDebug)
  671. {
  672. Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax);
  673. Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay);
  674. Serial.print(" az = "); Serial.print((int)1000 * myIMU.az);
  675. Serial.println(" mg");
  676.  
  677. Serial.print("gx = "); Serial.print(myIMU.gx, 2);
  678. Serial.print(" gy = "); Serial.print(myIMU.gy, 2);
  679. Serial.print(" gz = "); Serial.print(myIMU.gz, 2);
  680. Serial.println(" deg/s");
  681.  
  682. Serial.print("mx = "); Serial.print((int)myIMU.mx);
  683. Serial.print(" my = "); Serial.print((int)myIMU.my);
  684. Serial.print(" mz = "); Serial.print((int)myIMU.mz);
  685. Serial.println(" mG");
  686.  
  687. Serial.print("q0 = "); Serial.print(*getQ());
  688. Serial.print(" qx = "); Serial.print(*(getQ() + 1));
  689. Serial.print(" qy = "); Serial.print(*(getQ() + 2));
  690. Serial.print(" qz = "); Serial.println(*(getQ() + 3));
  691. }
  692.  
  693. // Define output variables from updated quaternion---these are Tait-Bryan
  694. // angles, commonly used in aircraft orientation. In this coordinate system,
  695. // the positive z-axis is down toward Earth. Yaw is the angle between Sensor
  696. // x-axis and Earth magnetic North (or true North if corrected for local
  697. // declination, looking down on the sensor positive yaw is counterclockwise.
  698. // Pitch is angle between sensor x-axis and Earth ground plane, toward the
  699. // Earth is positive, up toward the sky is negative. Roll is angle between
  700. // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
  701. // arise from the definition of the homogeneous rotation matrix constructed
  702. // from quaternions. Tait-Bryan angles as well as Euler angles are
  703. // non-commutative; that is, the get the correct orientation the rotations
  704. // must be applied in the correct order which for this configuration is yaw,
  705. // pitch, and then roll.
  706. // For more see
  707. // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  708. // which has additional links.
  709. myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ()
  710. * *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1)
  711. * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3)
  712. * *(getQ()+3));
  713. myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ()
  714. * *(getQ()+2)));
  715. myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2)
  716. * *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1)
  717. * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) + *(getQ()+3)
  718. * *(getQ()+3));
  719. myIMU.pitch *= RAD_TO_DEG;
  720. myIMU.yaw *= RAD_TO_DEG;
  721.  
  722. // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
  723. // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19
  724. // - http://www.ngdc.noaa.gov/geomag-web/#declination
  725. myIMU.yaw -= 8.5;
  726. myIMU.roll *= RAD_TO_DEG;
  727.  
  728. if(SerialDebug)
  729. {
  730. Serial.print("Yaw, Pitch, Roll: ");
  731. Serial.print(myIMU.yaw, 2);
  732. Serial.print(", ");
  733. Serial.print(myIMU.pitch, 2);
  734. Serial.print(", ");
  735. Serial.println(myIMU.roll, 2);
  736.  
  737. Serial.print("rate = ");
  738. Serial.print((float)myIMU.sumCount / myIMU.sum, 2);
  739. Serial.println(" Hz");
  740. }
  741.  
  742. myIMU.count = millis();
  743. myIMU.sumCount = 0;
  744. myIMU.sum = 0;
  745. } // if (myIMU.delt_t > 500)
  746. } // if (AHRS)
  747. }
  748. }
  749.  
  750. //==========================================================
  751. //================== START EMPTY LOOP ======================
  752. //==========================================================
  753. void loop() {
  754.  
  755. }
  756.  
  757. //==========================================================
  758. //==================== ENDEMPTY LOOP =======================
  759. //==========================================================
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!