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

Remove duplicate LAND mode, clean up mode reporting across the board to ensure consistency

Browse Source
This commit is contained in:
Lorenz Meier
2016-03-13 15:39:35 +01:00
parent f472ac577a
commit 1fdf252e96
5 changed files with 23 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+5 -17
View File
@@ -202,7 +202,7 @@ static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home;
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX - 1];
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];
static unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man = {};
@@ -1177,7 +1177,7 @@ int commander_thread_main(int argc, char *argv[])
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LAND] = "LAND";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
@@ -2944,9 +2944,10 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
/* we know something has changed - check if we are in mode slot operation */
if (sp_man->mode_slot > 0) {
if (sp_man->mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
if (sp_man->mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {
warnx("overflow");
return TRANSITION_DENIED;
}
@@ -3225,6 +3226,7 @@ set_control_mode()
control_mode.flag_external_manual_override_ok = false;
/* fallthrough */
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
@@ -3264,20 +3266,6 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
/* TODO: check if this makes sense */
control_mode.flag_control_manual_enabled = false;
src/modules/commander/px4_custom_mode.h
+2 -1
View File
@@ -50,7 +50,8 @@ enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
PX4_CUSTOM_MAIN_MODE_STABILIZED
PX4_CUSTOM_MAIN_MODE_STABILIZED,
PX4_CUSTOM_MAIN_MODE_RATTITUDE
};
enum PX4_CUSTOM_SUB_MODE_AUTO {
src/modules/commander/state_machine_helper.cpp
+9 -9
View File
@@ -592,7 +592,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
@@ -670,7 +670,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
@@ -687,7 +687,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
@@ -711,7 +711,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
@@ -725,7 +725,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
@@ -753,7 +753,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
@@ -774,7 +774,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
@@ -800,7 +800,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
}
break;
@@ -814,7 +814,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
src/modules/mavlink/mavlink_messages.cpp
+6 -1
View File
@@ -139,6 +139,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_RATTITUDE;
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;
@@ -192,7 +197,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case vehicle_status_s::NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
/* fallthrough */
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -486,7 +486,7 @@ Navigator::task_main()
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff;
break;
case vehicle_status_s::NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;