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

Somehow this task runs quite a few times at the beginning so dT was going to

Browse Source 
zero and making the attitude get NaN.  Wrote recovery code for that condition
(should never occur) and also force minimum dT to 1 ms (also shouldn't occur)
This commit is contained in:
James Cotton 2011-07-12 14:18:19 -05:00
parent 53ca934b2b
commit 2c7cfe435f
1 changed files with 30 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -121,14 +121,29 @@ int32_t AttitudeInitialize(void)
attitude.q4 = 0;
AttitudeActualSet(&attitude);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
// Cannot trust the values to init right above if BL runs
gyro_correct_int[0] = 0;
gyro_correct_int[1] = 0;
gyro_correct_int[2] = 0;
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
for(uint8_t i = 0; i < 3; i++)
for(uint8_t j = 0; j < 3; j++)
R[i][j] = 0;
// Create queue for passing gyro data, allow 2 back samples in case
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
if(gyro_queue == NULL)
return -1;
PIOS_ADC_SetQueue(gyro_queue);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -139,7 +154,6 @@ MODULE_INITCALL(AttitudeInitialize, 0, AttitudeStart, 0, MODULE_EXEC_NOORDER_FLA
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
@ -149,6 +163,7 @@ static void AttitudeTask(void *parameters)
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
@ -259,14 +274,11 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
static void updateAttitude(AttitudeRawData * attitudeRaw)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
static portTickType thisSysTime;
static float dT = 0;
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
@ -326,6 +338,15 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);