diff options
Diffstat (limited to 'apps/sensors')
-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]; |