philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added new calcualtions to accel gyro

Browse Source
This commit is contained in:
johan9107 2016-11-02 11:17:59 +01:00
parent 88f09eef25
commit 29e621b18e
2 changed files with 24 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
UAV-ControlSystem/src/Flight/pid.c
View File

@ -154,57 +154,9 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/
float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/
// float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
// float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
/*TODO add finetune for roll and pitch*/
// X_roll += accRollFineTune;
// X_pitch += accPitchFineTune;
sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune;
sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
// oldSensorValueRoll[i] = X_roll;
// oldSensorValuePitch[i] = X_pitch;
//
// float RollValue = 0;
// float PitchValue = 0;
//
// for (int ii = 0; ii < 12; ii++)
// {
// RollValue = RollValue + oldSensorValueRoll[ii];
// PitchValue = PitchValue + oldSensorValuePitch[ii];
//
// }
//
//
// i = (i < 11)? i + 1:0;
// sensorValues[ROLL] = RollValue/12;
// sensorValues[PITCH] = PitchValue/12;
//
// sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0];
// sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1];
////
// oldSensorValue[0] = sensorValues[ROLL];
// oldSensorValue[1] = sensorValues[PITCH];
//
// sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
// sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT);
//float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT);
break;
case PID_ID_COMPASS:
case PID_ID_BAROMETER:
@ -307,7 +259,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I) moved before integration to make limiting independent from PID settings
ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
// Anti windup protection
@ -320,12 +271,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
}
// if (motorLimitReached)
// {
// ITerm = pidProfileBuff->lastITerm[axis];
// }
pidProfileBuff->lastITerm[axis] = ITerm;

60
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

@ -528,6 +528,7 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
static float lpf_Acc[3] = {0};
static float smooth[3] = {0};
float sign[3] = {0};
/* We read the accelerometer to get fresh data */
mpu6000_read_accel(accel);
@ -535,15 +536,26 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
/* Filter part */
for (int i = 0; i < 3; i ++)
{
smooth[i] = lpf_Acc[i] / 16;
}
lpf_Acc[0] += sqrtf(accel->accelXconv) - smooth[0];
lpf_Acc[1] += sqrtf(accel->accelYconv) - smooth[1];
lpf_Acc[2] += sqrtf(accel->accelZconv) - smooth[2];
sign[0] = (accel->accelXconv< 0) ? -1 : 1;
lpf_Acc[0] += sign[0]*sqrtf(ABS_FLOAT(accel->accelXconv)) - smooth[0];
accel->accelXconv = smooth[0] * smooth[0];
accel->accelYconv = smooth[1] * smooth[1];
accel->accelZconv = smooth[2] * smooth[2];
sign[1] = (accel->accelYconv< 0) ? -1 : 1;
lpf_Acc[1] += sign[1]*sqrtf(ABS_FLOAT(accel->accelYconv)) - smooth[1];
sign[2] = (accel->accelZconv< 0) ? -1 : 1;
lpf_Acc[2] += sign[2]*sqrtf(ABS_FLOAT(accel->accelZconv)) - smooth[2];
accel->accelXconv = smooth[0] * smooth[0] * sign[0];
accel->accelYconv = smooth[1] * smooth[1] * sign[1];
accel->accelZconv = smooth[2] * smooth[2] * sign[2];
/* The total magnitude of the acceleration */
float magnitude = sqrtf( \
@ -555,44 +567,20 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
/* Everything is normal. No outer forces */
if (0.85 < magnitude && magnitude < 1.15)
{
//TODO: JOHAN FIXAR! Kolla så det är rätt här hela vägen
// Roll
accel->rollAngle = atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI;
if (accel->rollAngle > 0)
{
if (accel->accelYconv < 0 )
accel->rollAngle = 180 - accel->rollAngle;
}
else
{
if (accel->accelYconv < 0 )
accel->rollAngle = - 180 - accel->rollAngle;
}
accel->rollAngle = -accel->rollAngle;
accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI;
// Pitch
accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI;
if (accel->pitchAngle > 0)
{
if (accel->accelYconv < 0)
accel->pitchAngle = 180 - accel->pitchAngle;
}
else
{
if (accel->accelYconv < 0 )
accel->pitchAngle = - 180 - accel->pitchAngle;
}
}
/* Too big forces to calculate on ACC data. Fallback on gyro integration */
else
{
accel->rollAngle += (float)(delta_t * gyro->gyroX) / 1000000.0;
accel->pitchAngle += (float)(delta_t * gyro->gyroY) / 1000000.0;
accel->rollAngle += (float)(delta_t * gyro->gyroY) / 1000000.0;
accel->pitchAngle += (float)(delta_t * gyro->gyroX) / 1000000.0;
accel->rollAngle = constrainf(accel->rollAngle, -90,90);
accel->pitchAngle = constrainf(accel->pitchAngle, -90,90);
}
}