fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 03:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up control mode state machine / flight mode / navigation state machine still needs a bit cleaning up

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-27 18:27:08 +01:00
parent 61d7e1d285
commit f5bad08bd0
10 changed files with 625 additions and 300 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+73 -3
View File
@@ -1268,6 +1268,8 @@ int commander_thread_main(int argc, char *argv[])
current_status.offboard_control_signal_lost = true;
/* allow manual override initially */
current_status.flag_external_manual_override_ok = true;
/* flag position info as bad, do not allow auto mode */
current_status.flag_vector_flight_mode_ok = false;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1654,6 +1656,72 @@ 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)) {
/* 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;
} else {
/* assuming a fixed wing, fall back to direct pass-through */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
}
} else if (sp_man.manual_sas_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;
} else {
/* assuming a fixed wing, fall back to direct pass-through */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
}
} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
/* top stick position, set SAS for all vehicle types */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
} else {
/* center stick position, set rate control */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
}
/*
* Check if manual stability control modes have to be switched
*/
if (!isfinite(sp_man.manual_sas_switch)) {
/* this switch is not properly mapped, set default */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
} else {
/* center stick position, set default */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
}
/*
* Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft.
@@ -1686,19 +1754,21 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* check manual override switch - switch to manual or auto mode */
if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
/* enable manual override */
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else {
/* check auto mode switch for correct mode */
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
//update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
// XXX hack
/* enable stabilized mode */
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
update_state_machine_mode_hold(stat_pub, &current_status, mavlink_fd);
}
}