diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 09:53:45 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 09:53:45 +0100 |
commit | 62a95bf8e6592b31ae7e84e53b654bc5e6b71cd1 (patch) | |
tree | 07d27e3f0c56bb34c2d505fdafcce4490bf1b17e /apps/commander | |
parent | 0298714db53c44a28ab19d5ba01d70de28dd39b3 (diff) | |
download | px4-firmware-62a95bf8e6592b31ae7e84e53b654bc5e6b71cd1.tar.gz px4-firmware-62a95bf8e6592b31ae7e84e53b654bc5e6b71cd1.tar.bz2 px4-firmware-62a95bf8e6592b31ae7e84e53b654bc5e6b71cd1.zip |
Stabilization enabling / switching
Diffstat (limited to 'apps/commander')
-rw-r--r-- | apps/commander/commander.c | 117 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 21 |
2 files changed, 79 insertions, 59 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index be50289c3..ab50bab16 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1684,63 +1684,63 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.manual_mode_switch)) { - printf("man mode sw not finite\n"); - - /* this switch is not properly mapped, set default */ - if ((current_status.system_type == MAV_TYPE_QUADROTOR) || - (current_status.system_type == MAV_TYPE_HEXAROTOR) || - (current_status.system_type == MAV_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, fall back to SAS */ - current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - - /* assuming a fixed wing, fall back to direct pass-through */ - current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set direct mode for vehicles supporting it */ - if ((current_status.system_type == MAV_TYPE_QUADROTOR) || - (current_status.system_type == MAV_TYPE_HEXAROTOR) || - (current_status.system_type == MAV_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, fall back to SAS */ - current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - - /* assuming a fixed wing, set to direct pass-through as requested */ - current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } + // /* + // * Check if manual control modes have to be switched + // */ + // if (!isfinite(sp_man.manual_mode_switch)) { + // printf("man mode sw not finite\n"); + + // /* this switch is not properly mapped, set default */ + // if ((current_status.system_type == MAV_TYPE_QUADROTOR) || + // (current_status.system_type == MAV_TYPE_HEXAROTOR) || + // (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, fall back to direct pass-through */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + + // /* bottom stick position, set direct mode for vehicles supporting it */ + // if ((current_status.system_type == MAV_TYPE_QUADROTOR) || + // (current_status.system_type == MAV_TYPE_HEXAROTOR) || + // (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, set to direct pass-through as requested */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + + // /* top stick position, set SAS for all vehicle types */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; - } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set SAS for all vehicle types */ - current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - - } else { - /* center stick position, set rate control */ - current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = true; - } + // } else { + // /* center stick position, set rate control */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = true; + // } - printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + // printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* * Check if manual stability control modes have to be switched @@ -1801,7 +1801,7 @@ int commander_thread_main(int argc, char *argv[]) if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { /* enable manual override */ update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - } else { + } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { /* check auto mode switch for correct mode */ if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { /* enable stabilized mode */ @@ -1813,6 +1813,11 @@ int commander_thread_main(int argc, char *argv[]) } else { update_state_machine_mode_hold(stat_pub, ¤t_status, mavlink_fd); } + } else { + /* center stick position, set SAS for all vehicle types */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; } /* handle the case where RC signal was regained */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a8cef8c01..f30fd975b 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -520,9 +520,24 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; current_status->flag_control_manual_enabled = true; - /* enable attitude control per default */ - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; + + /* set behaviour based on airframe */ + if ((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, set to SAS */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { + + /* assuming a fixed wing, set to direct pass-through */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { |