int set_cal(int mag, char *cal_file)
{
int i;
FILE *f;
char buff[32];
long val[6];
caldata_t cal;
if (cal_file) {
f = fopen(cal_file, "r");
if (!f) {
perror("open(<cal-file>)");
return -1;
}
}
else {
if (mag) {
f = fopen("./magcal.txt", "r");
if (!f) {
printf("Default magcal.txt not found\n");
return 0;
}
}
else {
f = fopen("./accelcal.txt", "r");
if (!f) {
printf("Default accelcal.txt not found\n");
return 0;
}
}
}
memset(buff, 0, sizeof(buff));
for (i = 0; i < 6; i++) {
if (!fgets(buff, 20, f)) {
printf("Not enough lines in calibration file\n");
break;
}
val[i] = atoi(buff);
if (val[i] == 0) {
printf("Invalid cal value: %s\n", buff);
break;
}
}
fclose(f);
if (i != 6)
return -1;
cal.offset[0] = (short)((val[0] + val[1]) / 2);
cal.offset[1] = (short)((val[2] + val[3]) / 2);
cal.offset[2] = (short)((val[4] + val[5]) / 2);
cal.range[0] = (short)(val[1] - cal.offset[0]);
cal.range[1] = (short)(val[3] - cal.offset[1]);
cal.range[2] = (short)(val[5] - cal.offset[2]);
if (mag)
mpu9150_set_mag_cal(&cal);
else
mpu9150_set_accel_cal(&cal);
return 0;
}
int load_calibration(int mag, std::string cal_file)
{
int i;
FILE *f;
char buff[32];
long val[6];
caldata_t cal;
f = fopen(cal_file.c_str(), "r");
if (!f)
{
ROS_ERROR("am_mpu9150::load_calibration:: Failed to open file %s", cal_file.c_str());
perror("File problem:");
return -1;
}
memset(buff, 0, sizeof(buff));
for (i = 0; i < 6; i++)
{
if (!fgets(buff, 20, f))
{
ROS_ERROR("am_mpu9150::load_calibration::Not enough lines in calibration file\n");
break;
}
val[i] = atoi(buff);
if (val[i] == 0)
{
ROS_ERROR("am_mpu9150::load_calibration::Invalid cal value: %s\n", buff);
break;
}
}
fclose(f);
if (i != 6)
{
ROS_ERROR("am_mpu9150::load_calibration:: Failed to load calibration!");
return -1;
}
// Store calibration
cal.offset[0] = (short)((val[0] + val[1]) / 2);
cal.offset[1] = (short)((val[2] + val[3]) / 2);
cal.offset[2] = (short)((val[4] + val[5]) / 2);
cal.range[0] = (short)(val[1] - cal.offset[0]);
cal.range[1] = (short)(val[3] - cal.offset[1]);
cal.range[2] = (short)(val[5] - cal.offset[2]);
if (mag)
{
mpu9150_set_mag_cal(&cal);
}
else
{
mpu9150_set_accel_cal(&cal);
}
return 0;
}