119 lines
2.2 KiB
C++
119 lines
2.2 KiB
C++
#include "Arduino.h"
|
|
#include "imu.h"
|
|
|
|
|
|
MPU6050 mpu6050(Wire,IMU_AC,IMU_GC);
|
|
|
|
float maxAccX;
|
|
float maxAccY;
|
|
float maxAccZ;
|
|
|
|
|
|
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());*/
|
|
|
|
|
|
maxAccX-=UPDATE_IMU_INTERVAL/1000.0*1.0; //slowly reduce values
|
|
maxAccY-=UPDATE_IMU_INTERVAL/1000.0*1.0;
|
|
maxAccZ-=UPDATE_IMU_INTERVAL/1000.0*1.0;
|
|
static float last_accX;
|
|
static float last_accY;
|
|
static float last_accZ;
|
|
float _accX=getAccX();
|
|
float _accY=getAccY();
|
|
float _accZ=getAccZ();
|
|
maxAccX=max(abs(last_accX-_accX), maxAccX);
|
|
maxAccY=max(abs(last_accY-_accY), maxAccY);
|
|
maxAccZ=max(abs(last_accZ-_accZ), maxAccZ);
|
|
last_accX=_accX;
|
|
last_accY=_accY;
|
|
last_accZ=_accZ;
|
|
|
|
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 getMaxAccX() {
|
|
return maxAccX;
|
|
}
|
|
float getMaxAccY() {
|
|
return maxAccY;
|
|
}
|
|
float getMaxAccZ() {
|
|
return maxAccZ;
|
|
}
|
|
void resetMaxAcc(){
|
|
maxAccX=0;
|
|
maxAccY=0;
|
|
maxAccZ=0;
|
|
}
|
|
|
|
float getGyroAngleX() {
|
|
return mpu6050.getGyroAngleX();
|
|
}
|
|
float getGyroAngleY() {
|
|
return mpu6050.getGyroAngleY();
|
|
}
|
|
float getGyroAngleZ() {
|
|
return mpu6050.getGyroAngleZ();
|
|
}
|