fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 06:41:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Stabilization enabling / switching

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-30 09:53:45 +01:00
parent 0298714db5
commit 62a95bf8e6
4 changed files with 98 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+52 -47
View File
@@ -1684,63 +1684,63 @@ int commander_thread_main(int argc, char *argv[])
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
/* // /*
* Check if manual control modes have to be switched // * Check if manual control modes have to be switched
*/ // */
if (!isfinite(sp_man.manual_mode_switch)) { // if (!isfinite(sp_man.manual_mode_switch)) {
printf("man mode sw not finite\n"); // printf("man mode sw not finite\n");
/* this switch is not properly mapped, set default */ // /* this switch is not properly mapped, set default */
if ((current_status.system_type == MAV_TYPE_QUADROTOR) || // if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
(current_status.system_type == MAV_TYPE_HEXAROTOR) || // (current_status.system_type == MAV_TYPE_HEXAROTOR) ||
(current_status.system_type == MAV_TYPE_OCTOROTOR)) { // (current_status.system_type == MAV_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, fall back to SAS */ // /* assuming a rotary wing, fall back to SAS */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true; // current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true; // current_status.flag_control_rates_enabled = true;
} else { // } else {
/* assuming a fixed wing, fall back to direct pass-through */ // /* assuming a fixed wing, fall back to direct pass-through */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status.flag_control_attitude_enabled = false; // current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false; // current_status.flag_control_rates_enabled = false;
} // }
} else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set direct mode for vehicles supporting it */ // /* bottom stick position, set direct mode for vehicles supporting it */
if ((current_status.system_type == MAV_TYPE_QUADROTOR) || // if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
(current_status.system_type == MAV_TYPE_HEXAROTOR) || // (current_status.system_type == MAV_TYPE_HEXAROTOR) ||
(current_status.system_type == MAV_TYPE_OCTOROTOR)) { // (current_status.system_type == MAV_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, fall back to SAS */ // /* assuming a rotary wing, fall back to SAS */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true; // current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true; // current_status.flag_control_rates_enabled = true;
} else { // } else {
/* assuming a fixed wing, set to direct pass-through as requested */ // /* assuming a fixed wing, set to direct pass-through as requested */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status.flag_control_attitude_enabled = false; // current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false; // current_status.flag_control_rates_enabled = false;
} // }
} else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
/* top stick position, set SAS for all vehicle types */ // /* top stick position, set SAS for all vehicle types */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true; // current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true; // current_status.flag_control_rates_enabled = true;
} else { // } else {
/* center stick position, set rate control */ // /* center stick position, set rate control */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
current_status.flag_control_attitude_enabled = false; // current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = true; // 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 * 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) { if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
/* enable manual override */ /* enable manual override */
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else { } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
/* check auto mode switch for correct mode */ /* check auto mode switch for correct mode */
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
/* enable stabilized mode */ /* enable stabilized mode */
@@ -1813,6 +1813,11 @@ int commander_thread_main(int argc, char *argv[])
} else { } else {
update_state_machine_mode_hold(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_hold(stat_pub, &current_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 */ /* handle the case where RC signal was regained */
apps/commander/state_machine_helper.c
+18 -3
View File
@@ -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->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->flag_control_manual_enabled = true; current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true; /* set behaviour based on airframe */
current_status->flag_control_rates_enabled = true; 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 (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) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
apps/fixedwing_att_control/fixedwing_att_control_main.c
+14 -2
View File
@@ -208,7 +208,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
att_sp.roll_body = 0.3f; att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f; att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0; 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 // 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); //param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.3f; att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f; 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; att_sp.yaw_body = 0;
// XXX disable yaw control, loiter // XXX disable yaw control, loiter
apps/multirotor_att_control/multirotor_att_control_main.c
+14 -1
View File
@@ -253,7 +253,20 @@ mc_thread_main(int argc, char *argv[])
param_get(failsafe_throttle_handle, &failsafe_throttle); param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.0f; att_sp.roll_body = 0.0f;
att_sp.pitch_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, /* keep current yaw, do not attempt to go to north orientation,
* since if the pilot regains RC control, he will be lost regarding * since if the pilot regains RC control, he will be lost regarding