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

CC-20 Attitude: Jack up Kp and Ki values for first 5 seconds to force faster

Browse Source 
convergence of gyro bias, then the normal values can be lower.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2753 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-08 16:34:28 +00:00 committed by peabody124
parent 6ac2ddb0c1
commit 74cc7dbfc9
1 changed files with 37 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,9 +77,13 @@ static void AttitudeTask(void *parameters);
static float gyro_correct_int[3] = {0,0,0};
static xQueueHandle gyro_queue;
static void initSensors();
static void updateSensors(AttitudeRawData *);
static void updateAttitude(AttitudeRawData *);
static void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0;
static float accelKp = 0;
static float gyroGain = 0.42;
/**
* Initialise the module, called on startup
@ -102,6 +106,8 @@ int32_t AttitudeInitialize(void)
return -1;
PIOS_ADC_SetQueue(gyro_queue);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
@ -115,6 +121,7 @@ int32_t AttitudeInitialize(void)
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
@ -122,11 +129,19 @@ static void AttitudeTask(void *parameters)
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
initSensors();
// Main task loop
while (1) {
if(xTaskGetTickCount() < 5000) {
// For first 5 seconds use accels to get gyro bias
accelKi = 10;
accelKp = 1;
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
@ -138,27 +153,8 @@ static void AttitudeTask(void *parameters)
}
}
static void initSensors()
{
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
AttitudeSettingsData settings;
AttitudeSettingsGet(&settings);
// Zero initial bias
gyro_correct_int[0] = - attitudeRaw.gyros[0];
gyro_correct_int[1] = - attitudeRaw.gyros[1];
gyro_correct_int[2] = - attitudeRaw.gyros[2];
}
static void updateSensors(AttitudeRawData * attitudeRaw)
{
AttitudeSettingsData settings;
AttitudeSettingsGet(&settings);
struct pios_adxl345_data accel_data;
float gyro[4];
@ -170,9 +166,9 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
// First sample is temperature
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
@ -181,7 +177,7 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
// Because most crafts wont get enough information from gravity to zero yaw gyro
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] *
settings.AccelKI * UPDATE_RATE / 1000;
accelKi * UPDATE_RATE / 1000;
// Get the accel data
@ -208,9 +204,6 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
static void updateAttitude(AttitudeRawData * attitudeRaw)
{
AttitudeSettingsData settings;
AttitudeSettingsGet(&settings);
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
@ -239,14 +232,14 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
CrossProduct((const float *) accels, (const float *) grot, accel_err);
// Accumulate integral of error. Scale here so that units are rad/s
gyro_correct_int[0] += accel_err[0] * settings.AccelKI * dT;
gyro_correct_int[1] += accel_err[1] * settings.AccelKI * dT;
gyro_correct_int[0] += accel_err[0] * accelKi * dT;
gyro_correct_int[1] += accel_err[1] * accelKi * dT;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
// Correct rates based on error, integral component dealt with in updateSensors
gyro[0] += accel_err[0] * settings.AccelKp;
gyro[1] += accel_err[1] * settings.AccelKp;
gyro[2] += accel_err[2] * settings.AccelKp;
gyro[0] += accel_err[0] * accelKp;
gyro[1] += accel_err[1] * accelKp;
gyro[2] += accel_err[2] * accelKp;
}
{ // scoping variables to save memory
@ -282,6 +275,15 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
AttitudeActualSet(&attitudeActual);
}
static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKI;
gyroGain = attitudeSettings.GyroGain;
}
/**
* @}
* @}