405 lines
15 KiB
C++
405 lines
15 KiB
C++
// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW
|
|
// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net)
|
|
|
|
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
|
|
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
|
|
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
|
//
|
|
// Changelog:
|
|
// 2019-07-11 - added PID offset generation at begninning Generates first offsets
|
|
// - in @ 6 seconds and completes with 4 more sets @ 10 seconds
|
|
// - then continues with origional 2016 calibration code.
|
|
// 2016-11-25 - added delays to reduce sampling rate to ~200 Hz
|
|
// added temporizing printing during long computations
|
|
// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion
|
|
// dynamic speed change when closing in
|
|
// 2016-10-22 - cosmetic changes
|
|
// 2016-10-19 - initial release of IMU_Zero
|
|
// 2013-05-08 - added multiple output formats
|
|
// - added seamless Fastwire support
|
|
// 2011-10-07 - initial release of MPU6050_RAW
|
|
|
|
/* ============================================
|
|
I2Cdev device library code is placed under the MIT license
|
|
Copyright (c) 2011 Jeff Rowberg
|
|
|
|
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.
|
|
|
|
If an MPU6050
|
|
* is an ideal member of its tribe,
|
|
* is properly warmed up,
|
|
* is at rest in a neutral position,
|
|
* is in a location where the pull of gravity is exactly 1g, and
|
|
* has been loaded with the best possible offsets,
|
|
then it will report 0 for all accelerations and displacements, except for
|
|
Z acceleration, for which it will report 16384 (that is, 2^14). Your device
|
|
probably won't do quite this well, but good offsets will all get the baseline
|
|
outputs close to these target values.
|
|
|
|
Put the MPU6050 on a flat and horizontal surface, and leave it operating for
|
|
5-10 minutes so its temperature gets stabilized.
|
|
|
|
Run this program. A "----- done -----" line will indicate that it has done its best.
|
|
With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take
|
|
a few minutes to get there.
|
|
|
|
Along the way, it will generate a dozen or so lines of output, showing that for each
|
|
of the 6 desired offsets, it is
|
|
* first, trying to find two estimates, one too low and one too high, and
|
|
* then, closing in until the bracket can't be made smaller.
|
|
|
|
The line just above the "done" line will look something like
|
|
[567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4]
|
|
As will have been shown in interspersed header lines, the six groups making up this
|
|
line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration,
|
|
X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed
|
|
that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration,
|
|
and so on.
|
|
|
|
The need for the delay between readings (usDelay) was brought to my attention by Nikolaus Doppelhammer.
|
|
===============================================
|
|
*/
|
|
|
|
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
|
// for both classes must be in the include path of your project
|
|
#include "I2Cdev.h"
|
|
#include "MPU6050.h"
|
|
|
|
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
|
// is used in I2Cdev.h
|
|
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
|
#include "Wire.h"
|
|
#endif
|
|
|
|
// class default I2C address is 0x68
|
|
// specific I2C addresses may be passed as a parameter here
|
|
// AD0 low = 0x68 (default for InvenSense evaluation board)
|
|
// AD0 high = 0x69
|
|
MPU6050 accelgyro;
|
|
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
|
|
|
|
|
|
#include <BluetoothSerial.h>
|
|
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
|
|
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
|
|
#endif
|
|
|
|
BluetoothSerial SerialBT;
|
|
|
|
|
|
const char LBRACKET = '[';
|
|
const char RBRACKET = ']';
|
|
const char COMMA = ',';
|
|
const char BLANK = ' ';
|
|
const char PERIOD = '.';
|
|
|
|
const int iAx = 0;
|
|
const int iAy = 1;
|
|
const int iAz = 2;
|
|
const int iGx = 3;
|
|
const int iGy = 4;
|
|
const int iGz = 5;
|
|
|
|
const int usDelay = 3150; // empirical, to hold sampling to 200 Hz
|
|
const int NFast = 1000; // the bigger, the better (but slower)
|
|
const int NSlow = 10000; // ..
|
|
const int LinesBetweenHeaders = 5;
|
|
int LowValue[6];
|
|
int HighValue[6];
|
|
int Smoothed[6];
|
|
int LowOffset[6];
|
|
int HighOffset[6];
|
|
int Target[6];
|
|
int LinesOut;
|
|
int N;
|
|
|
|
void ForceHeader()
|
|
{ LinesOut = 99; }
|
|
|
|
void GetSmoothed()
|
|
{ int16_t RawValue[6];
|
|
int i;
|
|
long Sums[6];
|
|
for (i = iAx; i <= iGz; i++)
|
|
{ Sums[i] = 0; }
|
|
// unsigned long Start = micros();
|
|
|
|
for (i = 1; i <= N; i++)
|
|
{ // get sums
|
|
accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz],
|
|
&RawValue[iGx], &RawValue[iGy], &RawValue[iGz]);
|
|
if ((i % 500) == 0)
|
|
Serial.print(PERIOD);
|
|
SerialBT.print(PERIOD);
|
|
delayMicroseconds(usDelay);
|
|
for (int j = iAx; j <= iGz; j++)
|
|
Sums[j] = Sums[j] + RawValue[j];
|
|
} // get sums
|
|
// unsigned long usForN = micros() - Start;
|
|
// Serial.print(" reading at ");
|
|
// Serial.print(1000000/((usForN+N/2)/N));
|
|
// Serial.println(" Hz");
|
|
for (i = iAx; i <= iGz; i++)
|
|
{ Smoothed[i] = (Sums[i] + N/2) / N ; }
|
|
} // GetSmoothed
|
|
|
|
void Initialize()
|
|
{
|
|
// join I2C bus (I2Cdev library doesn't do this automatically)
|
|
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
|
Wire.begin();
|
|
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
|
Fastwire::setup(400, true);
|
|
#endif
|
|
|
|
Serial.begin(9600);
|
|
SerialBT.begin("WaveyWindCalibration"); //Bluetooth device name
|
|
|
|
|
|
// initialize device
|
|
Serial.println("Initializing I2C devices...");
|
|
SerialBT.println("Initializing I2C devices...");
|
|
accelgyro.initialize();
|
|
|
|
// verify connection
|
|
Serial.println("Testing device connections...");
|
|
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
|
|
Serial.println("PID tuning Each Dot = 100 readings");
|
|
SerialBT.println("Testing device connections...");
|
|
SerialBT.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
|
|
SerialBT.println("PID tuning Each Dot = 100 readings");
|
|
/*A tidbit on how PID (PI actually) tuning works.
|
|
When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and
|
|
integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral
|
|
uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it
|
|
to the integral value. Each reading narrows the error down to the desired offset. The greater the error from
|
|
set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the
|
|
integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the
|
|
noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100
|
|
readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to
|
|
the fact it reacts to any noise.
|
|
*/
|
|
accelgyro.CalibrateAccel(6);
|
|
accelgyro.CalibrateGyro(6);
|
|
Serial.println("\nat 600 Readings");
|
|
SerialBT.println("\nat 600 Readings");
|
|
accelgyro.PrintActiveOffsets();
|
|
Serial.println();
|
|
SerialBT.println();
|
|
accelgyro.CalibrateAccel(1);
|
|
accelgyro.CalibrateGyro(1);
|
|
Serial.println("700 Total Readings");
|
|
SerialBT.println("700 Total Readings");
|
|
accelgyro.PrintActiveOffsets();
|
|
Serial.println();
|
|
SerialBT.println();
|
|
accelgyro.CalibrateAccel(1);
|
|
accelgyro.CalibrateGyro(1);
|
|
Serial.println("800 Total Readings");
|
|
Serial.println("800 Total Readings");
|
|
accelgyro.PrintActiveOffsets();
|
|
Serial.println();
|
|
SerialBT.println();
|
|
accelgyro.CalibrateAccel(1);
|
|
accelgyro.CalibrateGyro(1);
|
|
Serial.println("900 Total Readings");
|
|
SerialBT.println("900 Total Readings");
|
|
accelgyro.PrintActiveOffsets();
|
|
Serial.println();
|
|
SerialBT.println();
|
|
accelgyro.CalibrateAccel(1);
|
|
accelgyro.CalibrateGyro(1);
|
|
Serial.println("1000 Total Readings");
|
|
SerialBT.println("1000 Total Readings");
|
|
accelgyro.PrintActiveOffsets();
|
|
Serial.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:");
|
|
SerialBT.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:");
|
|
} // Initialize
|
|
|
|
void SetOffsets(int TheOffsets[6])
|
|
{ accelgyro.setXAccelOffset(TheOffsets [iAx]);
|
|
accelgyro.setYAccelOffset(TheOffsets [iAy]);
|
|
accelgyro.setZAccelOffset(TheOffsets [iAz]);
|
|
accelgyro.setXGyroOffset (TheOffsets [iGx]);
|
|
accelgyro.setYGyroOffset (TheOffsets [iGy]);
|
|
accelgyro.setZGyroOffset (TheOffsets [iGz]);
|
|
} // SetOffsets
|
|
|
|
void ShowProgress()
|
|
{ if (LinesOut >= LinesBetweenHeaders)
|
|
{ // show header
|
|
Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro");
|
|
SerialBT.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro");
|
|
LinesOut = 0;
|
|
} // show header
|
|
Serial.print(BLANK);
|
|
SerialBT.print(BLANK);
|
|
for (int i = iAx; i <= iGz; i++)
|
|
{ Serial.print(LBRACKET);
|
|
Serial.print(LowOffset[i]),
|
|
Serial.print(COMMA);
|
|
Serial.print(HighOffset[i]);
|
|
Serial.print("] --> [");
|
|
Serial.print(LowValue[i]);
|
|
Serial.print(COMMA);
|
|
Serial.print(HighValue[i]);
|
|
SerialBT.print(LBRACKET);
|
|
SerialBT.print(LowOffset[i]),
|
|
SerialBT.print(COMMA);
|
|
SerialBT.print(HighOffset[i]);
|
|
SerialBT.print("] --> [");
|
|
SerialBT.print(LowValue[i]);
|
|
SerialBT.print(COMMA);
|
|
SerialBT.print(HighValue[i]);
|
|
if (i == iGz)
|
|
{
|
|
Serial.println(RBRACKET);
|
|
SerialBT.println(RBRACKET);
|
|
}
|
|
else
|
|
{
|
|
Serial.print("]\t");
|
|
SerialBT.print("]\t");
|
|
}
|
|
}
|
|
LinesOut++;
|
|
} // ShowProgress
|
|
|
|
void PullBracketsIn()
|
|
{ boolean AllBracketsNarrow;
|
|
boolean StillWorking;
|
|
int NewOffset[6];
|
|
|
|
Serial.println("\nclosing in:");
|
|
SerialBT.println("\nclosing in:");
|
|
AllBracketsNarrow = false;
|
|
ForceHeader();
|
|
StillWorking = true;
|
|
while (StillWorking)
|
|
{ StillWorking = false;
|
|
if (AllBracketsNarrow && (N == NFast))
|
|
{ SetAveraging(NSlow); }
|
|
else
|
|
{ AllBracketsNarrow = true; }// tentative
|
|
for (int i = iAx; i <= iGz; i++)
|
|
{ if (HighOffset[i] <= (LowOffset[i]+1))
|
|
{ NewOffset[i] = LowOffset[i]; }
|
|
else
|
|
{ // binary search
|
|
StillWorking = true;
|
|
NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2;
|
|
if (HighOffset[i] > (LowOffset[i] + 10))
|
|
{ AllBracketsNarrow = false; }
|
|
} // binary search
|
|
}
|
|
SetOffsets(NewOffset);
|
|
GetSmoothed();
|
|
for (int i = iAx; i <= iGz; i++)
|
|
{ // closing in
|
|
if (Smoothed[i] > Target[i])
|
|
{ // use lower half
|
|
HighOffset[i] = NewOffset[i];
|
|
HighValue[i] = Smoothed[i];
|
|
} // use lower half
|
|
else
|
|
{ // use upper half
|
|
LowOffset[i] = NewOffset[i];
|
|
LowValue[i] = Smoothed[i];
|
|
} // use upper half
|
|
} // closing in
|
|
ShowProgress();
|
|
} // still working
|
|
|
|
} // PullBracketsIn
|
|
|
|
void PullBracketsOut()
|
|
{ boolean Done = false;
|
|
int NextLowOffset[6];
|
|
int NextHighOffset[6];
|
|
|
|
Serial.println("expanding:");
|
|
SerialBT.println("expanding:");
|
|
ForceHeader();
|
|
|
|
while (!Done)
|
|
{ Done = true;
|
|
SetOffsets(LowOffset);
|
|
GetSmoothed();
|
|
for (int i = iAx; i <= iGz; i++)
|
|
{ // got low values
|
|
LowValue[i] = Smoothed[i];
|
|
if (LowValue[i] >= Target[i])
|
|
{ Done = false;
|
|
NextLowOffset[i] = LowOffset[i] - 1000;
|
|
}
|
|
else
|
|
{ NextLowOffset[i] = LowOffset[i]; }
|
|
} // got low values
|
|
|
|
SetOffsets(HighOffset);
|
|
GetSmoothed();
|
|
for (int i = iAx; i <= iGz; i++)
|
|
{ // got high values
|
|
HighValue[i] = Smoothed[i];
|
|
if (HighValue[i] <= Target[i])
|
|
{ Done = false;
|
|
NextHighOffset[i] = HighOffset[i] + 1000;
|
|
}
|
|
else
|
|
{ NextHighOffset[i] = HighOffset[i]; }
|
|
} // got high values
|
|
ShowProgress();
|
|
for (int i = iAx; i <= iGz; i++)
|
|
{ LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done
|
|
HighOffset[i] = NextHighOffset[i]; // ..
|
|
}
|
|
} // keep going
|
|
} // PullBracketsOut
|
|
|
|
void SetAveraging(int NewN)
|
|
{ N = NewN;
|
|
Serial.print("averaging ");
|
|
Serial.print(N);
|
|
Serial.println(" readings each time");
|
|
SerialBT.print("averaging ");
|
|
SerialBT.print(N);
|
|
SerialBT.println(" readings each time");
|
|
} // SetAveraging
|
|
|
|
void setup()
|
|
{ Initialize();
|
|
for (int i = iAx; i <= iGz; i++)
|
|
{ // set targets and initial guesses
|
|
Target[i] = 0; // must fix for ZAccel
|
|
HighOffset[i] = 0;
|
|
LowOffset[i] = 0;
|
|
} // set targets and initial guesses
|
|
Target[iAz] = 16384;
|
|
SetAveraging(NFast);
|
|
|
|
PullBracketsOut();
|
|
PullBracketsIn();
|
|
|
|
SerialBT.println("-------------- done --------------");
|
|
} // setup
|
|
|
|
void loop()
|
|
{
|
|
} // loop
|