rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +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) #if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory. // Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4]; float q_desired[4];
float q_curr[4];
float q_error[4]; float q_error[4];
float local_error[3]; float local_error[3];
// Compute stabilization error as (q_desired' * q_current)', convert to RPY // Essentially zero errors for anything in rate or none
RPY2Quaternion(&stabDesired.Roll, q_desired); if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
quat_copy(&attitudeActual.q1, q_curr); 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_inverse(q_desired);
quat_mult(q_desired, q_curr, q_error); quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error); quat_inverse(q_error);
Quaternion2RPY(q_error, local_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); RateDesiredSet(&rateDesired);
ActuatorDesiredGet(&actuatorDesired); ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command //Calculate desired command
@ -224,8 +238,6 @@ static void stabilizationTask(void* parameters)
break; break;
} }
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
//actuatorDesiredAxis[ct] = bound(manualAxis[ct]);
//shouldUpdate = 1;
switch (ct) switch (ct)
{ {
case ROLL: case ROLL: