From 36a8b00ab13e3185c1f968735fe028dc8626b642 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Aug 2012 23:43:56 +0200 Subject: Added sensor offset setting --- apps/sensors/sensors.cpp | 42 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) (limited to 'apps/sensors/sensors.cpp') 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]; -- cgit v1.2.3