From 2ffd179f79631353f9b50a338a8516b56ceb8a66 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 16 Jan 2016 11:39:00 +0100 Subject: [PATCH] LP-214 Calc changes --- flight/modules/ManualControl/stabilizedhandler.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 43f9aa155..8cb6d3156 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -107,10 +107,10 @@ void stabilizedHandler(bool newinit) float yawCommand = cmd.Yaw; // http://shrediquette.blogspot.de/2016/01/some-thoughts-on-camera-tilt.html - // Roll_output = cos(camera_tilt) * Roll_input - sin(camera_tilt) * Yaw_input - // Yaw_output = sin(camera_tilt) * Roll_input + cos(camera_tilt) * Yaw_input - cmd.Roll = boundf((cosAngle * rollCommand) - (sinAngle * yawCommand), -1.0f, 1.0f); - cmd.Yaw = boundf((cosAngle * yawCommand) + (sinAngle * rollCommand), -1.0f, 1.0f); + // When Roll right, add negative Yaw. + // When Yaw left, add negative Roll. + cmd.Roll = boundf((cosAngle * rollCommand) + (sinAngle * yawCommand), -1.0f, 1.0f); + cmd.Yaw = boundf((cosAngle * yawCommand) - (sinAngle * rollCommand), -1.0f, 1.0f); } uint8_t *stab_settings; -- 2.11.4.GIT