dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Switch assignment: added manual and stabilized switch for a default stabilized switch scheme, let FW go into Acro

This commit is contained in:
Andreas Antener 2017-02-07 18:11:36 +01:00 committed by Andreas Daniel Antener
parent 21f5219bbc
commit a5f3f65c2b
8 changed files with 199 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/manual_control_setpoint.msg
View File

@ -54,10 +54,12 @@ uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUA
uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
uint8 kill_switch # throttle kill: _NORMAL_, KILL
uint8 arm_switch # arm/disarm switch: _DISARMED_, ARMED
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
uint8 gear_switch # landing gear switch: _DOWN_, UP
int8 mode_slot # the slot a specific model selector is in
uint8 data_source # where this input is coming from
uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL

7
msg/rc_channels.msg
View File

@ -1,4 +1,3 @@
int32 RC_CHANNELS_FUNCTION_MAX=21
uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
uint8 RC_CHANNELS_FUNCTION_ROLL=1
uint8 RC_CHANNELS_FUNCTION_PITCH=2
@ -23,11 +22,13 @@ uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
uint8 RC_CHANNELS_FUNCTION_TRANSITION=21
uint8 RC_CHANNELS_FUNCTION_GEAR=22
uint8 RC_CHANNELS_FUNCTION_ARMSWITCH=23
uint8 RC_CHANNELS_FUNCTION_STAB=24
uint8 RC_CHANNELS_FUNCTION_MAN=25
uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
uint8 channel_count # Number of valid channels
int8[24] function # Functions mapping
uint8 rssi # Receive signal strength indicator (0-100)
int8[26] function # Functions mapping
uint8 rssi # Receive signal strength index
bool signal_lost # Control signal lost, should be checked together with topic timeout
uint32 frame_drop_count # Number of dropped frames

79
src/modules/commander/commander.cpp
View File

@ -3321,7 +3321,9 @@ set_main_state_rc(struct vehicle_status_s *status_local)
(_last_sp_man.rattitude_switch == sp_man.rattitude_switch) &&
(_last_sp_man.posctl_switch == sp_man.posctl_switch) &&
(_last_sp_man.loiter_switch == sp_man.loiter_switch) &&
(_last_sp_man.mode_slot == sp_man.mode_slot)))) {
(_last_sp_man.mode_slot == sp_man.mode_slot) &&
(_last_sp_man.stab_switch == sp_man.stab_switch) &&
(_last_sp_man.man_switch == sp_man.man_switch)))) {
// update these fields for the geofence system
@ -3519,31 +3521,72 @@ set_main_state_rc(struct vehicle_status_s *status_local)
break;
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
/*
* Legacy mode:
* Acro switch being used as stabilized switch in FW.
*/
// XXX: put ACRO and STAB on separate switches
if (status.is_rotary_wing && !status.is_vtol) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state);
} else if (!status.is_rotary_wing) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
*/
if (status.is_rotary_wing && !status.is_vtol) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state);
} else if (!status.is_rotary_wing) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
} else {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
}
} else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state);
} else {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
}
} else {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
}
}
else if(sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state);
} else {
/* New mode:
* - Acro is Acro
* - Manual is not default anymore when the manaul switch is assigned
*/
if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
} else if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state);
} else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* Similar to acro transitions for multirotors. FW aircraft don't have a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state);
} else {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
}
} else if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
} else if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
// default to MANUAL when no manual switch is set
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
} else {
// default to STAB when the manual switch is assigned (but off)
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
}
}else {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
}
// TRANSITION_DENIED is not possible here

3
src/modules/commander/state_machine_helper.cpp
View File

@ -380,6 +380,9 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case commander_state_s::MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case commander_state_s::MAIN_STATE_RATTITUDE:
/* ACRO and RATTITUDE only implemented in MC */

18
src/modules/sensors/parameters.cpp
View File

