Имеем готовый код по калибровке гироскопа и акселерометра
#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
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 = 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);
delayMicroseconds(usDelay);
for (int j = iAx; j <= iGz; j++)
Sums
} // 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 = (Sums + 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);
// initialize device
Serial.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");
accelgyro.CalibrateAccel(6);
accelgyro.CalibrateGyro(6);
Serial.println("\nat 600 Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("700 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("800 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("900 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.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:");
} // 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");
LinesOut = 0;
} // show header
Serial.print(BLANK);
for (int i = iAx; i <= iGz; i++)
{ Serial.print(LBRACKET);
Serial.print(LowOffset),
Serial.print(COMMA);
Serial.print(HighOffset);
Serial.print("] --> [");
Serial.print(LowValue);
Serial.print(COMMA);
Serial.print(HighValue);
if (i == iGz)
{ Serial.println(RBRACKET); }
else
{ Serial.print("]\t"); }
}
LinesOut++;
} // ShowProgress
void PullBracketsIn()
{ boolean AllBracketsNarrow;
boolean StillWorking;
int NewOffset[6];
Serial.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 <= (LowOffset+1))
{ NewOffset = 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:");
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");
} // 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();
Serial.println("-------------- done --------------");
} // setup
void loop()
{
} // loop
Создать блок на основе этого кода для меня не проблема. Но! меня интересует, как правильно сделать, чтобы данный блок запускался при старте программы, до старта основной программы, и старт данного кода по требованию ( в FBD я знаю, как это сделать). Проблема моя в том. что тут использованы функции, которые прописаны до void setup();, а функция петли пустая. Видно, что вызов предыдущей функции есть в других последующих функциях.
Почему именно этот код калибровки?! Потому что этот код более точно делает калибровку по все осям, и калибровка сохраняется только пока на датчике есть питание. Другие скрипты не дают должного результата калибровки. использование setOffset() не дает точности.
Подытожу : возможен ли перенос функций в секцию LOOP и там использовать условие запуска кода?