collab/pb-hal

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

jspspikeFinished interrupt gyro valuesd142ea3

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