67 lines
1.6 KiB
C++
67 lines
1.6 KiB
C++
#include "MPU6050.h"
|
|
#include <Wire.h>
|
|
#include "../Config.h"
|
|
#include "../Static.h"
|
|
|
|
MPU5060::MPU5060() : InputSource("MPU5060") {}
|
|
|
|
void
|
|
MPU5060::onStart()
|
|
{
|
|
Wire.begin();
|
|
|
|
// Turn on the sensor
|
|
Wire.beginTransmission(I2C_ADDRESS);
|
|
Wire.write(PWR_MGMT_1);
|
|
Wire.write(0);
|
|
Wire.endTransmission(true);
|
|
|
|
// Configure the filter
|
|
Wire.beginTransmission(I2C_ADDRESS);
|
|
Wire.write(CONFIG_REG);
|
|
Wire.write(3);
|
|
Wire.endTransmission(true);
|
|
|
|
// Configure the accel range
|
|
Wire.beginTransmission(I2C_ADDRESS);
|
|
Wire.write(ACCEL_CONFIG_REG);
|
|
// 4G
|
|
Wire.write(2 << 3);
|
|
Wire.endTransmission(true);
|
|
}
|
|
|
|
void
|
|
MPU5060::onStop()
|
|
{
|
|
Wire.beginTransmission(I2C_ADDRESS);
|
|
// Turn off the sensor
|
|
Wire.write(PWR_MGMT_1);
|
|
Wire.write(1);
|
|
Wire.endTransmission(true);
|
|
//Wire.end();
|
|
}
|
|
|
|
InputEvent
|
|
MPU5060::read()
|
|
{
|
|
EVERY_N_MILLISECONDS(5) {
|
|
Wire.beginTransmission(I2C_ADDRESS);
|
|
Wire.write(ACCEL_XOUT_HIGH);
|
|
Wire.endTransmission(false);
|
|
Wire.requestFrom(I2C_ADDRESS, 6);
|
|
const int16_t accelX = Wire.read() << 8 | Wire.read();
|
|
const int16_t accelY = Wire.read() << 8 | Wire.read();
|
|
const int16_t accelZ = Wire.read() << 8 | Wire.read();
|
|
const uint16_t accelSum = abs(accelX) + abs(accelY) + abs(accelZ);
|
|
const uint16_t delta = abs(m_value.value() - accelSum);
|
|
m_value.add(accelSum);
|
|
if (delta > 32) {
|
|
return InputEvent{InputEvent::Acceleration, delta};
|
|
}
|
|
}
|
|
return InputEvent{};
|
|
}
|
|
|
|
STATIC_ALLOC(MPU5060);
|
|
STATIC_TASK(MPU5060);
|