fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 16:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sanitize vtol_takeoff behavior

Browse Source
This commit is contained in:
sander
2016-04-02 21:29:26 +01:00
committed by Lorenz Meier
parent 1a20de7908
commit 26c8df0eb9
2 changed files with 30 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+25 -24
View File
@@ -476,10 +476,17 @@ Mission::set_mission_items()
&& _navigator->get_vstatus()->is_rotary_wing && _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed && !_navigator->get_vstatus()->condition_landed
&& has_next_position_item) { && has_next_position_item) {
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;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW; _mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_mission_item.yaw = NAN; _mission_item.yaw = NAN;
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
} else { } else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT; new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -489,11 +496,13 @@ Mission::set_mission_items()
} }
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) { && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT; new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
} }
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
@@ -586,28 +595,6 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_DEFAULT; new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
} }
/* don't advance mission after FW to MC command */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type != WORK_ITEM_TYPE_CMD_BEFORE_MOVE
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& pos_sp_triplet->current.valid) {
//new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
}
/* after FW to MC transition finish moving to the waypoint */
if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE
&& pos_sp_triplet->current.valid) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
copy_positon_if_valid(&_mission_item, &(pos_sp_triplet->current));
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
} }
/*********************************** set setpoints and check next *********************************************/ /*********************************** set setpoints and check next *********************************************/
@@ -709,6 +696,20 @@ Mission::do_need_move_to_land()
return false; return false;
} }
bool
Mission::do_need_move_to_takeoff()
{
if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
return d_current > _navigator->get_acceptance_radius();
}
return false;
}
void void
Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint) Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
{ {
src/modules/navigator/mission.h
+5
View File
@@ -129,6 +129,11 @@ private:
*/ */
bool do_need_move_to_land(); bool do_need_move_to_land();
/**
* Returns true if we need to move to waypoint location after vtol takeoff
*/
bool do_need_move_to_takeoff();
/** /**
* Copies position from setpoint if valid, otherwise copies current position * Copies position from setpoint if valid, otherwise copies current position
*/ */