From 57b9ac05de158befa4062451a0203b9a8f446167 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 00:23:13 +0100 Subject: [PATCH] Navigator: taking off now on takeoff command, but not yet holding altitude once completed. --- src/modules/navigator/mission_block.cpp | 26 +++++++++++++++++++++++++ src/modules/navigator/mission_block.h | 5 +++++ src/modules/navigator/takeoff.cpp | 3 ++- 3 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 94ded8a4b9..b230bcab3f 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -289,3 +289,29 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) item->origin = ORIGIN_ONBOARD; } } + +void +MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) +{ + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + /* use current position and use return altitude as clearance */ + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f) { + item->altitude += min_clearance; + } + + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = (_navigator->get_acceptance_radius() > min_clearance / 2.0f) ? + (min_clearance / 2) : _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = min_pitch; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index eb17a329ee..5490b4c6b8 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -96,6 +96,11 @@ protected: */ void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); + /** + * Set a takeoff mission item + */ + void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 13ba35c2b9..f669429449 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -74,12 +74,13 @@ void Takeoff::on_activation() { /* set current mission item to Takeoff */ - set_loiter_item(&_mission_item, _param_min_alt.get()); + set_takeoff_item(&_mission_item, _param_min_alt.get()); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; pos_sp_triplet->next.valid = false; _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);