@ -93,6 +93,8 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles)
parameter_handles.rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW");
parameter_handles.rc_map_stab_sw = param_find("RC_MAP_STAB_SW");
parameter_handles.rc_map_man_sw = param_find("RC_MAP_MAN_SW");
parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@ -124,6 +126,8 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles)
parameter_handles.rc_armswitch_th = param_find("RC_ARMSWITCH_TH");
parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
parameter_handles.rc_gear_th = param_find("RC_GEAR_TH");
parameter_handles.rc_stab_th = param_find("RC_STAB_TH");
parameter_handles.rc_man_th = param_find("RC_MAN_TH");
/* RC low pass filter configuration */
parameter_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE");
@ -339,6 +343,14 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_stab_sw, &(parameters.rc_map_stab_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_man_sw, &(parameters.rc_map_man_sw)) != OK) {
warnx("%s", paramerr);
}
param_get(parameter_handles.rc_map_aux1, &(parameters.rc_map_aux1));
param_get(parameter_handles.rc_map_aux2, &(parameters.rc_map_aux2));
param_get(parameter_handles.rc_map_aux3, &(parameters.rc_map_aux3));
@ -388,6 +400,12 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th));
parameters.rc_gear_inv = (parameters.rc_gear_th < 0);
parameters.rc_gear_th = fabs(parameters.rc_gear_th);
param_get(parameter_handles.rc_stab_th, &(parameters.rc_stab_th));
parameters.rc_stab_inv = (parameters.rc_stab_th < 0);
parameters.rc_stab_th = fabs(parameters.rc_stab_th);
param_get(parameter_handles.rc_man_th, &(parameters.rc_man_th));
parameters.rc_man_inv = (parameters.rc_man_th < 0);
parameters.rc_man_th = fabs(parameters.rc_man_th);
param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate));
parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate);

13
src/modules/sensors/parameters.h
View File

@ -87,7 +87,8 @@ struct Parameters {
int rc_map_arm_sw;
int rc_map_trans_sw;
int rc_map_gear_sw;
int rc_map_stab_sw;
int rc_map_man_sw;
int rc_map_flaps;
int rc_map_aux1;
@ -113,6 +114,9 @@ struct Parameters {
float rc_armswitch_th;
float rc_trans_th;
float rc_gear_th;
float rc_stab_th;
float rc_man_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_rattitude_inv;
@ -125,6 +129,8 @@ struct Parameters {
bool rc_armswitch_inv;
bool rc_trans_inv;
bool rc_gear_inv;
bool rc_stab_inv;
bool rc_man_inv;
float rc_flt_smp_rate;
float rc_flt_cutoff;
@ -169,8 +175,9 @@ struct ParameterHandles {
param_t rc_map_arm_sw;
param_t rc_map_trans_sw;
param_t rc_map_gear_sw;
param_t rc_map_flaps;
param_t rc_map_stab_sw;
param_t rc_map_man_sw;
param_t rc_map_aux1;
param_t rc_map_aux2;
@ -199,6 +206,8 @@ struct ParameterHandles {
param_t rc_armswitch_th;
param_t rc_trans_th;
param_t rc_gear_th;
param_t rc_stab_th;
param_t rc_man_th;
param_t rc_flt_smp_rate;
param_t rc_flt_cutoff;

6
src/modules/sensors/rc_update.cpp
View File

@ -103,6 +103,8 @@ void RCUpdate::update_rc_functions()
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH] = _parameters.rc_map_arm_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _parameters.rc_map_gear_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_STAB] = _parameters.rc_map_stab_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MAN] = _parameters.rc_map_man_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
@ -434,6 +436,10 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
_parameters.rc_trans_th, _parameters.rc_trans_inv);
manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
_parameters.rc_gear_th, _parameters.rc_gear_inv);
manual.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB,
_parameters.rc_stab_th, _parameters.rc_stab_inv);
manual.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN,
_parameters.rc_man_th, _parameters.rc_man_inv);
/* publish manual_control_setpoint topic */
orb_publish_auto(ORB_ID(manual_control_setpoint), &_manual_control_pub, &manual, &instance,

93
src/modules/sensors/sensor_params.c
View File

@ -2600,6 +2600,63 @@ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0);
/**
* Stabilize switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
/**
* Manual switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
/**
* AUX1 Passthrough RC Channel
*
@ -3068,6 +3125,42 @@ PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.25f);
*/
PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.25f);
/**
* Threshold for the stabilize switch.
*
* 0-1 indicate where in the full channel range the threshold sits
* 0 : min
* 1 : max
* sign indicates polarity of comparison
* positive : true when channel>th
* negative : true when channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*
*
*/
PARAM_DEFINE_FLOAT(RC_STAB_TH, 0.5f);
/**
* Threshold for the manual switch.
*
* 0-1 indicate where in the full channel range the threshold sits
* 0 : min
* 1 : max
* sign indicates polarity of comparison
* positive : true when channel>th
* negative : true when channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*
*
*/
PARAM_DEFINE_FLOAT(RC_MAN_TH, 0.5f);
/**
* PWM input channel that provides RSSI.
*