fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sanitize vtol_takeoff behavior

Browse Source
This commit is contained in:
sander 2016-03-24 00:59:00 +01:00 committed by Lorenz Meier
parent 1a20de7908
commit 26c8df0eb9
2 changed files with 30 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/navigator/mission.cpp
View File

@ -476,10 +476,17 @@ Mission::set_mission_items()
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& 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.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_mission_item.yaw = NAN;
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_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) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_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
@ -586,28 +595,6 @@ Mission::set_mission_items()
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 *********************************************/
@ -709,6 +696,20 @@ Mission::do_need_move_to_land()
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
Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
{

5
src/modules/navigator/mission.h
View File

@ -129,6 +129,11 @@ private:
*/
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
*/