From c1199d1202fb1a1b8fb1e14df73098faf88732d1 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Tue, 13 Jun 2017 19:55:31 +0200 Subject: [PATCH] mission don't apply slewrate for altitude --- src/modules/navigator/mission.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index eace84a075..17642b3827 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1064,11 +1064,15 @@ Mission::altitude_sp_foh_update() struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* Don't change setpoint if last and current waypoint are not valid - * or if the previous altitude isn't from a position or loiter setpoint + * or if the previous altitude isn't from a position or loiter setpoint or + * if rotary wing since that is handled in the mc_pos_control */ + + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt) || !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION || - pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { + pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER) || + _navigator->get_vstatus()->is_rotary_wing) { return; } @@ -1119,10 +1123,10 @@ Mission::altitude_sp_foh_update() pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; } - if (!_navigator->get_vstatus()->is_rotary_wing) { - // we set altitude directly so we can run this in parallel to the heading update - _navigator->set_position_setpoint_triplet_updated(); - } + + // we set altitude directly so we can run this in parallel to the heading update + _navigator->set_position_setpoint_triplet_updated(); + } void