368 lines
14 KiB
C++
368 lines
14 KiB
C++
/*
|
|
MIT License
|
|
|
|
Copyright (c) 2021 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.h"
|
|
#include "helper.h"
|
|
|
|
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 = ((float) raw.ax)/16384,
|
|
ay = ((float) raw.ay)/16384,
|
|
az = ((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 = ((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
|