API tools faq
paste
RichieHard

Untitled

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