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

Workitem handling for land

This commit is contained in:
sander 2016-03-22 10:59:55 +01:00 committed by Lorenz Meier
parent 903a0cd689
commit 31862be420
2 changed files with 60 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -432,6 +432,9 @@ Mission::set_mission_items()
/*********************************** handle mission item *********************************************/
/* handle position mission items */
printf("NAV_CMD %.2f workitem: %2f \n", (double)_mission_item.nav_cmd, (double)_work_item_type);
if (item_contains_position(&_mission_item)) {
/* we have a new position item so set previous position setpoint to current */
@ -464,47 +467,64 @@ Mission::set_mission_items()
/* if we just did a takeoff navigate to the actual waypoint now */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& has_next_position_item) {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
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;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
}
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
}
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& has_next_position_item) {
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
set_align_mission_item(&_mission_item, &mission_item_next_position);
}
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_ALIGN) {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
}
if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND ||
(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_vstatus()->condition_landed) {
printf("vt move to land \n");
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));
has_next_position_item = true;
float altitude = _navigator->get_global_position()->alt;
if (pos_sp_triplet->current.valid) {
altitude = pos_sp_triplet->current.alt;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND;
}
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed) {
printf("vt transition \n");
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_mission_item.autocontinue = true;
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
}
/* move to landing waypoint before descent if necessary */
if (do_need_move_to_land() && _work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
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;
printf("doneedmovetoland \n");
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
@ -530,10 +550,14 @@ Mission::set_mission_items()
}
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND) {
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& _navigator->get_vstatus()->is_rotary_wing) {
printf("move to land complete \n");
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
/* ignore yaw for landing items */
/* XXX: if specified heading for landing is desired we could add another step before the descent
* that aligns the vehicle first */
@ -543,15 +567,16 @@ Mission::set_mission_items()
/* handle non-position mission items such as commands */
} else {
printf("not a position item \n");
/* turn towards next waypoint before MC to FW transition */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type != WORK_ITEM_TYPE_ALIGN
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
set_align_mission_item(&_mission_item, &mission_item_next_position);
}
@ -672,7 +697,7 @@ 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) {
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);
@ -958,14 +983,14 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
&mission_item_tmp, len) != len) {
&mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP waypoint could not be written");
return false;
}
report_do_jump_mission_changed(*mission_index_ptr,
mission_item_tmp.do_jump_repeat_count);
mission_item_tmp.do_jump_repeat_count);
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/

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

@ -258,7 +258,8 @@ private:
WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */
WORK_ITEM_TYPE_CMD_BEFORE_MOVE, /**< */
WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, /**< */
WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND /**< */
WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND, /**< */
WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION /**< */
} _work_item_type; /**< current type of work to do (sub mission item) */
};