From 03fb82ad0abe24ab7369a3c576e7a4c4df4c3e78 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Tue, 8 Feb 2011 16:34:18 +0000 Subject: [PATCH] CC Attitude: Change when AttitudeRaw is set to end of attitude estimation, because this is what is determines when the Stabilization code executes. Because we aren't oversampling gyros relative to the PID in CC we should set the attitude first (computational time to do so is negligible). Also change update rate to 500 Hz. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2750 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/Modules/Attitude/attitude.c | 75 ++++++++++++------------- flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 5 +- 2 files changed, 38 insertions(+), 42 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index e624e35f5..0d3a82141 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -62,7 +62,7 @@ #define STACK_SIZE_BYTES 540 #define TASK_PRIORITY (tskIDLE_PRIORITY+3) -#define UPDATE_RATE 3 +#define UPDATE_RATE 2.0f #define GYRO_NEUTRAL 1665 #define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI) @@ -78,8 +78,8 @@ static float gyro_correct_int[3] = {0,0,0}; static xQueueHandle gyro_queue; static void initSensors(); -static void updateSensors(); -static void updateAttitude(); +static void updateSensors(AttitudeRawData *); +static void updateAttitude(AttitudeRawData *); /** * Initialise the module, called on startup @@ -117,7 +117,7 @@ static void AttitudeTask(void *parameters) AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - PIOS_ADC_Config(PIOS_ADC_RATE / (1000 / UPDATE_RATE)); + PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); // Keep flash CS pin high while talking accel PIOS_FLASH_DISABLE; @@ -129,17 +129,21 @@ static void AttitudeTask(void *parameters) while (1) { PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); - updateSensors(); - updateAttitude(); + AttitudeRawData attitudeRaw; + AttitudeRawGet(&attitudeRaw); + updateSensors(&attitudeRaw); + updateAttitude(&attitudeRaw); + AttitudeRawSet(&attitudeRaw); + } } static void initSensors() { - updateSensors(); AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); + updateSensors(&attitudeRaw); AttitudeSettingsData settings; AttitudeSettingsGet(&settings); @@ -150,11 +154,8 @@ static void initSensors() gyro_correct_int[2] = - attitudeRaw.gyros[2]; } -static void updateSensors() -{ - AttitudeRawData attitudeRaw; - AttitudeRawGet(&attitudeRaw); - +static void updateSensors(AttitudeRawData * attitudeRaw) +{ AttitudeSettingsData settings; AttitudeSettingsGet(&settings); @@ -169,54 +170,50 @@ static void updateSensors() // 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) * settings.GyroGain; + attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * settings.GyroGain; + attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * settings.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]; - attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1]; + attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0]; + attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1]; // 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] * + attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2]; + gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * settings.AccelKI * UPDATE_RATE / 1000; // Get the accel data uint8_t i = 0; - attitudeRaw.accels[ATTITUDERAW_ACCELS_X] = 0; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] = 0; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] = 0; + attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = 0; + attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = 0; + attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = 0; do { i++; - attitudeRaw.gyrotemp[0] = PIOS_ADXL345_Read(&accel_data); + attitudeRaw->gyrotemp[0] = PIOS_ADXL345_Read(&accel_data); - attitudeRaw.accels[ATTITUDERAW_ACCELS_X] += (float) accel_data.x * 0.004f * 9.81; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] += -(float) accel_data.y * 0.004f * 9.81; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] += -(float) accel_data.z * 0.004f * 9.81; - } while ( (i < 32) && (attitudeRaw.gyrotemp[0] > 0) ); - attitudeRaw.gyrotemp[1] = i; + attitudeRaw->accels[ATTITUDERAW_ACCELS_X] += (float) accel_data.x * 0.004f * 9.81; + attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] += -(float) accel_data.y * 0.004f * 9.81; + attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] += -(float) accel_data.z * 0.004f * 9.81; + } while ( (i < 32) && (attitudeRaw->gyrotemp[0] > 0) ); + attitudeRaw->gyrotemp[1] = i; - attitudeRaw.accels[ATTITUDERAW_ACCELS_X] /= i; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] /= i; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] /= i; + attitudeRaw->accels[ATTITUDERAW_ACCELS_X] /= i; + attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] /= i; + attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] /= i; - AttitudeRawSet(&attitudeRaw); } -static void updateAttitude() +static void updateAttitude(AttitudeRawData * attitudeRaw) { AttitudeSettingsData settings; AttitudeSettingsGet(&settings); AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); - - AttitudeRawData attitudeRaw; - AttitudeRawGet(&attitudeRaw); - + static portTickType lastSysTime = 0; static portTickType thisSysTime; @@ -229,9 +226,9 @@ static void updateAttitude() // Bad practice to assume structure order, but saves memory float * q = &attitudeActual.q1; - float * gyro = attitudeRaw.gyros; + float * gyro = attitudeRaw->gyros; { - float * accels = attitudeRaw.accels; + float * accels = attitudeRaw->accels; float grot[3]; float accel_err[3]; diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 29a243c1b..1d93798cf 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -221,9 +221,8 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 // Currently analog acquistion hard coded at 480 Hz // PCKL2 = HCLK / 16 // ADCCLK = PCLK2 / 2 -#define PIOS_ADC_RATE (72.0e6 / 1 / 8 / 252 / (PIOS_ADC_NUM_ADC_CHANNELS >> PIOS_ADC_USE_ADC2)) -#define EKF_RATE (PIOS_ADC_RATE / adc_oversampling / 2) -#define PIOS_ADC_MAX_OVERSAMPLING 55 +#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_MAX_OVERSAMPLING 36 //------------------------- // Receiver PWM inputs