dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

This commit is contained in:
Julian Oes 2014-06-19 10:30:44 +02:00
parent 32e2df3d08 62faa2ee51
commit c23bc93681
6 changed files with 20 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/commander/commander.cpp
View File

@ -649,8 +649,7 @@ int commander_thread_main(int argc, char *argv[])
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
@ -1734,8 +1733,7 @@ set_control_mode()
case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_FS_RC_LOSS: case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
control_mode.flag_control_manual_enabled = false; control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true; control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true; control_mode.flag_control_rates_enabled = true;

1
src/modules/commander/px4_custom_mode.h
View File

@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4_CUSTOM_SUB_MODE_AUTO_LAND,
PX4_CUSTOM_SUB_MODE_AUTO_RTGS
}; };
union px4_custom_mode { union px4_custom_mode {

12
src/modules/commander/state_machine_helper.cpp
View File

@ -384,7 +384,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) { } else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND; status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) { } else if (status->condition_local_altitude_valid) {
@ -427,7 +427,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) { } else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND; status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) { } else if (status->condition_local_altitude_valid) {
@ -441,7 +441,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) { } else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND; status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) { } else if (status->condition_local_altitude_valid) {
@ -467,7 +467,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) { } else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND; status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) { } else if (status->condition_local_altitude_valid) {
@ -481,7 +481,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) { } else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND; status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) { } else if (status->condition_local_altitude_valid) {
@ -495,7 +495,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) { } else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND; status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) { } else if (status->condition_local_altitude_valid) {

8
src/modules/mavlink/mavlink_messages.cpp
View File

@ -176,6 +176,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break; break;
case NAVIGATION_STATE_AUTO_RTGS:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
break;
case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_TERMINATION:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;

3
src/modules/navigator/navigator_main.cpp
View File

@ -351,10 +351,9 @@ Navigator::task_main()
_navigation_mode = &_loiter; _navigation_mode = &_loiter;
break; break;
case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
_navigation_mode = &_rtl; _navigation_mode = &_rtl;
break; break;
case NAVIGATION_STATE_AUTO_FS_DL_LOSS: case NAVIGATION_STATE_AUTO_RTGS:
_navigation_mode = &_rtl; /* TODO: change this to something else */ _navigation_mode = &_rtl; /* TODO: change this to something else */
break; break;
case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_LAND:

5
src/modules/uORB/topics/vehicle_status.h
View File

@ -100,9 +100,8 @@ typedef enum {
NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_POSCTL, /**< Position control mode */
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */
NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */