Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

Allow disarm by RC in assisted modes if landed and in AUTO_READY state.

Browse Source
This commit is contained in:
Anton Babushkin
2013-08-24 20:31:01 +02:00
parent c42c28ebf4
commit 8579d0b7c9
1 changed files with 26 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+11 -14
View File
@@ -1017,12 +1017,15 @@ int commander_thread_main(int argc, char *argv[])
/* arm/disarm by RC */ /* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED; res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
* do it only for rotary wings */ * do it only for rotary wings */
if (status.is_rotary_wing && if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { (status.condition_landed && (
status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
status.navigation_state == NAVIGATION_STATE_VECTOR
))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
@@ -1032,18 +1035,14 @@ int commander_thread_main(int argc, char *argv[])
} else { } else {
stick_off_counter++; stick_off_counter++;
} }
stick_on_counter = 0;
} else { } else {
stick_off_counter = 0; stick_off_counter = 0;
} }
}
/* check if left stick is in lower right position and we're in manual mode -> arm */ /* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY && if (status.arming_state == ARMING_STATE_STANDBY &&
status.main_state == MAIN_STATE_MANUAL) { status.main_state == MAIN_STATE_MANUAL &&
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
stick_on_counter = 0; stick_on_counter = 0;
@@ -1052,12 +1051,9 @@ int commander_thread_main(int argc, char *argv[])
stick_on_counter++; stick_on_counter++;
} }
stick_off_counter = 0;
} else { } else {
stick_on_counter = 0; stick_on_counter = 0;
} }
}
if (res == TRANSITION_CHANGED) { if (res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) { if (status.arming_state == ARMING_STATE_ARMED) {
@@ -1702,7 +1698,8 @@ void *commander_low_prio_loop(void *arg)
/* ignore commands the high-prio loop handles */ /* ignore commands the high-prio loop handles */
if (cmd.command == VEHICLE_CMD_DO_SET_MODE || if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
continue; continue;
/* only handle low-priority commands here */ /* only handle low-priority commands here */