From 0ffb2dee024a55ecbb09db59bb0e94db1d9e9a8c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 11 Jul 2013 14:35:37 +0200 Subject: [PATCH] bugfix in pathplanner - 3d distance not calculated correctly --- flight/modules/PathPlanner/pathplanner.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index f4ece087b..c7be26409 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -371,7 +371,7 @@ static uint8_t conditionDistanceToTarget() if (pathAction.ConditionParameters[1] > 0.5f) { distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) + powf(waypoint.Position[1] - positionState.East, 2) - + powf(waypoint.Position[1] - positionState.Down, 2)); + + powf(waypoint.Position[2] - positionState.Down, 2)); } else { distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) + powf(waypoint.Position[1] - positionState.East, 2));