403 lines
14 KiB
C++
403 lines
14 KiB
C++
/*
|
|
MIT License
|
|
|
|
Copyright (c) 2021-22 Magnus
|
|
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
of this software and associated documentation files (the "Software"), to deal
|
|
in the Software without restriction, including without limitation the rights
|
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
copies of the Software, and to permit persons to whom the Software is
|
|
furnished to do so, subject to the following conditions:
|
|
|
|
The above copyright notice and this permission notice shall be included in all
|
|
copies or substantial portions of the Software.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
SOFTWARE.
|
|
*/
|
|
#include <gyro.hpp>
|
|
#include <main.hpp>
|
|
|
|
GyroSensor myGyro;
|
|
MPU6050 accelgyro;
|
|
|
|
#define GYRO_USE_INTERRUPT // Use interrupt to detect when new sample is ready
|
|
#define GYRO_SHOW_MINMAX // Will calculate the min/max values when doing
|
|
// calibration
|
|
// #define GYRO_CALIBRATE_STARTUP // Will calibrate sensor at startup
|
|
|
|
//
|
|
// Initialize the sensor chip.
|
|
//
|
|
bool GyroSensor::setup() {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Setting up hardware." CR));
|
|
#endif
|
|
Wire.begin(PIN_SDA, PIN_SCL);
|
|
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having
|
|
// compilation difficulties
|
|
|
|
uint8_t id = accelgyro.getDeviceID();
|
|
|
|
if (id != 0x34 && id != 0x38) { // Allow both MPU6050 and MPU6000
|
|
writeErrorLog("GYRO: Failed to connect to gyro, is it connected?");
|
|
_sensorConnected = false;
|
|
} else {
|
|
#if !defined(GYRO_DISABLE_LOGGING)
|
|
Log.notice(F("GYRO: Connected to MPU6050 (gyro)." CR));
|
|
#endif
|
|
accelgyro.initialize();
|
|
_sensorConnected = true;
|
|
|
|
// Configure the sensor
|
|
accelgyro.setTempSensorEnabled(true);
|
|
accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);
|
|
#if defined(GYRO_USE_INTERRUPT)
|
|
// Alternative method to read data, let the MPU signal when sampling is
|
|
// done.
|
|
accelgyro.setRate(17);
|
|
accelgyro.setInterruptDrive(1);
|
|
accelgyro.setInterruptMode(1);
|
|
accelgyro.setInterruptLatch(0);
|
|
accelgyro.setIntDataReadyEnabled(true);
|
|
#endif
|
|
|
|
#if defined(GYRO_CALIBRATE_STARTUP)
|
|
// Run the calibration at start, useful for testing.
|
|
calibrateSensor();
|
|
#endif
|
|
|
|
// Once we have calibration values stored we just apply them from the
|
|
// config.
|
|
_calibrationOffset = myConfig.getGyroCalibration();
|
|
applyCalibration();
|
|
}
|
|
return _sensorConnected;
|
|
}
|
|
|
|
//
|
|
// Set sensor in sleep mode to conserve battery
|
|
//
|
|
void GyroSensor::enterSleep() {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Setting up hardware." CR));
|
|
#endif
|
|
accelgyro.setSleepEnabled(true);
|
|
}
|
|
|
|
//
|
|
// Do a number of reads to get a more stable value.
|
|
//
|
|
void GyroSensor::readSensor(RawGyroData &raw, const int noIterations,
|
|
const int delayTime) {
|
|
RawGyroDataL average = {0, 0, 0, 0, 0, 0};
|
|
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Reading sensor with %d iterations %d us delay." CR),
|
|
noIterations, delayTime);
|
|
#endif
|
|
|
|
// Set some initial values
|
|
#if defined(GYRO_SHOW_MINMAX)
|
|
RawGyroData min, max;
|
|
accelgyro.getAcceleration(&min.ax, &min.ay, &min.az);
|
|
min.temp = accelgyro.getTemperature();
|
|
max = min;
|
|
#endif
|
|
for (int cnt = 0; cnt < noIterations; cnt++) {
|
|
#if defined(GYRO_USE_INTERRUPT)
|
|
while (accelgyro.getIntDataReadyStatus() == 0) {
|
|
delayMicroseconds(1);
|
|
}
|
|
#endif
|
|
|
|
accelgyro.getMotion6(&raw.ax, &raw.ay, &raw.az, &raw.gx, &raw.gy, &raw.gz);
|
|
raw.temp = accelgyro.getTemperature();
|
|
|
|
average.ax += raw.ax;
|
|
average.ay += raw.ay;
|
|
average.az += raw.az;
|
|
average.gx += raw.gx;
|
|
average.gy += raw.gy;
|
|
average.gz += raw.gz;
|
|
average.temp += raw.temp;
|
|
|
|
// Log what the minium value is
|
|
#if defined(GYRO_SHOW_MINMAX)
|
|
if (raw.ax < min.ax) min.ax = raw.ax;
|
|
if (raw.ay < min.ay) min.ay = raw.ay;
|
|
if (raw.az < min.az) min.az = raw.az;
|
|
if (raw.gx < min.gx) min.gx = raw.gx;
|
|
if (raw.gy < min.gy) min.gy = raw.gy;
|
|
if (raw.gz < min.gz) min.gz = raw.gz;
|
|
if (raw.temp < min.temp) min.temp = raw.temp;
|
|
|
|
// Log what the maximum value is
|
|
if (raw.ax > max.ax) max.ax = raw.ax;
|
|
if (raw.ay > max.ay) max.ay = raw.ay;
|
|
if (raw.az > max.az) max.az = raw.az;
|
|
if (raw.gx > max.gx) max.gx = raw.gx;
|
|
if (raw.gy > max.gy) max.gy = raw.gy;
|
|
if (raw.gz > max.gz) max.gz = raw.gz;
|
|
if (raw.temp > max.temp) max.temp = raw.temp;
|
|
#endif
|
|
|
|
#if !defined(GYRO_USE_INTERRUPT)
|
|
delayMicroseconds(delayTime);
|
|
#endif
|
|
}
|
|
|
|
raw.ax = average.ax / noIterations;
|
|
raw.ay = average.ay / noIterations;
|
|
raw.az = average.az / noIterations;
|
|
raw.gx = average.gx / noIterations;
|
|
raw.gy = average.gy / noIterations;
|
|
raw.gz = average.gz / noIterations;
|
|
raw.temp = average.temp / noIterations;
|
|
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
#if defined(GYRO_SHOW_MINMAX)
|
|
Log.verbose(F("GYRO: Min \t%d\t%d\t%d\t%d\t%d\t%d\t%d." CR), min.ax,
|
|
min.ay, min.az, min.gx, min.gy, min.gz, min.temp);
|
|
Log.verbose(F("GYRO: Max \t%d\t%d\t%d\t%d\t%d\t%d\t%d." CR), max.ax,
|
|
max.ay, max.az, max.gx, max.gy, max.gz, max.temp);
|
|
#endif
|
|
Log.verbose(F("GYRO: Average\t%d\t%d\t%d\t%d\t%d\t%d\t%d." CR), raw.ax,
|
|
raw.ay, raw.az, raw.gx, raw.gy, raw.gz, raw.temp);
|
|
// Log.verbose(F("GYRO: Result \t%d\t%d\t%d\t%d\t%d\t%d." CR),
|
|
// average.ax/noIterations, average.ay/noIterations, average.az/noIterations,
|
|
// average.gx/noIterations,
|
|
// average.gy/noIterations,
|
|
// average.gz/noIterations
|
|
// );
|
|
#endif
|
|
}
|
|
|
|
//
|
|
// Calcuate the angles (tilt)
|
|
//
|
|
float GyroSensor::calculateAngle(RawGyroData &raw) {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Calculating the angle." CR));
|
|
#endif
|
|
|
|
// Smooth out the readings to we can have a more stable angle/tilt.
|
|
// ------------------------------------------------------------------------------------------------------------
|
|
// Accelerometer full scale range of +/- 2g with Sensitivity Scale Factor of
|
|
// 16,384 LSB(Count)/g. Gyroscope full scale range of +/- 250 °/s with
|
|
// Sensitivity Scale Factor of 131 LSB (Count)/°/s.
|
|
float ax = (static_cast<float>(raw.ax)) / 16384,
|
|
ay = (static_cast<float>(raw.ay)) / 16384,
|
|
az = (static_cast<float>(raw.az)) / 16384;
|
|
|
|
// Source: https://www.nxp.com/docs/en/application-note/AN3461.pdf
|
|
float vY = (acos(abs(ay) / sqrt(ax * ax + ay * ay + az * az)) * 180.0 / PI);
|
|
// float vZ = (acos(abs(az) / sqrt(ax * ax + ay * ay + az * az)) * 180.0 /
|
|
// PI); float vX = (acos(abs(ax) / sqrt(ax * ax + ay * ay + az * az)) * 180.0
|
|
// / PI);
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
// Log.notice(F("GYRO: angleX= %F." CR), vX);
|
|
Log.notice(F("GYRO: angleY= %F." CR), vY);
|
|
// Log.notice(F("GYRO: angleZ= %F." CR), vZ);
|
|
#endif
|
|
return vY;
|
|
}
|
|
|
|
//
|
|
// Check if the values are high that indicate that the sensor is moving.
|
|
//
|
|
bool GyroSensor::isSensorMoving(RawGyroData &raw) {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Checking for sensor movement." CR));
|
|
#endif
|
|
|
|
int x = abs(raw.gx), y = abs(raw.gy), z = abs(raw.gz);
|
|
int threashold = myAdvancedConfig.getGyroSensorMovingThreashold();
|
|
|
|
if (x > threashold || y > threashold || z > threashold) {
|
|
Log.notice(F("GYRO: Movement detected (%d)\t%d\t%d\t%d." CR), threashold, x,
|
|
y, z);
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
//
|
|
// Read the tilt angle from the gyro.
|
|
//
|
|
bool GyroSensor::read() {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Getting new gyro position." CR));
|
|
#endif
|
|
|
|
if (!_sensorConnected) return false;
|
|
|
|
readSensor(
|
|
_lastGyroData, myAdvancedConfig.getGyroReadCount(),
|
|
myAdvancedConfig.getGyroReadDelay()); // Last param is unused if
|
|
// GYRO_USE_INTERRUPT is defined.
|
|
|
|
// If the sensor is unstable we return false to signal we dont have valid
|
|
// value
|
|
if (isSensorMoving(_lastGyroData)) {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.notice(F("GYRO: Sensor is moving." CR));
|
|
#endif
|
|
_validValue = false;
|
|
} else {
|
|
_validValue = true;
|
|
_angle = calculateAngle(_lastGyroData);
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Sensor values %d,%d,%d\t%F" CR), _lastGyroData.ax,
|
|
_lastGyroData.ay, _lastGyroData.az, _angle);
|
|
#endif
|
|
}
|
|
|
|
_sensorTemp = (static_cast<float>(_lastGyroData.temp)) / 340 + 36.53;
|
|
|
|
// The first read value is close to the DS18 value according to my tests, if
|
|
// more reads are done then the gyro temp will increase to much
|
|
if (_initialSensorTemp == INVALID_TEMPERATURE)
|
|
_initialSensorTemp = _sensorTemp;
|
|
|
|
return _validValue;
|
|
}
|
|
|
|
//
|
|
// Dump the stored calibration values.
|
|
//
|
|
void GyroSensor::dumpCalibration() {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Accel offset\t%d\t%d\t%d" CR), _calibrationOffset.ax,
|
|
_calibrationOffset.ay, _calibrationOffset.az);
|
|
Log.verbose(F("GYRO: Gyro offset \t%d\t%d\t%d" CR), _calibrationOffset.gx,
|
|
_calibrationOffset.gy, _calibrationOffset.gz);
|
|
#endif
|
|
}
|
|
|
|
//
|
|
// Update the sensor with out calculated offsets.
|
|
//
|
|
void GyroSensor::applyCalibration() {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Applying calibration offsets to sensor." CR));
|
|
#endif
|
|
|
|
if ((_calibrationOffset.ax + _calibrationOffset.ay + _calibrationOffset.az +
|
|
_calibrationOffset.gx + _calibrationOffset.gy + _calibrationOffset.gz) ==
|
|
0) {
|
|
writeErrorLog("GYRO: No valid calibration values, please calibrate the device.");
|
|
return;
|
|
}
|
|
|
|
accelgyro.setXAccelOffset(_calibrationOffset.ax);
|
|
accelgyro.setYAccelOffset(_calibrationOffset.ay);
|
|
accelgyro.setZAccelOffset(_calibrationOffset.az);
|
|
accelgyro.setXGyroOffset(_calibrationOffset.gx);
|
|
accelgyro.setYGyroOffset(_calibrationOffset.gy);
|
|
accelgyro.setZGyroOffset(_calibrationOffset.gz);
|
|
}
|
|
|
|
//
|
|
// Calculate the offsets for calibration.
|
|
//
|
|
void GyroSensor::calibrateSensor() {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Calibrating sensor" CR));
|
|
#endif
|
|
// accelgyro.PrintActiveOffsets();
|
|
// Serial.print( CR );
|
|
|
|
accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);
|
|
accelgyro.CalibrateAccel(6); // 6 = 600 readings
|
|
accelgyro.CalibrateGyro(6);
|
|
|
|
accelgyro.PrintActiveOffsets();
|
|
Serial.print(CR);
|
|
|
|
_calibrationOffset.ax = accelgyro.getXAccelOffset();
|
|
_calibrationOffset.ay = accelgyro.getYAccelOffset();
|
|
_calibrationOffset.az = accelgyro.getZAccelOffset();
|
|
_calibrationOffset.gx = accelgyro.getXGyroOffset();
|
|
_calibrationOffset.gy = accelgyro.getYGyroOffset();
|
|
_calibrationOffset.gz = accelgyro.getZGyroOffset();
|
|
|
|
// Save the calibrated values
|
|
myConfig.setGyroCalibration(_calibrationOffset);
|
|
myConfig.saveFile();
|
|
}
|
|
|
|
//
|
|
// Calibrate the device.
|
|
//
|
|
void GyroSensor::debug() {
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: Debug - Clock src %d." CR),
|
|
accelgyro.getClockSource());
|
|
Log.verbose(F("GYRO: Debug - Device ID %d." CR), accelgyro.getDeviceID());
|
|
Log.verbose(F("GYRO: Debug - DHPF Mode %d." CR), accelgyro.getDHPFMode());
|
|
Log.verbose(F("GYRO: Debug - DMP on %s." CR),
|
|
accelgyro.getDMPEnabled() ? "on" : "off");
|
|
Log.verbose(F("GYRO: Debug - Acc range %d." CR),
|
|
accelgyro.getFullScaleAccelRange());
|
|
Log.verbose(F("GYRO: Debug - Gyr range %d." CR),
|
|
accelgyro.getFullScaleGyroRange());
|
|
Log.verbose(F("GYRO: Debug - Int %s." CR),
|
|
accelgyro.getIntEnabled() ? "on" : "off");
|
|
Log.verbose(F("GYRO: Debug - Clock %d." CR),
|
|
accelgyro.getMasterClockSpeed());
|
|
Log.verbose(F("GYRO: Debug - Rate %d." CR), accelgyro.getRate());
|
|
Log.verbose(F("GYRO: Debug - Gyro range %d." CR),
|
|
accelgyro.getFullScaleGyroRange());
|
|
Log.verbose(F("GYRO: Debug - Acc FactX %d." CR),
|
|
accelgyro.getAccelXSelfTestFactoryTrim());
|
|
Log.verbose(F("GYRO: Debug - Acc FactY %d." CR),
|
|
accelgyro.getAccelYSelfTestFactoryTrim());
|
|
Log.verbose(F("GYRO: Debug - Acc FactZ %d." CR),
|
|
accelgyro.getAccelZSelfTestFactoryTrim());
|
|
Log.verbose(F("GYRO: Debug - Gyr FactX %d." CR),
|
|
accelgyro.getGyroXSelfTestFactoryTrim());
|
|
Log.verbose(F("GYRO: Debug - Gyr FactY %d." CR),
|
|
accelgyro.getGyroYSelfTestFactoryTrim());
|
|
Log.verbose(F("GYRO: Debug - Gyr FactZ %d." CR),
|
|
accelgyro.getGyroZSelfTestFactoryTrim());
|
|
|
|
switch (accelgyro.getFullScaleAccelRange()) {
|
|
case 0:
|
|
Log.verbose(F("GYRO: Debug - Accel range +/- 2g." CR));
|
|
break;
|
|
case 1:
|
|
Log.verbose(F("GYRO: Debug - Accel range +/- 4g." CR));
|
|
break;
|
|
case 2:
|
|
Log.verbose(F("GYRO: Debug - Accel range +/- 8g." CR));
|
|
break;
|
|
case 3:
|
|
Log.verbose(F("GYRO: Debug - Accel range +/- 16g." CR));
|
|
break;
|
|
}
|
|
|
|
Log.verbose(F("GYRO: Debug - Acc OffX %d\t%d." CR),
|
|
accelgyro.getXAccelOffset(), _calibrationOffset.az);
|
|
Log.verbose(F("GYRO: Debug - Acc OffY %d\t%d." CR),
|
|
accelgyro.getYAccelOffset(), _calibrationOffset.ay);
|
|
Log.verbose(F("GYRO: Debug - Acc OffZ %d\t%d." CR),
|
|
accelgyro.getZAccelOffset(), _calibrationOffset.az);
|
|
Log.verbose(F("GYRO: Debug - Gyr OffX %d\t%d." CR),
|
|
accelgyro.getXGyroOffset(), _calibrationOffset.gx);
|
|
Log.verbose(F("GYRO: Debug - Gyr OffY %d\t%d." CR),
|
|
accelgyro.getYGyroOffset(), _calibrationOffset.gy);
|
|
Log.verbose(F("GYRO: Debug - Gyr OffZ %d\t%d." CR),
|
|
accelgyro.getZGyroOffset(), _calibrationOffset.gz);
|
|
#endif
|
|
}
|
|
|
|
// EOF
|