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

align towards next waypoint before MC to FW transition

Browse Source
This commit is contained in:
Andreas Antener 2016-01-28 09:05:10 +01:00
parent b75eaf3c4a
commit 02aa0eacd5
2 changed files with 52 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -412,6 +412,8 @@ Mission::set_mission_items()
/*********************************** handle mission item *********************************************/
bool do_issue_command = true;
/* handle position mission items */
if (item_contains_position(&_mission_item)) {
@ -466,18 +468,50 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* handle non-position mission items such as commands */
} else {
// XXX: before issuing command, check if we need to align for transition first
issue_command(&_mission_item);
/* 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
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
mission_item_next_position.lat,
mission_item_next_position.lon);
do_issue_command = false;
}
/* yaw is aligned now */
if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
}
/*********************************** clean up and set next *********************************************/
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected agains 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) */
if (do_issue_command) {
issue_command(&_mission_item);
}
/* set current work item type */
_work_item_type = new_work_item_type;
@ -513,7 +547,8 @@ 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,
_distance_current_previous = get_distance_to_next_waypoint(
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon);
@ -588,8 +623,9 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
void
Mission::heading_sp_update()
{
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
/* we don't want to be yawing during takeoff */
/* we don't want to be yawing during takeoff or align */
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _work_item_type == WORK_ITEM_TYPE_ALIGN) {
return;
}
@ -601,11 +637,6 @@ Mission::heading_sp_update()
return;
}
/* Don't change heading for takeoff waypoints, the ground may be near */
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
return;
}
/* set yaw angle for the waypoint iff a loiter time has been specified */
if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
_mission_item.yaw = _on_arrival_yaw;

13
src/modules/navigator/mission_block.cpp
View File

@ -105,10 +105,10 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_LOITER_UNLIMITED:
return false;
case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: /* fallthrough */
case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
case NAV_CMD_DO_DIGICAM_CONTROL: /* fallthrough */
case NAV_CMD_DO_VTOL_TRANSITION:
// XXX: we should wait on command ACK or status change instead
// currently we just wait so the command be processed
// currently we just wait so the command can be processed
if (hrt_absolute_time() - _action_start > 1000) {
return true;
@ -185,6 +185,7 @@ MissionBlock::is_mission_item_reached()
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
PX4_WARN("yaw err: %.3f", (float)yaw_err);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
@ -251,7 +252,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
return;
}
warnx("forwarding command.\n");
warnx("forwarding command %d\n", item->nav_cmd);
struct vehicle_command_s cmd = {};
mission_item_to_vehicle_command(item, &cmd);
_action_start = hrt_absolute_time();
@ -268,8 +269,8 @@ bool
MissionBlock::item_contains_position(const struct mission_item_s *item)
{
// XXX: maybe extend that check onto item properties
if (item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL ||
item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
if (item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
return false;
}