diff options
| author | Julian Oes <joes@student.ethz.ch> | 2013-02-18 16:35:34 -0800 |
|---|---|---|
| committer | Julian Oes <joes@student.ethz.ch> | 2013-02-18 16:35:34 -0800 |
| commit | b7faaca435551064e6fdb070a9e762b4146ae4e8 (patch) | |
| tree | 52f790046a396d523d5a0ffb46a18617766d5242 /apps | |
| parent | f8326300e89c63c57662868d6253b06a13621401 (diff) | |
| download | px4-firmware-b7faaca435551064e6fdb070a9e762b4146ae4e8.tar.gz px4-firmware-b7faaca435551064e6fdb070a9e762b4146ae4e8.tar.bz2 px4-firmware-b7faaca435551064e6fdb070a9e762b4146ae4e8.zip | |
Checkpoint: Arming/Disarming works
Diffstat (limited to 'apps')
| -rw-r--r-- | apps/commander/commander.c | 18 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.c | 9 | ||||
| -rw-r--r-- | apps/mavlink_onboard/mavlink.c | 2 |
3 files changed, 22 insertions, 7 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 27c5f1989..ac535dd9a 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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, ¤t_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, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 68b4bbe30..f1de99e4d 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 6babe14ae..c5a1e82a8 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -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; |