collab/pb-hal
git clone https://git.julia.blue/collab/pb-hal
d142ea3
imu
1// 2// Created by jspspike on 4/12/18. 3// 4/* 5To compile on a Raspberry Pi (1 or 2) 61. install the bcm2835 library, see http://www.airspayce.com/mikem/bcm2835/index.html 72. enable i2c on your RPi , see https://learn.adafruit.com/adafruits-raspberry-pi-lesson-4-gpio-setup/configuring-i2c 83. connect your i2c devices 94. then from bash 10$ PATH_I2CDEVLIB=~/i2cdevlib/ 11$ gcc -o MPU6050_DCM_test ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/MPU6050/examples/MPU6050_DCM_test.cpp \ 12-I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp \ 13-I ${PATH_I2CDEVLIB}/Arduino/MPU6050/ ${PATH_I2CDEVLIB}/Arduino/MPU6050/MPU6050.cpp -l bcm2835 -l m 14$ sudo ./MPU6050_DCM_test 15 16*/ 17 18#include <stdio.h> 19#include <bcm2835.h> 20#include "I2Cdev.h" 21#include "MPU6050_6Axis_MotionApps20.h" 22#include <pigpio.h> 23//#include "../../I2Cdev/I2Cdev.h" 24//#include "../../../MSP430/MPU6050/MPU6050z.h" 25//#include "../../../Arduino/MPU6050/helper_3dmath.h" 26#include <math.h> 27#include <cstdint> 28#include <time.h> 29 30 31 32int gyroInit () { 33 34} 35 36void getGyroValues () { 37 38 39} 40 41void printGyroValues () { 42 43} 44 45int main (int argc ,char ** argv ) { 46 47MPU6050 gyro ; 48 49printf ("Starting DMP MPU6050 test\n" ); 50I2Cdev ::initialize (); 51 52if (gyro .testConnection ()) 53printf ("Connected\n" ); 54else 55printf ("Connection failed\nContinuing anyways\n" ); 56 57printf ("Init Gyro\n" ); 58gyro .initialize (); 59printf ("Init DMP\n" ); 60int dmpStatus = gyro .dmpInitialize (); 61 62 63 64gyro .setXGyroOffset (220 ); 65gyro .setYGyroOffset (76 ); 66gyro .setZGyroOffset (-85 ); 67gyro .setZAccelOffset (1788 ); 68 69if (dmpStatus == 0 ) { 70printf ("Enabling DMP\n" ); 71gyro .setDMPEnabled (true); 72 } 73else { 74printf ("DMP Init failed\n" ); 75return 1 ; 76 } 77 78uint16_t fifoCount = 0 ; 79uint8_t fifoBuffer [64 ]; 80uint8_t mpuIntStatus ; 81 82uint16_t packetSize ; 83 84Quaternion q ; 85float euler [3 ]; 86 87packetSize = gyro .dmpGetFIFOPacketSize (); 88 89 90while (true) { 91// mpuIntStatus = gyro.getIntStatus(); 92// fifoCount = gyro.getFIFOCount(); 93 94while (!((mpuIntStatus = gyro .getIntStatus ())& 0x02 )); 95 96 97 98if ((mpuIntStatus & 0x10 )|| (fifoCount == 1024 )) { 99gyro .resetFIFO (); 100printf ("FIFO Overflow\r" ); 101 }else if (mpuIntStatus & 0x02 ) { 102while ((fifoCount = gyro .getFIFOCount ())< packetSize ) {} 103 104gyro .getFIFOBytes (fifoBuffer ,packetSize ); 105 106fifoCount -= packetSize ; 107 108gyro .dmpGetQuaternion (& q ,fifoBuffer ); 109gyro .dmpGetEuler (euler ,& q ); 110 111printf ("Euler: " ); 112printf ("X: %f " ,euler [0 ]* 180 /M_PI ); 113printf ("Y: %f " ,euler [1 ]* 180 /M_PI ); 114printf ("Z: %f\r" ,euler [2 ]* 180 /M_PI ); 115 116 } 117else 118printf ("MPUIntStatus incorrect\n" ); 119 } 120 121return 0 ; 122}