diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-27 23:43:56 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-27 23:43:56 +0200 |
commit | 36a8b00ab13e3185c1f968735fe028dc8626b642 (patch) | |
tree | 24dca2dbda5b66fcd1c6be4af0213963288cad70 /apps/sensors/sensors.cpp | |
parent | 2d2548e714505b1a9d1074c0f9a78c44c8363314 (diff) | |
download | px4-firmware-36a8b00ab13e3185c1f968735fe028dc8626b642.tar.gz px4-firmware-36a8b00ab13e3185c1f968735fe028dc8626b642.tar.bz2 px4-firmware-36a8b00ab13e3185c1f968735fe028dc8626b642.zip |
Added sensor offset setting
Diffstat (limited to 'apps/sensors/sensors.cpp')
-rw-r--r-- | apps/sensors/sensors.cpp | 42 |
1 files changed, 41 insertions, 1 deletions
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index bcde4b14f..98b476814 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -750,7 +750,47 @@ Sensors::parameter_update_poll() /* update parameters */ parameters_update(); - /* Update RC scalings and function mappings */ + /* update sensor offsets */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + _parameters.gyro_offset[0], + 1.0f, + _parameters.gyro_offset[1], + 1.0f, + _parameters.gyro_offset[2], + 1.0f, + }; + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + close(fd); + + fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + _parameters.acc_offset[0], + 1.0f, + _parameters.acc_offset[1], + 1.0f, + _parameters.acc_offset[2], + 1.0f, + }; + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + close(fd); + + fd = open(MAG_DEVICE_PATH, 0); + struct mag_scale mscale = { + _parameters.mag_offset[0], + 1.0f, + _parameters.mag_offset[1], + 1.0f, + _parameters.mag_offset[2], + 1.0f, + }; + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + close(fd); + + /* update RC scalings and function mappings */ _rc.chan[0].scaling_factor = (1.0f / ((_parameters.max[0] - _parameters.min[0]) / 2.0f) * _parameters.rev[0]); _rc.chan[0].mid = _parameters.trim[0]; |