rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-313 Account for which axis are in rate and none to get the desired

Browse Source 
quaternion

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2933 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-03-02 01:25:38 +00:00 committed by peabody124
parent 8930ec01de
commit 915ae1ae3c
1 changed files with 20 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/Modules/Stabilization/stabilization.c
View File

@ -157,16 +157,30 @@ static void stabilizationTask(void* parameters)
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_curr[4];
float q_error[4];
float local_error[3];
// Compute stabilization error as (q_desired' * q_current)', convert to RPY
RPY2Quaternion(&stabDesired.Roll, q_desired);
quat_copy(&attitudeActual.q1, q_curr);
// Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll;
else
rpy_desired[0] = attitudeActual.Roll;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[1] = stabDesired.Pitch;
else
rpy_desired[1] = attitudeActual.Pitch;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[2] = stabDesired.Yaw;
else
rpy_desired[2] = attitudeActual.Yaw;
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, q_curr, q_error);
quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
@ -197,7 +211,7 @@ static void stabilizationTask(void* parameters)
}
}
uint8_t shouldUpdate = 0;
uint8_t shouldUpdate = 1;
RateDesiredSet(&rateDesired);
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
@ -224,8 +238,6 @@ static void stabilizationTask(void* parameters)
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
//actuatorDesiredAxis[ct] = bound(manualAxis[ct]);
//shouldUpdate = 1;
switch (ct)
{
case ROLL: