diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-07 10:59:43 -0700 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-07 10:59:43 -0700 |
| commit | cf2dbdf9a1ae06c7d0e0a7963916a3709a1bc075 (patch) | |
| tree | 79cb506ef7dbc1a7c1045f68b5ce8c169683c41e /src/modules/sensors/sensors.cpp | |
| parent | 7cf121472e7ba3d83084083792b0f159f238a8ef (diff) | |
| parent | 422c675c551c4a160e8bcdb18ffe3c6160b63980 (diff) | |
| download | px4-firmware-cf2dbdf9a1ae06c7d0e0a7963916a3709a1bc075.tar.gz px4-firmware-cf2dbdf9a1ae06c7d0e0a7963916a3709a1bc075.tar.bz2 px4-firmware-cf2dbdf9a1ae06c7d0e0a7963916a3709a1bc075.zip | |
Merge pull request #320 from PX4/integration
NuttX integration merge
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
| -rw-r--r-- | src/modules/sensors/sensors.cpp | 19 |
1 files changed, 14 insertions, 5 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6b6aeedee..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)) @@ -1445,7 +1453,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_spawn("sensors_task", + _sensors_task = task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, @@ -1502,6 +1510,7 @@ int sensors_main(int argc, char *argv[]) } } - errx(1, "unrecognized command"); + warnx("unrecognized command"); + return 1; } |