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 29MPU6050 gyro ; 30uint16_t packetSize ; 31 32Quaternion q ; 33float euler [3 ]; 34 35 36 37void getGyroValues (int GPIO ,int level ,uint32_t tick ) { 38 39uint16_t fifoCount = gyro .getFIFOCount (); 40uint8_t fifoBuffer [64 ]; 41uint8_t mpuIntStatus = gyro .getIntStatus (); 42 43 44if ((mpuIntStatus & 0x10 )|| (fifoCount == 1024 )) { 45gyro .resetFIFO (); 46printf ("FIFO Overflow\r" ); 47 }else if (mpuIntStatus & 0x02 ) { 48while ((fifoCount = gyro .getFIFOCount ())< packetSize ) {} 49 50gyro .getFIFOBytes (fifoBuffer ,packetSize ); 51 52fifoCount -= packetSize ; 53 54gyro .dmpGetQuaternion (& q ,fifoBuffer ); 55gyro .dmpGetEuler (euler ,& q ); 56 } 57else 58printf ("MPUIntStatus incorrect\n" ); 59} 60 61int gyroInit () { 62gpioInitialise (); 63gpioSetMode (22 ,PI_INPUT ); 64gpioSetISRFunc (22 ,RISING_EDGE ,0 ,getGyroValues ); 65 66printf ("Starting DMP MPU6050 test\n" ); 67I2Cdev ::initialize (); 68 69if (gyro .testConnection ()) 70printf ("Connected\n" ); 71else 72printf ("Connection failed\nContinuing anyways\n" ); 73 74printf ("Init Gyro\n" ); 75gyro .initialize (); 76printf ("Init DMP\n" ); 77int dmpStatus = gyro .dmpInitialize (); 78 79 80 81gyro .setXGyroOffset (220 ); 82gyro .setYGyroOffset (76 ); 83gyro .setZGyroOffset (-85 ); 84gyro .setZAccelOffset (1788 ); 85 86if (dmpStatus == 0 ) { 87printf ("Enabling DMP\n" ); 88gyro .setDMPEnabled (true); 89 } 90else { 91printf ("DMP Init failed\n" ); 92return 1 ; 93 } 94 95packetSize = gyro .dmpGetFIFOPacketSize (); 96 97return 0 ; 98} 99 100void printGyroValues () { 101printf ("Euler: " ); 102printf ("X: %f " ,euler [0 ]* 180 /M_PI ); 103printf ("Y: %f " ,euler [1 ]* 180 /M_PI ); 104printf ("Z: %f\r" ,euler [2 ]* 180 /M_PI ); 105 106} 107 108int main (int argc ,char ** argv ) { 109 110if (gyroInit ()) 111return 1 ; 112 113while (true) { 114printGyroValues (); 115 } 116 117return 0 ; 118}