fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 08:04:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Checkpoint: Arming/Disarming works

Browse Source
This commit is contained in:
Julian Oes 2013-02-18 16:35:34 -08:00
parent f8326300e8
commit b7faaca435
3 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
apps/commander/commander.c
View File

@ -1649,6 +1649,13 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (current_status.arming_state == ARMING_STATE_INIT) {
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else {
// XXX: Add emergency stuff if sensors are lost
}
/*
* Check for valid position information.
@ -1894,12 +1901,11 @@ int commander_thread_main(int argc, char *argv[])
* Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft.
*/
if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
) &&
((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
(sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
// ) &&
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
stick_off_counter = 0;

9
apps/commander/state_machine_helper.c
View File

@ -304,8 +304,11 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
bool valid_transition = false;
int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state);
if (current_status->arming_state == new_state) {
warnx("Arming state not changed");
valid_transition = true;
} else {
@ -333,6 +336,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
} else {
mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
}
} else if (current_status->arming_state == ARMING_STATE_ARMED) {
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
valid_transition = true;
}
break;

2
apps/mavlink_onboard/mavlink.c
View File

@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
}
/* set arming state */
if (armed->armed) {
if (v_status->flag_system_armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;