fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 12:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

setpoint type IDLE added (for AUTO_READY state), LAND mode fixed

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-29 14:39:36 +01:00
parent 48cec50dd3
commit 591b355981
3 changed files with 324 additions and 280 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+33 -13
View File
@@ -1030,8 +1030,11 @@ void
Navigator::start_ready()
{
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
_mission_item_valid = false;
_reset_loiter_pos = true;
@@ -1229,20 +1232,32 @@ Navigator::start_rtl()
void
Navigator::start_land()
{
/* this state can be requested by commander even if no global position available,
* in his case controller must perform landing without position control */
_do_takeoff = false;
_reset_loiter_pos = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.next.valid = false;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.current.type = SETPOINT_TYPE_LAND;
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.alt = _global_pos.alt;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.yaw = NAN;
_mission_item_valid = true;
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
}
void
@@ -1546,8 +1561,8 @@ Navigator::on_mission_item_reached()
}
}
} else {
/* RTL finished */
} else if (myState == NAV_STATE_RTL) {
/* RTL completed */
if (_rtl_state == RTL_STATE_LAND) {
/* landed at home position */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed");
@@ -1558,6 +1573,11 @@ Navigator::on_mission_item_reached()
_rtl_state = (RTLState)(_rtl_state + 1);
set_rtl_item();
}
} else if (myState == NAV_STATE_LAND) {
/* landing completed */
mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
dispatch(EVENT_READY_REQUESTED);
}
}