From bed21b43e1069fdc4214e41967ab5d3105a83485 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Jul 2019 14:38:39 +0900 Subject: [PATCH] AP_OAPathPlanner: minor fix to return original origin and dest upon failure this has no functional effect because the consumers are not using the origin_new and destination_new unless the ret_state was OA_SUCCESS --- libraries/AC_Avoidance/AP_OAPathPlanner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 00f971235a..b66542316c 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -271,8 +271,8 @@ void AP_OAPathPlanner::avoidance_thread() // give the main thread the avoidance result WITH_SEMAPHORE(_rsem); avoidance_result.destination = avoidance_request2.destination; - avoidance_result.origin_new = res ? origin_new : avoidance_result.origin_new; - avoidance_result.destination_new = res ? destination_new : avoidance_result.destination; + avoidance_result.origin_new = (res == OA_SUCCESS) ? origin_new : avoidance_result.origin_new; + avoidance_result.destination_new = (res == OA_SUCCESS) ? destination_new : avoidance_result.destination; avoidance_result.result_time_ms = AP_HAL::millis(); avoidance_result.ret_state = res; }