73 lines
1.3 KiB
C++
73 lines
1.3 KiB
C++
|
#include "Arduino.h"
|
||
|
|
#include "imu.h"
|
||
|
|
|
||
|
|
|
||
|
|
MPU6050 mpu6050(Wire,IMU_AC,IMU_GC);
|
||
|
|
|
||
|
|
|
||
|
|
bool imu_bellyup=false;
|
||
|
|
|
||
|
|
void initIMU() {
|
||
|
|
initIMU(0,0,0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void initIMU(float x,float y,float z) {
|
||
|
|
Wire.begin(25,24);
|
||
|
|
mpu6050.begin();
|
||
|
|
|
||
|
|
if (x==0 && y==0 && z==0) {
|
||
|
|
mpu6050.calcGyroOffsets(true);
|
||
|
|
}else{
|
||
|
|
mpu6050.setGyroOffsets(x, y, z);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void loopIMU(unsigned long millis) {
|
||
|
|
static unsigned long last_update_imu=0;
|
||
|
|
if (millis-last_update_imu>UPDATE_IMU_INTERVAL) {
|
||
|
|
last_update_imu=millis;
|
||
|
|
mpu6050.update();
|
||
|
|
/*Serial.print("angleX : ");
|
||
|
|
Serial.print(mpu6050.getAngleX());
|
||
|
|
Serial.print("\tangleY : ");
|
||
|
|
Serial.print(mpu6050.getAngleY());
|
||
|
|
Serial.print("\tangleZ : ");
|
||
|
|
Serial.println(mpu6050.getAngleZ());*/
|
||
|
|
|
||
|
|
if(mpu6050.getAccZ()<-0.5) { //is upside down
|
||
|
|
imu_bellyup=true;
|
||
|
|
}else{
|
||
|
|
if (mpu6050.getAccZ()>0.7) { //is on flat surface
|
||
|
|
imu_bellyup=false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
bool isBellyUp() {
|
||
|
|
return imu_bellyup;
|
||
|
|
}
|
||
|
|
|
||
|
|
float getAngleX() {
|
||
|
|
return mpu6050.getAngleX();
|
||
|
|
}
|
||
|
|
|
||
|
|
float getAngleY() {
|
||
|
|
return mpu6050.getAngleY();
|
||
|
|
}
|
||
|
|
|
||
|
|
float getAngleZ() {
|
||
|
|
return mpu6050.getAngleZ();
|
||
|
|
}
|
||
|
|
|
||
|
|
float getAccX() {
|
||
|
|
return mpu6050.getAccX();
|
||
|
|
}
|
||
|
|
float getAccY() {
|
||
|
|
return mpu6050.getAccY();
|
||
|
|
}
|
||
|
|
float getAccZ() {
|
||
|
|
return mpu6050.getAccZ();
|
||
|
|
}
|