diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-05 11:44:10 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-05 11:44:10 +0200 |
| commit | 5f2d35d7150d9ae5d72eba008f785aee65a513c4 (patch) | |
| tree | 090e437f4585b27e9650b2bb30815e623da99e19 /src/modules/sensors/sensors.cpp | |
| parent | 7ca0698a6b41af500e4070717ef4d46a9b805d1f (diff) | |
| download | px4-firmware-5f2d35d7150d9ae5d72eba008f785aee65a513c4.tar.gz px4-firmware-5f2d35d7150d9ae5d72eba008f785aee65a513c4.tar.bz2 px4-firmware-5f2d35d7150d9ae5d72eba008f785aee65a513c4.zip | |
Added gyro scaling as parameter
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
| -rw-r--r-- | src/modules/sensors/sensors.cpp | 14 |
1 files changed, 11 insertions, 3 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 94998f5c3..1ded14a91 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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)) |