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 | |
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')
-rw-r--r-- | apps/commander/commander.c | 117 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 21 | ||||
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 16 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 15 |
4 files changed, 107 insertions, 62 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) { diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index cd479d40d..646deb62e 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -208,7 +208,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) att_sp.roll_body = 0.3f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0; - att_sp.thrust = 0.4f; + + /* limit throttle to 60 % of last value */ + if (isfinite(manual_sp.throttle)) { + att_sp.thrust = 0.6f * manual_sp.throttle; + } else { + att_sp.thrust = 0.0f; + } // XXX disable yaw control, loiter @@ -250,7 +256,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) //param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.3f; att_sp.pitch_body = 0.0f; - att_sp.thrust = 0.4f; + + /* limit throttle to 60 % of last value */ + if (isfinite(manual_sp.throttle)) { + att_sp.thrust = 0.6f * manual_sp.throttle; + } else { + att_sp.thrust = 0.0f; + } att_sp.yaw_body = 0; // XXX disable yaw control, loiter diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index f184859a3..1b03e8c22 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -253,7 +253,20 @@ mc_thread_main(int argc, char *argv[]) param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - att_sp.thrust = failsafe_throttle; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; + } else { + att_sp.thrust = 0.0f; + } /* keep current yaw, do not attempt to go to north orientation, * since if the pilot regains RC control, he will be lost regarding |