diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-21 10:40:58 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-21 10:40:58 +0200 |
commit | 950571eaf6b67344b0c46287c45f06e8f85e8ece (patch) | |
tree | 8995f76f65a60851727c073d12b8dd2f3bcf0fc7 /src/modules/commander | |
parent | 8c4b35cc23ddf1eaef0dfc90f8fbb066b5b845af (diff) | |
parent | fb801b6faecd77fe2aac54d3389cacf73993ccc4 (diff) | |
download | px4-firmware-950571eaf6b67344b0c46287c45f06e8f85e8ece.tar.gz px4-firmware-950571eaf6b67344b0c46287c45f06e8f85e8ece.tar.bz2 px4-firmware-950571eaf6b67344b0c46287c45f06e8f85e8ece.zip |
Merge branch 'master' into offboard2
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 48 | ||||
-rw-r--r-- | src/modules/commander/px4_custom_mode.h | 1 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 4 |
3 files changed, 49 insertions, 4 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 96735d25e..90efd5115 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -456,6 +456,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { + /* ACRO */ + main_res = main_state_transition(status, MAIN_STATE_ACRO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); @@ -658,7 +662,8 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[1] = "ALTCTL"; main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; - main_states_str[4] = "OFFBOARD"; + main_states_str[4] = "ACRO"; + main_states_str[5] = "OFFBOARD"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1018,7 +1023,26 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_position_valid and condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); + /* hysteresis for EPH */ + bool local_eph_good; + + if (status.condition_global_position_valid) { + if (local_position.eph > eph_epv_threshold * 2.0f) { + local_eph_good = false; + + } else { + local_eph_good = true; + } + + } else { + if (local_position.eph < eph_epv_threshold) { + local_eph_good = true; + + } else { + local_eph_good = false; + } + } + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; @@ -1193,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1650,7 +1674,12 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_OFF: // MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (sp_man->acro_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_ACRO); + + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1763,6 +1792,17 @@ set_control_mode() navigator_enabled = true; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case MAIN_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 9589c9acb..04532d09a 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -15,6 +15,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_ALTCTL, PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_OFFBOARD, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index aaabde66b..55f73e043 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -222,6 +222,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; + case MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ |