dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added sensor offset setting

This commit is contained in:
Lorenz Meier 2012-08-27 23:43:56 +02:00
parent 2d2548e714
commit 36a8b00ab1
1 changed files with 41 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
apps/sensors/sensors.cpp
View File

@ -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];