void IMU::printCalib(){
Console.println(F("--------"));
Console.print(F("accOfs="));
printPt(accOfs);
Console.print(F("accScale="));
printPt(accScale);
Console.print(F("comOfs="));
printPt(comOfs);
Console.print(F("comScale="));
printPt(comScale);
Console.println(F("--------"));
}
int main(int argc, char **argv)
{
double radius=1;
int Nchl=2;
channel_coordinate graph(Nchl,radius);
matrix m= graph.coordinates();
std::ofstream chal_dist;
chal_dist.open("channel_coordinates.txt");
//std::fill(m.data().begin(), m.data().end(), 1);
//using namespace karma;
//chal_dist<<format(auto_ % '\t' % eol,make_view(m)) << "\n";
printPt(chal_dist, m);
chal_dist.close();
return 0;
}
// calculate gyro offsets
void IMU::calibGyro(){
Console.println(F("---calibGyro---"));
useGyroCalibration = false;
gyroOfs.x = gyroOfs.y = gyroOfs.z = 0;
point_float_t ofs;
while(true){
float zmin = 99999;
float zmax = -99999;
gyroNoise = 0;
ofs.x = ofs.y = ofs.z = 0;
for (int i=0; i < 50; i++){
delay(10);
readL3G4200D(true);
zmin = min(zmin, gyro.z);
zmax = max(zmax, gyro.z);
ofs.x += ((float)gyro.x)/ 50.0;
ofs.y += ((float)gyro.y)/ 50.0;
ofs.z += ((float)gyro.z)/ 50.0;
gyroNoise += sq(gyro.z-gyroOfs.z) /50.0; // noise is computed with last offset calculation
}
Console.print(F("gyro calib min="));
Console.print(zmin);
Console.print(F("\tmax="));
Console.print(zmax);
Console.print(F("\tofs="));
Console.print(ofs.z);
Console.print(F("\tnoise="));
Console.println(gyroNoise);
if (gyroNoise < 20) break; // optimum found
gyroOfs = ofs; // new offset found
}
useGyroCalibration = true;
Console.print(F("counter="));
Console.println(gyroCounter);
Console.print(F("ofs="));
printPt(gyroOfs);
Console.println(F("------------"));
}
