82 lines
1.4 KiB
C++
82 lines
1.4 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();
|
|
}
|
|
|
|
float getGyroAngleX() {
|
|
return mpu6050.getGyroAngleX();
|
|
}
|
|
float getGyroAngleY() {
|
|
return mpu6050.getGyroAngleY();
|
|
}
|
|
float getGyroAngleZ() {
|
|
return mpu6050.getGyroAngleZ();
|
|
}
|