Applied precommit cpp checks
This commit is contained in:
Magnus Persson
777
src/gyro.cpp
777
src/gyro.cpp
@ -1,368 +1,409 @@
|
||||
/*
|
||||
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
|
||||
/*
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user