rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

CC-3 Remove unused code from CCAltitude

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2572 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-24 07:51:56 +00:00 committed by peabody124
parent 2881370eab
commit e16b1bffa9
1 changed files with 15 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
flight/Modules/CCAttitude/ccattitude.c
View File

@ -62,7 +62,7 @@
#define UPDATE_RATE 2 /* ms */
#define GYRO_NEUTRAL 1665
#define GYRO_SCALE 0.010f
#define GYRO_SCALE (0.010f * 180 / M_PI)
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
// Private types
@ -118,24 +118,14 @@ static void CCAttitudeTask(void *parameters)
}
}
void updateInput()
{
ManualControlCommandData manual;
ManualControlCommandGet(&manual);
manual.Throttle = (float) (PIOS_PWM_Get(0) - 1100.0f) / 900.0f;
manual.Roll = (float) (PIOS_PWM_Get(1) - 1500.0f) / 500.0f;
manual.Pitch = (float) (PIOS_PWM_Get(2) - 1500.0f) / 500.0f;
manual.Yaw = (float) (PIOS_PWM_Get(3) - 1500.0f) / 500.0f;
manual.FlightMode = (float) (PIOS_PWM_Get(4) - 1500) / 500;
ManualControlCommandSet(&manual);
}
void updateSensors()
{
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
struct pios_adxl345_data accel_data;
static float gyro_bias[3] = {0,0,0};
static const float tau = 0.999f;
attitudeRaw.gyros[ATTITUDERAW_GYROS_X] = PIOS_ADC_PinGet(1);
attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] = PIOS_ADC_PinGet(2);
@ -145,6 +135,14 @@ void updateSensors()
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] - GYRO_NEUTRAL) * GYRO_SCALE;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = (attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] - GYRO_NEUTRAL) * GYRO_SCALE;
gyro_bias[0] = tau * gyro_bias[0] + (1-tau) * attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X];
gyro_bias[1] = tau * gyro_bias[1] + (1-tau) * attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y];
gyro_bias[2] = tau * gyro_bias[2] + (1-tau) * attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z];
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] -= gyro_bias[0];
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] -= gyro_bias[1];
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] -= gyro_bias[2];
attitudeRaw.gyrotemp[0] = PIOS_ADXL345_Read(&accel_data);
attitudeRaw.accels[ATTITUDERAW_ACCELS_X] = accel_data.x;
@ -183,9 +181,9 @@ void updateAttitude()
attitudeActual.Yaw = attitudeActual.Yaw * M_PI / 180;
// Integrate gyros
attitudeActual.Roll = PI_MOD(attitudeActual.Roll + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] * dT);
attitudeActual.Pitch = PI_MOD(attitudeActual.Pitch + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] * dT);
attitudeActual.Yaw += fmod(attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] * dT, 2 * M_PI);
attitudeActual.Roll = PI_MOD(attitudeActual.Roll + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] * dT * M_PI / 180);
attitudeActual.Pitch = PI_MOD(attitudeActual.Pitch + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] * dT * M_PI / 180);
attitudeActual.Yaw += attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] * dT * M_PI / 180;
// Compute gravity sense of ground
accel_roll = atan2(-attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y],
@ -199,7 +197,7 @@ void updateAttitude()
// Weighted average and back into degrees
attitudeActual.Roll = (UPDATE_FRAC * attitudeActual.Roll + (1-UPDATE_FRAC) * accel_roll) * 180 / M_PI;
attitudeActual.Pitch = (UPDATE_FRAC * attitudeActual.Pitch + (1-UPDATE_FRAC) * accel_pitch) * 180 / M_PI;
attitudeActual.Yaw = attitudeActual.Yaw * 180 / M_PI;
attitudeActual.Yaw = fmod(attitudeActual.Yaw * 180 / M_PI, 360);
AttitudeActualSet(&attitudeActual);
}