mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
Checkpoint: New hierarchical states
This commit is contained in:
Julian Oes
Binary file not shown.
+53
-64
@@ -1785,103 +1785,90 @@ 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.mode_switch)) {
|
||||
warnx("mode sw not finite");
|
||||
/*
|
||||
* Check if manual control modes have to be switched
|
||||
*/
|
||||
if (!isfinite(sp_man.mode_switch)) {
|
||||
warnx("mode sw not finite");
|
||||
|
||||
/* this switch is not properly mapped, set attitude stabilized */
|
||||
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
|
||||
/* no valid stick position, go to default */
|
||||
|
||||
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.flag_control_attitude_enabled = false;
|
||||
current_status.flag_control_rates_enabled = false;
|
||||
}
|
||||
} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
|
||||
|
||||
} 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 */
|
||||
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
|
||||
} else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
|
||||
|
||||
/* assuming a rotary wing, fall back to m */
|
||||
current_status.flag_control_attitude_enabled = true;
|
||||
current_status.flag_control_rates_enabled = true;
|
||||
} else {
|
||||
/* top stick position, set auto/mission for all vehicle types */
|
||||
current_status.mode_switch = MODE_SWITCH_AUTO;
|
||||
|
||||
/* assuming a fixed wing, set to direct pass-through as requested */
|
||||
current_status.flag_control_attitude_enabled = false;
|
||||
current_status.flag_control_rates_enabled = false;
|
||||
}
|
||||
|
||||
} 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;
|
||||
}
|
||||
} else {
|
||||
/* center stick position, set seatbelt/simple control */
|
||||
current_status.mode_switch = MODE_SWITCH_SEATBELT;
|
||||
}
|
||||
|
||||
// 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)) {
|
||||
|
||||
/* 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) {
|
||||
|
||||
/* 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) {
|
||||
|
||||
/* top stick position */
|
||||
current_status.flag_land_requested = true;
|
||||
current_status.return_switch = RETURN_SWITCH_RETURN;
|
||||
|
||||
} else {
|
||||
/* center stick position, set default */
|
||||
current_status.flag_land_requested = false; // XXX default?
|
||||
current_status.return_switch = RETURN_SWITCH_NONE;
|
||||
}
|
||||
|
||||
/* check mission switch */
|
||||
if (!isfinite(sp_man.mission_switch)) {
|
||||
|
||||
/* 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) {
|
||||
|
||||
/* 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) {
|
||||
|
||||
/* bottom switch position */
|
||||
current_status.flag_mission_activated = false;
|
||||
current_status.mission_switch = MISSION_SWITCH_NONE;
|
||||
|
||||
} else {
|
||||
|
||||
/* 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, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL );
|
||||
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
|
||||
do_navigation_state_update(stat_pub, ¤t_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, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 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.");
|
||||
|
||||
} 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.
|
||||
* Do this only for multirotors, not for fixed wing aircraft.
|
||||
*/
|
||||
// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
|
||||
// ) &&
|
||||
* Check if left stick is in lower left position --> switch to standby state.
|
||||
* Do this only for multirotors, not for fixed wing aircraft.
|
||||
*/
|
||||
// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
|
||||
// ) &&
|
||||
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) {
|
||||
do_arming_state_update(stat_pub, ¤t_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 */
|
||||
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_POSITION);
|
||||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
|
||||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
|
||||
|
||||
/* decide about rate control flag, enable it always XXX (for now) */
|
||||
bool rates_ctrl_enabled = true;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -47,9 +47,9 @@
|
||||
#include <uORB/uORB.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);
|
||||
|
||||
|
||||
@@ -60,27 +60,48 @@
|
||||
|
||||
/* State Machine */
|
||||
typedef enum {
|
||||
NAVIGATION_STATE_STANDBY=0,
|
||||
NAVIGATION_STATE_MANUAL,
|
||||
NAVIGATION_STATE_SEATBELT,
|
||||
NAVIGATION_STATE_DESCENT,
|
||||
NAVIGATION_STATE_LOITER,
|
||||
NAVIGATION_STATE_AUTO_READY,
|
||||
NAVIGATION_STATE_MISSION,
|
||||
NAVIGATION_STATE_RTL,
|
||||
NAVIGATION_STATE_LAND,
|
||||
NAVIGATION_STATE_TAKEOFF
|
||||
} navigation_state_t;
|
||||
SYSTEM_STATE_INIT=0,
|
||||
SYSTEM_STATE_MANUAL,
|
||||
SYSTEM_STATE_SEATBELT,
|
||||
SYSTEM_STATE_AUTO,
|
||||
SYSTEM_STATE_REBOOT,
|
||||
SYSTEM_STATE_IN_AIR_RESTORE
|
||||
} system_state_t;
|
||||
|
||||
typedef enum {
|
||||
ARMING_STATE_INIT = 0,
|
||||
ARMING_STATE_STANDBY,
|
||||
ARMING_STATE_ARMED,
|
||||
ARMING_STATE_ABORT,
|
||||
ARMING_STATE_ERROR,
|
||||
ARMING_STATE_REBOOT,
|
||||
ARMING_STATE_IN_AIR_RESTORE
|
||||
} arming_state_t;
|
||||
MANUAL_STANDBY = 0,
|
||||
MANUAL_READY,
|
||||
MANUAL_IN_AIR
|
||||
} manual_state_t;
|
||||
|
||||
typedef enum {
|
||||
SEATBELT_STANDBY,
|
||||
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 {
|
||||
HIL_STATE_OFF = 0,
|
||||
@@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG {
|
||||
|
||||
typedef enum {
|
||||
MODE_SWITCH_MANUAL = 0,
|
||||
MODE_SWITCH_ASSISTED,
|
||||
MODE_SWITCH_SEATBELT,
|
||||
MODE_SWITCH_AUTO
|
||||
} mode_switch_pos_t;
|
||||
|
||||
@@ -114,26 +135,6 @@ typedef enum {
|
||||
MISSION_SWITCH_MISSION
|
||||
} 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
|
||||
*/
|
||||
@@ -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_highlevel_begin; TO BE COMPLETED
|
||||
|
||||
navigation_state_t navigation_state; /**< current navigation state */
|
||||
arming_state_t arming_state; /**< current arming state */
|
||||
system_state_t system_state; /**< current system state */
|
||||
// arming_state_t arming_state; /**< current arming state */
|
||||
hil_state_t hil_state; /**< current hil state */
|
||||
|
||||
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;
|
||||
mission_switch_pos_t mission_switch;
|
||||
|
||||
|
||||
bool flag_system_armed; /**< true is motors / actuators are armed */
|
||||
bool flag_system_emergency;
|
||||
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_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_mag_calibration; /**< true if mag calibration is requested */
|
||||
bool flag_preflight_accel_calibration;
|
||||
|
||||
Reference in New Issue
Block a user