fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 05:53:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+12 -6
View File
@@ -1649,6 +1649,13 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */ /* 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. * 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. * Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft. * Do this only for multirotors, not for fixed wing aircraft.
*/ */
if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR) // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
) && // ) &&
((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
(sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY); do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
stick_off_counter = 0; stick_off_counter = 0;
apps/commander/state_machine_helper.c
+9
View File
@@ -304,8 +304,11 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
bool valid_transition = false; bool valid_transition = false;
int ret = ERROR; int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state);
if (current_status->arming_state == new_state) { if (current_status->arming_state == new_state) {
warnx("Arming state not changed"); warnx("Arming state not changed");
valid_transition = true;
} else { } else {
@@ -333,6 +336,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
} else { } else {
mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); 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; break;
apps/mavlink_onboard/mavlink.c
+1 -1
View File
@@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
} }
/* set arming state */ /* set arming state */
if (armed->armed) { if (v_status->flag_system_armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else { } else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;