diff options
Diffstat (limited to 'src/modules/sensors')
-rw-r--r-- | src/modules/sensors/sensor_params.c | 39 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 32 |
2 files changed, 61 insertions, 10 deletions
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b81588..763723554 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC9_MIN, 1000); PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); @@ -190,11 +190,30 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC16_MIN, 1000); +PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC16_MAX, 2000); +PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC17_MIN, 1000); +PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC17_MAX, 2000); +PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC18_MIN, 1000); +PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC18_MAX, 2000); +PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else @@ -226,3 +245,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); + +PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6d38b98ec..d9f037c27 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -165,7 +165,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 */ @@ -260,6 +260,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; } _parameters; /**< local copies of interesting parameters */ @@ -305,6 +309,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 board_rotation; @@ -517,6 +525,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"); @@ -589,7 +602,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])); @@ -668,6 +681,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; @@ -1256,6 +1272,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) |