forked from Archive/PX4-Autopilot
Simplify the failsafe handling, reduce 3 params to one
This commit is contained in:
parent
1ecd3e9291
commit
fb44ad8e22
|
@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Failsafe channel mapping.
|
||||
*
|
||||
* @min 0
|
||||
* @max 18
|
||||
* @group Radio Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_FS_CH, 0);
|
||||
|
||||
/**
|
||||
* Failsafe channel mode.
|
||||
*
|
||||
* 0 = too low means signal loss,
|
||||
* 1 = too high means signal loss
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Radio Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_FS_MODE, 0);
|
||||
|
||||
/**
|
||||
* Failsafe channel PWM threshold.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @min 800
|
||||
* @max 2200
|
||||
* @group Radio Calibration
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
|
||||
PARAM_DEFINE_FLOAT(RC_FS_THR, 0);
|
||||
|
|
|
@ -263,8 +263,6 @@ 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;
|
||||
|
@ -313,8 +311,6 @@ 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;
|
||||
|
@ -531,8 +527,6 @@ Sensors::Sensors() :
|
|||
_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 */
|
||||
|
@ -689,8 +683,6 @@ 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 */
|
||||
|
@ -1312,19 +1304,15 @@ Sensors::rc_poll()
|
|||
manual_control.aux5 = NAN;
|
||||
|
||||
/* require at least four channels to consider the signal valid */
|
||||
if (rc_input.channel_count < 4)
|
||||
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;
|
||||
}
|
||||
/* check for failsafe */
|
||||
if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
|
||||
|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) {
|
||||
/* do not publish manual control setpoints when there are none */
|
||||
return;
|
||||
}
|
||||
|
||||
unsigned channel_limit = rc_input.channel_count;
|
||||
|
|
Loading…
Reference in New Issue