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("------------")); }