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

implement MAV_CMD_NAV_LOITER_TO_ALT and general mission cleanup

Browse Source
This commit is contained in:
Daniel Agar
2016-07-12 23:36:05 +02:00
committed by Lorenz Meier
parent 7419151314
commit 59b4350aa0
8 changed files with 314 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+26 -12
View File
@@ -348,7 +348,7 @@ Mission::set_mission_items()
/* make sure param is up to date */
updateParams();
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -453,6 +453,7 @@ Mission::set_mission_items()
/* do takeoff before going to setpoint if needed and not already in takeoff */
if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
/* use current mission item as next position item */
@@ -482,8 +483,9 @@ Mission::set_mission_items()
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
/* check if the vtol_takeoff command is on top of us */
if(do_need_move_to_takeoff()){
if (do_need_move_to_takeoff()){
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
@@ -516,6 +518,7 @@ Mission::set_mission_items()
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_land_detected()->landed) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
@@ -537,6 +540,7 @@ Mission::set_mission_items()
&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_mission_item.autocontinue = true;
@@ -547,6 +551,7 @@ Mission::set_mission_items()
if (do_need_move_to_land() &&
(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
@@ -572,6 +577,7 @@ Mission::set_mission_items()
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& _navigator->get_vstatus()->is_rotary_wing) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
@@ -607,7 +613,7 @@ Mission::set_mission_items()
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected agains non-position items) */
/* set current position setpoint from mission item (is protected against non-position items) */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* issue command if ready (will do nothing for position mission items) */
@@ -617,7 +623,9 @@ Mission::set_mission_items()
_work_item_type = new_work_item_type;
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
@@ -647,6 +655,7 @@ Mission::set_mission_items()
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
@@ -661,6 +670,7 @@ bool
Mission::do_need_takeoff()
{
if (_navigator->get_vstatus()->is_rotary_wing) {
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
/* force takeoff if landed (additional protection) */
@@ -674,13 +684,12 @@ Mission::do_need_takeoff()
/* check if current mission item is one that requires takeoff before */
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
_need_takeoff = false;
return true;
@@ -693,7 +702,8 @@ Mission::do_need_takeoff()
bool
Mission::do_need_move_to_land()
{
if (_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
if (_navigator->get_vstatus()->is_rotary_wing
&& (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
@@ -757,7 +767,7 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
/* calculate takeoff altitude */
float takeoff_alt = get_absolute_altitude_for_item(*mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
/* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_land_detected()->landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
@@ -866,10 +876,14 @@ Mission::altitude_sp_foh_update()
}
/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic */
* and the FW controller has a custom landing logic
*
* NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
* */
if (_mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
|| !_navigator->is_planned_mission()) {
return;
}