collab/pb-hal

git clone https://git.julia.blue/collab/pb-hal

jspspikeFinished interrupt gyro valuesd142ea3

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