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

UAVObjects: Remove the raw versus filtered fields in the sensor data. Save

Browse Source 
some memory.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2706 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-03 02:42:39 +00:00 committed by peabody124
parent c95a6447bf
commit 810fa70856
6 changed files with 40 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
flight/AHRS/ahrs.c
View File

@ -759,34 +759,28 @@ void adc_callback(float * downsampled_data)
AttitudeRawData raw;
raw.accels[0] = downsampled_data[2];
raw.accels[1] = downsampled_data[0];
raw.accels[2] = downsampled_data[4];
raw.gyros[0] = downsampled_data[1];
raw.gyros[1] = downsampled_data[3];
raw.gyros[2] = downsampled_data[5];
raw.gyrotemp[0] = downsampled_data[6];
raw.gyrotemp[1] = downsampled_data[7];
raw.gyros_filtered[0] = gyro[0] * RAD_TO_DEG;
raw.gyros_filtered[1] = gyro[1] * RAD_TO_DEG;
raw.gyros_filtered[2] = gyro[2] * RAD_TO_DEG;
raw.gyros[0] = gyro[0] * RAD_TO_DEG;
raw.gyros[1] = gyro[1] * RAD_TO_DEG;
raw.gyros[2] = gyro[2] * RAD_TO_DEG;
raw.accels_filtered[0] = accel[0];
raw.accels_filtered[1] = accel[1];
raw.accels_filtered[2] = accel[2];
raw.accels[0] = accel[0];
raw.accels[1] = accel[1];
raw.accels[2] = accel[2];
raw.magnetometers[0] = mag_data.scaled.axis[0];
raw.magnetometers[1] = mag_data.scaled.axis[1];
raw.magnetometers[2] = mag_data.scaled.axis[2];
if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
raw.gyros_filtered[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
raw.gyros_filtered[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
raw.gyros_filtered[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
raw.accels_filtered[0] -= Nav.accel_bias[0];
raw.accels_filtered[1] -= Nav.accel_bias[1];
raw.accels_filtered[2] -= Nav.accel_bias[2];
raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
raw.accels[0] -= Nav.accel_bias[0];
raw.accels[1] -= Nav.accel_bias[1];
raw.accels[2] -= Nav.accel_bias[2];
}
AttitudeRawSet(&raw);

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

@ -138,7 +138,6 @@ static void AttitudeTask(void *parameters)
static void initSensors()
{
vTaskDelay(50);
updateSensors();
AttitudeRawData attitudeRaw;
@ -148,9 +147,9 @@ static void initSensors()
AttitudeSettingsGet(&settings);
// Zero initial bias
gyro_correct_int[0] = - attitudeRaw.gyros_filtered[0];
gyro_correct_int[1] = - attitudeRaw.gyros_filtered[1];
gyro_correct_int[2] = - attitudeRaw.gyros_filtered[2];
gyro_correct_int[0] = - attitudeRaw.gyros[0];
gyro_correct_int[1] = - attitudeRaw.gyros[1];
gyro_correct_int[2] = - attitudeRaw.gyros[2];
}
static void updateSensors()
@ -170,43 +169,39 @@ static void updateSensors()
return;
}
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = -(gyro[0] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = -(gyro[2] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros[ATTITUDERAW_GYROS_X] = -(gyro[0] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] = (gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] = -(gyro[2] - GYRO_NEUTRAL) * settings.GyroGain;
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] += gyro_correct_int[0];
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_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_filtered[ATTITUDERAW_GYROS_FILTERED_Z] += gyro_correct_int[2];
gyro_correct_int[2] += - attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_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_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = 0;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = 0;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_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.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] += (float) accel_data.x * 0.004f * 9.81;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] += -(float) accel_data.y * 0.004f * 9.81;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] += -(float) accel_data.z * 0.004f * 9.81;
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_filtered[ATTITUDERAW_ACCELS_FILTERED_X] /= i;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] /= i;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] /= i;
attitudeRaw.accels[ATTITUDERAW_ACCELS_X] = accel_data.x;
attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] = accel_data.y;
attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] = accel_data.z;
attitudeRaw.accels[ATTITUDERAW_ACCELS_X] /= i;
attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] /= i;
attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] /= i;
AttitudeRawSet(&attitudeRaw);
}
@ -234,9 +229,9 @@ static void updateAttitude()
// Bad practice to assume structure order, but saves memory
float * q = &attitudeActual.q1;
float gyro[3] = {attitudeRaw.gyros_filtered[0], attitudeRaw.gyros_filtered[1], attitudeRaw.gyros_filtered[2]};
float * gyro = attitudeRaw.gyros;
{
float * accels = attitudeRaw.accels_filtered;
float * accels = attitudeRaw.accels;
float grot[3];
float accel_err[3];

6
flight/Modules/Guidance/guidance.c
View File

@ -136,9 +136,9 @@ static void guidanceTask(void *parameters)
// Collect downsampled attitude data
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
accel[0] += attitudeRaw.gyros_filtered[0];
accel[1] += attitudeRaw.gyros_filtered[1];
accel[2] += attitudeRaw.gyros_filtered[2];
accel[0] += attitudeRaw.accels[0];
accel[1] += attitudeRaw.accels[1];
accel[2] += attitudeRaw.accels[2];
accel_accum++;
// Continue collecting data if not enough time

2
flight/Modules/Stabilization/stabilization.c
View File

@ -198,7 +198,7 @@ static void stabilizationTask(void* parameters)
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct], attitudeRaw.gyros_filtered[ct], 0);
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct], attitudeRaw.gyros[ct], 0);
actuatorDesiredAxis[ct] = bound(command);
shouldUpdate = 1;
break;

2
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -7367,6 +7367,7 @@
65C35E5612EFB2F3004811C2 /* attitudeactual.xml */,
65C35E5712EFB2F3004811C2 /* attitudedesired.xml */,
65C35E5812EFB2F3004811C2 /* attituderaw.xml */,
65E410AE12F65AEA00725888 /* attitudesettings.xml */,
65C35E5912EFB2F3004811C2 /* baroaltitude.xml */,
65C35E5A12EFB2F3004811C2 /* batterysettings.xml */,
65C35E5B12EFB2F3004811C2 /* firmwareiap.xml */,
@ -7400,7 +7401,6 @@
65C35E7712EFB2F3004811C2 /* velocityactual.xml */,
65C35E7812EFB2F3004811C2 /* velocitydesired.xml */,
65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */,
65E410AE12F65AEA00725888 /* attitudesettings.xml */,
);
path = uavobjectdefinition;
sourceTree = "<group>";

6
shared/uavobjectdefinition/attituderaw.xml
View File

@ -2,11 +2,9 @@
<object name="AttitudeRaw" singleinstance="true" settings="false">
<description>The raw attitude sensor data from @ref AHRSCommsModule. Not always updated.</description>
<field name="magnetometers" units="mGa" type="int16" elementnames="X,Y,Z"/>
<field name="gyros" units="raw" type="int16" elementnames="X,Y,Z"/>
<field name="gyros_filtered" units="deg/s" type="float" elementnames="X,Y,Z"/>
<field name="gyros" units="deg/s" type="float" elementnames="X,Y,Z"/>
<field name="gyrotemp" units="raw" type="uint16" elementnames="XY,Z"/>
<field name="accels" units="raw" type="int16" elementnames="X,Y,Z"/>
<field name="accels_filtered" units="m/s" type="float" elementnames="X,Y,Z"/>
<field name="accels" units="m/s" type="float" elementnames="X,Y,Z"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>