philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Pid update angle caluclations

Browse Source
This commit is contained in:
johan9107 2016-10-26 16:22:33 +02:00
parent 8b511b1098
commit 69c9f22dad
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
UAV-ControlSystem/src/Flight/pid.c
View File

@ -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;