zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Sensors: Support single channel flight mode selection

sbg
Lorenz Meier 9 years ago
parent
4e36973cee
commit
1c21a63101
2 changed files with 68 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 31
      src/modules/sensors/sensor_params.c
  2. 57
      src/modules/sensors/sensors.cpp

31
src/modules/sensors/sensor_params.c
Unescape View File

@ -1941,6 +1941,37 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0); @@ -1941,6 +1941,37 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_YAW, 0);
/**
* Single channel flight mode selection
*
* If this parameter is non-zero, flight modes are only selected
* by this channel and are assigned to six slots.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 RC Channel 1
* @value 2 RC Channel 2
* @value 3 RC Channel 3
* @value 4 RC Channel 4
* @value 5 RC Channel 5
* @value 6 RC Channel 6
* @value 7 RC Channel 7
* @value 8 RC Channel 8
* @value 9 RC Channel 9
* @value 10 RC Channel 10
* @value 11 RC Channel 11
* @value 12 RC Channel 12
* @value 13 RC Channel 13
* @value 14 RC Channel 14
* @value 15 RC Channel 15
* @value 16 RC Channel 16
* @value 17 RC Channel 17
* @value 18 RC Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_FLTMODE, 0);
/**
* Mode switch channel mapping.
*

57
src/modules/sensors/sensors.cpp
Unescape View File

@ -298,6 +298,8 @@ private: @@ -298,6 +298,8 @@ private:
int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
int rc_map_flightmode;
int32_t rc_fails_thr;
float rc_assist_th;
float rc_auto_th;
@ -364,6 +366,8 @@ private: @@ -364,6 +366,8 @@ private:
_parameters struct are not existing
because these parameters are never read. */
param_t rc_map_flightmode;
param_t rc_fails_thr;
param_t rc_assist_th;
param_t rc_auto_th;
@ -600,6 +604,8 @@ Sensors::Sensors() : @@ -600,6 +604,8 @@ Sensors::Sensors() :
_parameter_handles.rc_map_param[i] = param_find(name);
}
_parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE");
/* RC thresholds */
_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
@ -790,6 +796,8 @@ Sensors::parameters_update() @@ -790,6 +796,8 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
}
param_get(_parameter_handles.rc_map_flightmode, &(_parameters.rc_map_flightmode));
param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
@ -1870,8 +1878,7 @@ Sensors::rc_poll() @@ -1870,8 +1878,7 @@ Sensors::rc_poll()
}
if (!signal_lost) {
struct manual_control_setpoint_s manual;
memset(&manual, 0 , sizeof(manual));
struct manual_control_setpoint_s manual = {};
/* fill values in manual_control_setpoint topic only if signal is valid */
manual.timestamp = rc_input.timestamp_last_signal;
@ -1888,24 +1895,34 @@ Sensors::rc_poll() @@ -1888,24 +1895,34 @@ Sensors::rc_poll()
manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
_parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
_parameters.rc_rattitude_th,
_parameters.rc_rattitude_inv);
manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
_parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,
_parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th,
_parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th,
_parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
_parameters.rc_offboard_th, _parameters.rc_offboard_inv);
manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
if (_parameters.rc_map_flightmode > 0) {
const unsigned slot_min = 1000;
const unsigned slot_max = 2000;
const unsigned num_slots = 6;
manual.mode_slot = ((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) / (slot_max - slot_min);
} else {
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
_parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
_parameters.rc_rattitude_th,
_parameters.rc_rattitude_inv);
manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
_parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,
_parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th,
_parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th,
_parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
_parameters.rc_offboard_th, _parameters.rc_offboard_inv);
manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
}
/* publish manual_control_setpoint topic */
if (_manual_control_pub != nullptr) {

Write Preview
Loading…
Cancel
Save