forked from Archive/PX4-Autopilot
renamed mission_switch to loiter_switch and assisted_switch to easy_switch
This commit is contained in:
parent
d6e6ee3440
commit
7e621070ca
|
@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
} else {
|
||||
/* MISSION switch */
|
||||
if (sp_man.mission_switch == SWITCH_POS_ON) {
|
||||
if (sp_man.loiter_switch == SWITCH_POS_ON) {
|
||||
/* stick is in LOITER position */
|
||||
status.set_nav_state = NAV_STATE_LOITER;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
|
||||
} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
|
||||
} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
|
||||
/* stick is in MISSION position */
|
||||
status.set_nav_state = NAV_STATE_MISSION;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
|
@ -1296,7 +1296,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
// TODO remove this hack
|
||||
/* flight termination in manual mode if assisted switch is on easy position */
|
||||
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
|
||||
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) {
|
||||
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
|
||||
tune_positive(armed.armed);
|
||||
}
|
||||
|
@ -1528,7 +1528,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
|||
break;
|
||||
|
||||
case SWITCH_POS_MIDDLE: // ASSISTED
|
||||
if (sp_man->assisted_switch == SWITCH_POS_ON) {
|
||||
if (sp_man->easy_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status, MAIN_STATE_EASY);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
|
@ -1545,7 +1545,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
|||
break; // changed successfully or already in this mode
|
||||
}
|
||||
|
||||
if (sp_man->assisted_switch != SWITCH_POS_ON) {
|
||||
if (sp_man->easy_switch != SWITCH_POS_ON) {
|
||||
print_reject_mode(status, "SEATBELT");
|
||||
}
|
||||
|
||||
|
|
|
@ -620,7 +620,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
|
|||
* @max 18
|
||||
* @group Radio Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
|
||||
PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0);
|
||||
|
||||
/**
|
||||
* Mission switch channel mapping.
|
||||
|
@ -629,7 +629,7 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
|
|||
* @max 18
|
||||
* @group Radio Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
|
||||
PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
|
||||
|
||||
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
|
||||
|
||||
|
|
|
@ -253,8 +253,8 @@ private:
|
|||
|
||||
int rc_map_mode_sw;
|
||||
int rc_map_return_sw;
|
||||
int rc_map_assisted_sw;
|
||||
int rc_map_mission_sw;
|
||||
int rc_map_easy_sw;
|
||||
int rc_map_loiter_sw;
|
||||
|
||||
// int rc_map_offboard_ctrl_mode_sw;
|
||||
|
||||
|
@ -296,8 +296,8 @@ private:
|
|||
|
||||
param_t rc_map_mode_sw;
|
||||
param_t rc_map_return_sw;
|
||||
param_t rc_map_assisted_sw;
|
||||
param_t rc_map_mission_sw;
|
||||
param_t rc_map_easy_sw;
|
||||
param_t rc_map_loiter_sw;
|
||||
|
||||
// param_t rc_map_offboard_ctrl_mode_sw;
|
||||
|
||||
|
@ -507,8 +507,8 @@ Sensors::Sensors() :
|
|||
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
|
||||
|
||||
/* optional mode switches, not mapped per default */
|
||||
_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
|
||||
_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
|
||||
_parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW");
|
||||
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
|
||||
|
||||
// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
|
||||
|
||||
|
@ -650,11 +650,11 @@ Sensors::parameters_update()
|
|||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
|
||||
if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) {
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
|
||||
if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
|
@ -681,8 +681,8 @@ Sensors::parameters_update()
|
|||
|
||||
_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
|
||||
_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
|
||||
_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
|
||||
_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
|
||||
_rc.function[EASY] = _parameters.rc_map_easy_sw - 1;
|
||||
_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
|
||||
|
||||
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
|
||||
|
||||
|
@ -1415,8 +1415,8 @@ Sensors::rc_poll()
|
|||
|
||||
/* mode switches */
|
||||
manual.mode_switch = get_rc_switch_position(MODE);
|
||||
manual.assisted_switch = get_rc_switch_position(ASSISTED);
|
||||
manual.mission_switch = get_rc_switch_position(MISSION);
|
||||
manual.easy_switch = get_rc_switch_position(EASY);
|
||||
manual.loiter_switch = get_rc_switch_position(LOITER);
|
||||
manual.return_switch = get_rc_switch_position(RETURN);
|
||||
|
||||
/* publish manual_control_setpoint topic */
|
||||
|
|
|
@ -78,8 +78,8 @@ struct manual_control_setpoint_s {
|
|||
|
||||
switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
|
||||
switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */
|
||||
switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
|
||||
switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */
|
||||
switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
|
||||
switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */
|
||||
}; /**< manual control inputs */
|
||||
|
||||
/**
|
||||
|
|
|
@ -64,8 +64,8 @@ enum RC_CHANNELS_FUNCTION {
|
|||
YAW = 3,
|
||||
MODE = 4,
|
||||
RETURN = 5,
|
||||
ASSISTED = 6,
|
||||
MISSION = 7,
|
||||
EASY = 6,
|
||||
LOITER = 7,
|
||||
OFFBOARD_MODE = 8,
|
||||
FLAPS = 9,
|
||||
AUX_1 = 10,
|
||||
|
|
Loading…
Reference in New Issue