410 lines
15 KiB
C++
410 lines
15 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 <helper.hpp>
|
|
|
|
GyroSensor myGyro;
|
|
|
|
#define GYRO_USE_INTERRUPT // Use interrupt to detect when new sample is ready
|
|
#define SENSOR_MOVING_THREASHOLD 500
|
|
#define SENSOR_READ_COUNT 50
|
|
#define SENSOR_READ_DELAY 3150 // us, empirical, to hold sampling to 200 Hz
|
|
|
|
#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(D3, D4);
|
|
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having
|
|
// compilation difficulties
|
|
accelgyro.initialize();
|
|
|
|
if (!accelgyro.testConnection()) {
|
|
Log.error(F("GYRO: Failed to connect to MPU6050 (gyro)." CR));
|
|
sensorConnected = false;
|
|
} else {
|
|
#if !defined(GYRO_DISABLE_LOGGING)
|
|
Log.notice(F("GYRO: Connected to MPU6050 (gyro)." CR));
|
|
#endif
|
|
sensorConnected = true;
|
|
|
|
// Configure the sensor
|
|
accelgyro.setTempSensorEnabled(true);
|
|
// accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // Set in
|
|
// .initalize() accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250); //
|
|
// Set in .initalize()
|
|
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.getRotation( &min.gx, &min.gy, &min.gz );
|
|
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.getRotation( &raw.gx, &raw.gy, &raw.gz );
|
|
// accelgyro.getAcceleration( &raw.ax, &raw.ay, &raw.az );
|
|
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 v = (acos(ay / sqrt(ax * ax + ay * ay + az * az)) * 180.0 / PI);
|
|
// Log.notice(F("GYRO: angle = %F." CR), v );
|
|
// double v = (acos( raw.az / sqrt( raw.ax*raw.ax + raw.ay*raw.ay +
|
|
// raw.az*raw.az ) ) *180.0 / PI); Log.notice(F("GYRO: angle = %F." CR), v );
|
|
|
|
#if LOG_LEVEL == 6 && !defined(GYRO_DISABLE_LOGGING)
|
|
Log.verbose(F("GYRO: angle = %F." CR), v);
|
|
#endif
|
|
return v;
|
|
}
|
|
|
|
//
|
|
// 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);
|
|
|
|
if (x > SENSOR_MOVING_THREASHOLD || y > SENSOR_MOVING_THREASHOLD ||
|
|
z > SENSOR_MOVING_THREASHOLD) {
|
|
Log.notice(F("GYRO: Movement detected (%d)\t%d\t%d\t%d." CR),
|
|
SENSOR_MOVING_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, SENSOR_READ_COUNT,
|
|
SENSOR_READ_DELAY); // 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 !defined(GYRO_DISABLE_LOGGING)
|
|
Log.notice(F("GYRO: Sensor is moving." CR));
|
|
#endif
|
|
validValue = false;
|
|
} else {
|
|
validValue = true;
|
|
angle = calculateAngle(lastGyroData);
|
|
#if !defined(GYRO_DISABLE_LOGGING)
|
|
Log.notice(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) {
|
|
Log.error(F("GYRO: No valid calibraion values exist, aborting." CR));
|
|
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 - I2C bypass %s." CR),
|
|
// accelgyro.getI2CBypassEnabled()?"on":"off" ); Log.verbose(F("GYRO: Debug -
|
|
// I2C master %s." CR), accelgyro.getI2CMasterModeEnabled()?"on":"off" );
|
|
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
|