From 69c9f22dadedf6eeebb7a53bf21589969cd6e495 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 26 Oct 2016 16:22:33 +0200 Subject: [PATCH] Pid update angle caluclations --- UAV-ControlSystem/src/Flight/pid.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 2ddce64..065ad46 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -65,15 +65,21 @@ pt1Filter_t accelFilter[2] = {0}; float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis) { float angle; + float angle_offset = (z_axis < 0 )? 90: 0; switch (axis) { case ROLL: - angle = atan2(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90; - angle = (x_axis < 0) ? -angle : angle; /*CW (right, form behind) = pos angle*/ + + angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI; + angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle); + break; case PITCH: - angle = atan2(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; /*down (the front down against ground) = pos angle*/ + + angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; /*down (the front down against ground) = pos angle*/ + angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle; + break; default: angle = 0;