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

navigator, commander: RTL and RC failsafe fixes

This commit is contained in:
Anton Babushkin 2014-01-18 14:25:24 +01:00
parent 5a1b39a172
commit b175937b5f
3 changed files with 31 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1133,7 +1133,7 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
if (status.main_state != MAIN_STATE_AUTO) {
if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode");

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

@ -180,6 +180,8 @@ private:
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
char *nav_states_str[NAV_STATE_MAX];
struct {
float min_altitude;
float acceptance_radius;
@ -415,6 +417,12 @@ Navigator::Navigator() :
memset(&_mission_result, 0, sizeof(struct mission_result_s));
nav_states_str[0] = "NONE";
nav_states_str[1] = "READY";
nav_states_str[2] = "LOITER";
nav_states_str[3] = "MISSION";
nav_states_str[4] = "RTL";
/* Initialize state machine */
myState = NAV_STATE_NONE;
start_none();
@ -788,7 +796,7 @@ Navigator::task_main()
/* notify user about state changes */
if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]);
prevState = myState;
pub_control_mode = true;
}
@ -881,7 +889,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
},
{
/* STATE_READY */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
@ -917,7 +925,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state
},
};
@ -945,8 +953,8 @@ Navigator::start_ready()
_reset_loiter_pos = true;
_do_takeoff = false;
// TODO check
if (_rtl_state != RTL_STATE_LAND) {
/* allow RTL if landed not at home */
_rtl_state = RTL_STATE_NONE;
}
@ -977,6 +985,11 @@ Navigator::start_loiter()
_mission_item_triplet.current.altitude = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
}
if (_rtl_state == RTL_STATE_LAND) {
/* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */
_rtl_state = RTL_STATE_DESCEND;
}
}
_mission_item_triplet.previous_valid = false;
@ -1103,11 +1116,19 @@ Navigator::advance_mission()
void
Navigator::start_rtl()
{
_reset_loiter_pos = true;
_do_takeoff = false;
if (_rtl_state == RTL_STATE_NONE)
_rtl_state = RTL_STATE_CLIMB;
if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
if (_reset_loiter_pos) {
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.altitude = _global_pos.alt;
}
}
}
_reset_loiter_pos = true;
mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
set_rtl_item();
}
@ -1150,7 +1171,6 @@ Navigator::set_rtl_item()
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
// don't change altitude setpoint

2
src/modules/navigator/navigator_params.c
View File

@ -55,6 +55,6 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode