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

fixing takeoff mission and swtiching to previous flight mode after land/takeoff

Browse Source
This commit is contained in:
Andreas Antener
2015-12-28 15:21:50 +01:00
committed by Lorenz Meier
parent 64349c7a09
commit f17c5d8d55
3 changed files with 20 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+10 -2
View File
@@ -119,8 +119,16 @@ MissionBlock::is_mission_item_reached()
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
/* _mission_item.acceptance_radius is not always set */
float mission_acceptance_radius = _mission_item.acceptance_radius;
/* if set to zero use the default instead */
if (mission_acceptance_radius < NAV_EPSILON_POSITION) {
mission_acceptance_radius = _navigator->get_acceptance_radius();
}
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
altitude_amsl - mission_acceptance_radius) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
@@ -301,7 +309,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
{
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
item->nav_cmd = NAV_CMD_TAKEOFF;
/* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat;
src/modules/navigator/takeoff.cpp
+2 -1
View File
@@ -78,6 +78,7 @@ Takeoff::on_activation()
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -96,7 +97,7 @@ Takeoff::on_activation()
void
Takeoff::on_active()
{
if (is_mission_item_reached()) {
if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
}