Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

Checkpoint: New hierarchical states

Browse Source
This commit is contained in:
Julian Oes
2013-02-20 10:38:06 -08:00
parent aab6214cdc
commit 0e29f2505a
5 changed files with 392 additions and 496 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/flight_mode_state_machine.odg
BIN
View File
Binary file not shown.
apps/commander/commander.c
+53 -64
View File
@@ -1785,103 +1785,90 @@ 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.mode_switch)) { if (!isfinite(sp_man.mode_switch)) {
warnx("mode sw not finite"); warnx("mode sw not finite");
/* this switch is not properly mapped, set attitude stabilized */ /* no valid stick position, go to default */
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
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 */ } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false;
}
} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */
current_status.mode_switch = MODE_SWITCH_MANUAL;
/* this switch is not properly mapped, set attitude stabilized */ } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, fall back to m */ /* top stick position, set auto/mission for all vehicle types */
current_status.flag_control_attitude_enabled = true; current_status.mode_switch = MODE_SWITCH_AUTO;
current_status.flag_control_rates_enabled = true;
} else {
/* assuming a fixed wing, set to direct pass-through as requested */ } else {
current_status.flag_control_attitude_enabled = false; /* center stick position, set seatbelt/simple control */
current_status.flag_control_rates_enabled = false; current_status.mode_switch = MODE_SWITCH_SEATBELT;
} }
} else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
/* top stick position, set auto/mission for all vehicle types */
current_status.flag_control_position_enabled = true;
current_status.flag_control_velocity_enabled = true;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* center stick position, set seatbelt/simple control */
current_status.flag_control_velocity_enabled = true;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
}
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
/* /*
* Check if land/RTL is requested * Check if land/RTL is requested
*/ */
if (!isfinite(sp_man.return_switch)) { if (!isfinite(sp_man.return_switch)) {
/* this switch is not properly mapped, set default */ /* this switch is not properly mapped, set default */
current_status.flag_land_requested = false; // XXX default? current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */ /* bottom stick position, set altitude hold */
current_status.flag_land_requested = false; current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */ /* top stick position */
current_status.flag_land_requested = true; current_status.return_switch = RETURN_SWITCH_RETURN;
} else { } else {
/* center stick position, set default */ /* center stick position, set default */
current_status.flag_land_requested = false; // XXX default? current_status.return_switch = RETURN_SWITCH_NONE;
} }
/* check mission switch */ /* check mission switch */
if (!isfinite(sp_man.mission_switch)) { if (!isfinite(sp_man.mission_switch)) {
/* this switch is not properly mapped, set default */ /* this switch is not properly mapped, set default */
current_status.flag_mission_activated = false; current_status.mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) {
/* top switch position */ /* top switch position */
current_status.flag_mission_activated = true; current_status.mission_switch = MISSION_SWITCH_MISSION;
} else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) {
/* bottom switch position */ /* bottom switch position */
current_status.flag_mission_activated = false; current_status.mission_switch = MISSION_SWITCH_NONE;
} else { } else {
/* center switch position, set default */ /* center switch position, set default */
current_status.flag_mission_activated = false; // XXX default? current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default?
}
/* Now it's time to handle the stick inputs */
if (current_status.arming_state == ARMING_STATE_ARMED) {
if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MANUAL );
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_SEATBELT );
} else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) {
do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MISSION );
}
}
} }
/* handle the case where RC signal was regained */ /* handle the case where RC signal was regained */
@@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
} else { } else {
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); if (current_status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
}
} }
/* /*
* Check if left stick is in lower left position --> switch to standby state. * Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft. * Do this only for multirotors, not for fixed wing aircraft.
*/ */
// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
// ) && // ) &&
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY); do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
@@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[])
/* decide about attitude control flag, enable in att/pos/vel */ /* decide about attitude control flag, enable in att/pos/vel */
bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
/* decide about rate control flag, enable it always XXX (for now) */ /* decide about rate control flag, enable it always XXX (for now) */
bool rates_ctrl_enabled = true; bool rates_ctrl_enabled = true;
apps/commander/state_machine_helper.c
+293 -385
View File
File diff suppressed because it is too large Load Diff
apps/commander/state_machine_helper.h
+2 -2
View File
@@ -47,9 +47,9 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); //int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
apps/uORB/topics/vehicle_status.h
+44 -45
View File
@@ -60,27 +60,48 @@
/* State Machine */ /* State Machine */
typedef enum { typedef enum {
NAVIGATION_STATE_STANDBY=0, SYSTEM_STATE_INIT=0,
NAVIGATION_STATE_MANUAL, SYSTEM_STATE_MANUAL,
NAVIGATION_STATE_SEATBELT, SYSTEM_STATE_SEATBELT,
NAVIGATION_STATE_DESCENT, SYSTEM_STATE_AUTO,
NAVIGATION_STATE_LOITER, SYSTEM_STATE_REBOOT,
NAVIGATION_STATE_AUTO_READY, SYSTEM_STATE_IN_AIR_RESTORE
NAVIGATION_STATE_MISSION, } system_state_t;
NAVIGATION_STATE_RTL,
NAVIGATION_STATE_LAND,
NAVIGATION_STATE_TAKEOFF
} navigation_state_t;
typedef enum { typedef enum {
ARMING_STATE_INIT = 0, MANUAL_STANDBY = 0,
ARMING_STATE_STANDBY, MANUAL_READY,
ARMING_STATE_ARMED, MANUAL_IN_AIR
ARMING_STATE_ABORT, } manual_state_t;
ARMING_STATE_ERROR,
ARMING_STATE_REBOOT, typedef enum {
ARMING_STATE_IN_AIR_RESTORE SEATBELT_STANDBY,
} arming_state_t; SEATBELT_READY,
SEATBELT,
SEATBELT_ASCENT,
SEATBELT_DESCENT,
} seatbelt_state_t;
typedef enum {
AUTO_STANDBY,
AUTO_READY,
AUTO_LOITER,
AUTO_MISSION,
AUTO_RTL,
AUTO_TAKEOFF,
AUTO_LAND,
} auto_state_t;
//typedef enum {
// ARMING_STATE_INIT = 0,
// ARMING_STATE_STANDBY,
// ARMING_STATE_ARMED_GROUND,
// ARMING_STATE_ARMED_AIRBORNE,
// ARMING_STATE_ERROR_GROUND,
// ARMING_STATE_ERROR_AIRBORNE,
// ARMING_STATE_REBOOT,
// ARMING_STATE_IN_AIR_RESTORE
//} arming_state_t;
typedef enum { typedef enum {
HIL_STATE_OFF = 0, HIL_STATE_OFF = 0,
@@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG {
typedef enum { typedef enum {
MODE_SWITCH_MANUAL = 0, MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_ASSISTED, MODE_SWITCH_SEATBELT,
MODE_SWITCH_AUTO MODE_SWITCH_AUTO
} mode_switch_pos_t; } mode_switch_pos_t;
@@ -114,26 +135,6 @@ typedef enum {
MISSION_SWITCH_MISSION MISSION_SWITCH_MISSION
} mission_switch_pos_t; } mission_switch_pos_t;
//enum VEHICLE_FLIGHT_MODE {
// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */
// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */
// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */
//};
//
//enum VEHICLE_MANUAL_CONTROL_MODE {
// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */
// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */
// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */
//};
//
//enum VEHICLE_MANUAL_SAS_MODE {
// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */
// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */
// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */
// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */
//};
/** /**
* Should match 1:1 MAVLink's MAV_TYPE ENUM * Should match 1:1 MAVLink's MAV_TYPE ENUM
*/ */
@@ -180,8 +181,8 @@ struct vehicle_status_s
uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
// uint64_t failsave_highlevel_begin; TO BE COMPLETED // uint64_t failsave_highlevel_begin; TO BE COMPLETED
navigation_state_t navigation_state; /**< current navigation state */ system_state_t system_state; /**< current system state */
arming_state_t arming_state; /**< current arming state */ // arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */ hil_state_t hil_state; /**< current hil state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
@@ -194,6 +195,7 @@ struct vehicle_status_s
return_switch_pos_t return_switch; return_switch_pos_t return_switch;
mission_switch_pos_t mission_switch; mission_switch_pos_t mission_switch;
bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_armed; /**< true is motors / actuators are armed */
bool flag_system_emergency; bool flag_system_emergency;
bool flag_system_in_air_restore; /**< true if we can restore in mid air */ bool flag_system_in_air_restore; /**< true if we can restore in mid air */
@@ -212,9 +214,6 @@ struct vehicle_status_s
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_land_requested; /**< true if land requested */
bool flag_mission_activated; /**< true if in mission mode */
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration; bool flag_preflight_accel_calibration;