|
|
@ -145,7 +145,7 @@ |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
static const int ERROR = -1; |
|
|
|
static const int ERROR = -1; |
|
|
|
|
|
|
|
|
|
|
|
#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL" |
|
|
|
#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" |
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* Sensor app start / stop handling function |
|
|
|
* Sensor app start / stop handling function |
|
|
@ -1382,12 +1382,12 @@ Sensors::parameter_update_poll(bool forced) |
|
|
|
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); |
|
|
|
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); |
|
|
|
|
|
|
|
|
|
|
|
if (failed) { |
|
|
|
if (failed) { |
|
|
|
warnx("%s: gyro #%u", CAL_FAILED_APPLY_CAL_MSG, gyro_count); |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
/* apply new scaling and offsets */ |
|
|
|
/* apply new scaling and offsets */ |
|
|
|
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); |
|
|
|
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); |
|
|
|
if (res) { |
|
|
|
if (res) { |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG); |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
gyro_count++; |
|
|
|
gyro_count++; |
|
|
|
config_ok = true; |
|
|
|
config_ok = true; |
|
|
@ -1449,12 +1449,12 @@ Sensors::parameter_update_poll(bool forced) |
|
|
|
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); |
|
|
|
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); |
|
|
|
|
|
|
|
|
|
|
|
if (failed) { |
|
|
|
if (failed) { |
|
|
|
warnx("%s: acc #%u", CAL_FAILED_APPLY_CAL_MSG, accel_count); |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
/* apply new scaling and offsets */ |
|
|
|
/* apply new scaling and offsets */ |
|
|
|
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); |
|
|
|
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); |
|
|
|
if (res) { |
|
|
|
if (res) { |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG); |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
accel_count++; |
|
|
|
accel_count++; |
|
|
|
config_ok = true; |
|
|
|
config_ok = true; |
|
|
@ -1566,12 +1566,12 @@ Sensors::parameter_update_poll(bool forced) |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if (failed) { |
|
|
|
if (failed) { |
|
|
|
warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count); |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
/* apply new scaling and offsets */ |
|
|
|
/* apply new scaling and offsets */ |
|
|
|
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); |
|
|
|
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); |
|
|
|
if (res) { |
|
|
|
if (res) { |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG); |
|
|
|
warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
mag_count++; |
|
|
|
mag_count++; |
|
|
|
config_ok = true; |
|
|
|
config_ok = true; |
|
|
|