rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-24 13:54:16 +01:00
Code Issues Releases Activity

Fix initialization of the complimentary filter where it didn't get the right

Browse Source 
initial dT
This commit is contained in:
James Cotton 2012-05-10 00:29:23 -05:00
parent a857530832
commit 98655aacc8
1 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
flight/Modules/Attitude/revolution/attitude.c
View File

@ -269,24 +269,26 @@ static int32_t updateAttitudeComplimentary(bool first_run)
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
if(first_run) { if(first_run) {
// To initialize we need a valid mag reading
if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
return -1;
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
MagnetometerData magData; MagnetometerData magData;
MagnetometerGet(&magData); MagnetometerGet(&magData);
init = 0; init = 0;
float rpy[3]; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
float q[4]; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
rpy[2] = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(rpy,q); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
timeval = PIOS_DELAY_GetRaw();
return 0;
} }
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
@ -306,7 +308,7 @@ static int32_t updateAttitudeComplimentary(bool first_run)
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
magKp = 0.01f; magKp = 0.01f;
init = 1; init = 1;
} }
GyrosGet(&gyrosData); GyrosGet(&gyrosData);