rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

CC-24: Support mounting CC at any angle by applying a rotation. Now need to

Browse Source 
rotate sensors to make sure stabilization behaves well so don't use till then.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3100 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-03-29 02:15:57 +00:00 committed by peabody124
parent cebca6f67e
commit a57c289b3a
2 changed files with 49 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
flight/Modules/Attitude/attitude.c
View File

@ -84,6 +84,9 @@ static float accelKp = 0;
static float yawBiasRate = 0;
static float gyroGain = 0.42;
static int16_t accelbias[3];
static float rotationQuat[4] = {1,0,0,0};
static float q[4] = {1,0,0,0};
static int8_t rotate = 0;
/**
* Initialise the module, called on startup
@ -202,13 +205,14 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = ((float)x * 0.004f * 9.81f) / i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = ((float)y * 0.004f * 9.81f) / i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = ((float)z * 0.004f * 9.81f) / i;
if(rotate) {
// TODO: rotate sensors too so stabilization is well behaved
}
}
static void updateAttitude(AttitudeRawData * attitudeRaw)
{
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
static portTickType lastSysTime = 0;
static portTickType thisSysTime;
@ -220,7 +224,6 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
float * q = &attitudeActual.q1;
float gyro[3];
gyro[0] = attitudeRaw->gyros[0];
gyro[1] = attitudeRaw->gyros[1];
@ -277,13 +280,18 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
attitudeActual.q1 = q[0];
attitudeActual.q2 = q[1];
attitudeActual.q3 = q[2];
attitudeActual.q4 = q[3];
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
if(rotate) {
// Rotate the attitude q from the state q to allow board to be mounted at weird angles
quat_mult(q,rotationQuat,&attitudeActual.q1);
} else {
quat_copy(q, &attitudeActual.q1);
}
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(q,&attitudeActual.Roll);
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
}
@ -301,6 +309,37 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
// Indicates not to expend cycles on rotation
if(attitudeSettings.RotationQuaternion[0] == 126) {
rotate = 0;
rotationQuat[0] = 1;
rotationQuat[1] = rotationQuat[2] = rotationQuat[3] = 0;
return;
}
rotate = 1;
rotationQuat[0] = (float) attitudeSettings.RotationQuaternion[0] / 127.0;
rotationQuat[1] = (float) attitudeSettings.RotationQuaternion[1] / 127.0f;
rotationQuat[2] = (float) attitudeSettings.RotationQuaternion[2] / 127.0f;
rotationQuat[3] = (float) attitudeSettings.RotationQuaternion[3] / 127.0f;
float len = sqrt(rotationQuat[0] * rotationQuat[0] + rotationQuat[1] * rotationQuat[1] +
rotationQuat[2] * rotationQuat[2] + rotationQuat[3] * rotationQuat[3]);
// Sanitize input. Shouldn't do anything functional.
if(len < 1e-3) {
// Really wrong value
rotationQuat[0] = 1;
rotationQuat[1] = rotationQuat[2] = rotationQuat[3] = 0;
rotate = 0;
} else {
rotationQuat[0] /= len;
rotationQuat[1] /= len;
rotationQuat[2] /= len;
rotationQuat[3] /= len;
}
}
/**
* @}

1
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -2,6 +2,7 @@
<object name="AttitudeSettings" singleinstance="true" settings="true">
<description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="AccelBias" units="lsb" type="int16" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="RotationQuaternion" units="quaternion * 255" type="int8" elementnames="q1,q2,q3,q4" defaultvalue="127,0,0,0"/>
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.01"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>