141 lines
3.5 KiB
C++
141 lines
3.5 KiB
C++
///////////////////////////////////////////////////////////
|
|
// TILT SENSOR
|
|
|
|
void set_last_read_angle_data(unsigned long t, int16_t x, int16_t y, int16_t z, int16_t x_gyro, int16_t y_gyro, int16_t z_gyro) {
|
|
last_read_time = t;
|
|
last_x_angle = x;
|
|
last_y_angle = y;
|
|
last_z_angle = z;
|
|
last_gyro_x_angle = x_gyro;
|
|
last_gyro_y_angle = y_gyro;
|
|
last_gyro_z_angle = z_gyro;
|
|
}
|
|
|
|
//Function used to detect motion. Tolerance variable adjusts the sensitivity of movement detected.
|
|
boolean checkMotion() {
|
|
boolean tempB = false;
|
|
xVal = last_x_angle;
|
|
yVal = last_y_angle;
|
|
zVal = last_z_angle;
|
|
|
|
if (xVal > (xMax + tolerance) || xVal < (xMin - tolerance)) {
|
|
tempB = true;
|
|
}
|
|
|
|
if (yVal > (yMax + tolerance) || yVal < (yMin - tolerance)) {
|
|
tempB = true;
|
|
}
|
|
|
|
if (zVal > (zMax + tolerance) || zVal < (zMin - tolerance)) {
|
|
tempB = true;
|
|
}
|
|
|
|
return tempB;
|
|
}
|
|
|
|
void calibrateTILT() {
|
|
// reset alarm
|
|
moveDetected = false;
|
|
|
|
tiltReading = 0;
|
|
|
|
//initialise x,y,z variables
|
|
xVal = last_x_angle;
|
|
xMin = xVal;
|
|
xMax = xVal;
|
|
|
|
yVal = last_y_angle;
|
|
yMin = yVal;
|
|
yMax = yVal;
|
|
|
|
zVal = last_z_angle;
|
|
zMin = zVal;
|
|
zMax = zVal;
|
|
|
|
//calibrate the Accelerometer (should take about 0.5 seconds)
|
|
for (unsigned int i = 0; i < 50; i++) {
|
|
// Calibrate X Values
|
|
xVal = last_x_angle;
|
|
if (xVal > xMax) {
|
|
xMax = xVal;
|
|
} else if (xVal < xMin) {
|
|
xMin = xVal;
|
|
}
|
|
|
|
// Calibrate Y Values
|
|
yVal = last_y_angle;
|
|
if (yVal > yMax) {
|
|
yMax = yVal;
|
|
} else if (yVal < yMin) {
|
|
yMin = yVal;
|
|
}
|
|
|
|
// Calibrate Z Values
|
|
zVal = last_z_angle;
|
|
if (zVal > zMax) {
|
|
zMax = zVal;
|
|
} else if (zVal < zMin) {
|
|
zMin = zVal;
|
|
}
|
|
|
|
}
|
|
|
|
calibrated = true;
|
|
|
|
}
|
|
|
|
void updateTILT(){
|
|
|
|
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
|
|
|
float gyro_x = (gx - base_x_gyro) / 131.0;
|
|
float gyro_y = (gy - base_y_gyro) / 131.0;
|
|
float gyro_z = (gz - base_z_gyro) / 131.0;
|
|
|
|
float accel_x = ax;
|
|
float accel_y = ay;
|
|
float accel_z = az;
|
|
|
|
float accel_angle_y = atan(-1 * accel_x / sqrt(pow(accel_y, 2) + pow(accel_z, 2))) * (180 / 3.14159);
|
|
float accel_angle_x = atan(accel_y / sqrt(pow(accel_x, 2) + pow(accel_z, 2))) * (180 / 3.14159);
|
|
float accel_angle_z = 0;
|
|
|
|
// Compute the (filtered) gyro angles
|
|
unsigned long t_now = millis();
|
|
float dt = (t_now - last_read_time) / 1000.0;
|
|
float gyro_angle_x = gyro_x * dt + last_x_angle;
|
|
float gyro_angle_y = gyro_y * dt + last_y_angle;
|
|
float gyro_angle_z = gyro_z * dt + last_z_angle;
|
|
|
|
// Compute the drifting gyro angles
|
|
float unfiltered_gyro_angle_x = gyro_x * dt + last_gyro_x_angle;
|
|
float unfiltered_gyro_angle_y = gyro_y * dt + last_gyro_y_angle;
|
|
float unfiltered_gyro_angle_z = gyro_z * dt + last_gyro_z_angle;
|
|
|
|
// Apply the complementary filter to figure out the change in angle
|
|
float angle_x = 0.96 * gyro_angle_x + (1.0 - 0.96) * accel_angle_x;
|
|
float angle_y = 0.96 * gyro_angle_y + (1.0 - 0.96) * accel_angle_y;
|
|
float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle
|
|
|
|
set_last_read_angle_data(t_now, (int16_t)angle_x, (int16_t)angle_y, (int16_t)angle_z, (int16_t)unfiltered_gyro_angle_x, (int16_t)unfiltered_gyro_angle_y, (int16_t)unfiltered_gyro_angle_z);
|
|
|
|
if (calibrated) {
|
|
if (checkMotion()) {
|
|
timeTilt = millis();
|
|
moveDetected = true;
|
|
}
|
|
}
|
|
|
|
if (moveDetected) {
|
|
tiltReading = 1;
|
|
//don't check for movement until recalibrated again
|
|
calibrated = false;
|
|
if (millis() - timeTilt > tiltWait) {
|
|
calibrateTILT();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|