From 6bac376bcc79c01ecf71a0040cc84618d91d9eb7 Mon Sep 17 00:00:00 2001 From: hali9 Date: Wed, 25 Dec 2019 16:25:51 +0100 Subject: [PATCH] fix wp linear climb dive --- src/main/navigation/navigation.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0334bc1147d..3cfe2459ef1 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1399,8 +1399,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na fpVector3_t tmpWaypoint; tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; - tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance), - posControl.wpInitialDistance, posControl.wpInitialDistance / 10, + tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), + posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); return NAV_FSM_EVENT_NONE; // will re-process state in >10ms