From b53bf19c8df5623130de22ddc67596c7d71685cd Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 27 Mar 2019 08:11:54 +0300 Subject: [PATCH] simple_offboard: comment out incorrect yaw calculation if yaw=nan --- clever/src/simple_offboard.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/clever/src/simple_offboard.cpp b/clever/src/simple_offboard.cpp index f0e1f7cf..3af21297 100644 --- a/clever/src/simple_offboard.cpp +++ b/clever/src/simple_offboard.cpp @@ -528,12 +528,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl nav_speed = speed; } - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { - if (std::isnan(yaw) && yaw_rate == 0) { - // keep yaw unchanged - yaw = tf2::getYaw(local_position.pose.orientation); - } - } + // if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { + // if (std::isnan(yaw) && yaw_rate == 0) { + // // keep yaw unchanged + // // TODO: this is incorrect, because we need yaw in desired frame + // yaw = tf2::getYaw(local_position.pose.orientation); + // } + // } if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) { // destination point and/or yaw