diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-07 15:34:13 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-07 15:34:13 +0100 |
commit | 9b711d6a722fc254bb0d461b9aec66d20fbf59b1 (patch) | |
tree | e4e1cea58709e8513da969cf356de9917dd12f18 /src/modules/sensors/sensors.cpp | |
parent | 2a2c8337e8a01c59a542c8dd3dc77a087b34e3c2 (diff) | |
parent | 94b539dfddc5a2e293f51058ee5bf0d6ffc78406 (diff) | |
download | px4-firmware-9b711d6a722fc254bb0d461b9aec66d20fbf59b1.tar.gz px4-firmware-9b711d6a722fc254bb0d461b9aec66d20fbf59b1.tar.bz2 px4-firmware-9b711d6a722fc254bb0d461b9aec66d20fbf59b1.zip |
Merged origin/master into batt_fixes
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
-rw-r--r-- | src/modules/sensors/sensors.cpp | 32 |
1 files changed, 30 insertions, 2 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f205ff8f5..ff6c5882e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -164,7 +164,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ @@ -262,6 +262,10 @@ private: float rc_scale_yaw; float rc_scale_flaps; + int rc_fs_ch; + int rc_fs_mode; + float rc_fs_thr; + float battery_voltage_scaling; float battery_current_scaling; @@ -308,6 +312,10 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; + param_t rc_fs_ch; + param_t rc_fs_mode; + param_t rc_fs_thr; + param_t battery_voltage_scaling; param_t battery_current_scaling; @@ -523,6 +531,11 @@ Sensors::Sensors() : _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); + /* RC failsafe */ + _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); + _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); + _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); @@ -596,7 +609,7 @@ Sensors::parameters_update() float tmpRevFactor = 0.0f; /* rc values */ - for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { + for (unsigned int i = 0; i < _rc_max_chan_count; i++) { param_get(_parameter_handles.min[i], &(_parameters.min[i])); param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); @@ -675,6 +688,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); + param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); + param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); + param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -1288,6 +1304,18 @@ Sensors::rc_poll() if (rc_input.channel_count < 4) return; + /* failsafe check */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) + return; + + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) + return; + } + } + unsigned channel_limit = rc_input.channel_count; if (channel_limit > _rc_max_chan_count) |