fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 18:39:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added gyro scaling as parameter

Browse Source
This commit is contained in:
Lorenz Meier 2013-07-05 11:44:10 +02:00
parent 7ca0698a6b
commit 5f2d35d715
2 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/sensors/sensor_params.c
View File

@ -48,6 +48,10 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);

14
src/modules/sensors/sensors.cpp
View File

@ -194,6 +194,7 @@ private:
float scaling_factor[_rc_max_chan_count];
float gyro_offset[3];
float gyro_scale[3];
float mag_offset[3];
float mag_scale[3];
float accel_offset[3];
@ -243,6 +244,7 @@ private:
param_t rc_demix;
param_t gyro_offset[3];
param_t gyro_scale[3];
param_t accel_offset[3];
param_t accel_scale[3];
param_t mag_offset[3];
@ -486,6 +488,9 @@ Sensors::Sensors() :
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
_parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE");
_parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE");
_parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE");
/* accel offsets */
_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
@ -696,6 +701,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0]));
param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1]));
param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2]));
/* accel offsets */
param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0]));
@ -983,11 +991,11 @@ Sensors::parameter_update_poll(bool forced)
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
_parameters.gyro_offset[0],
1.0f,
_parameters.gyro_scale[0],
_parameters.gyro_offset[1],
1.0f,
_parameters.gyro_scale[1],
_parameters.gyro_offset[2],
1.0f,
_parameters.gyro_scale[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))