From 29e621b18e06def7ed6bf8cf948697197bda51ee Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 2 Nov 2016 11:17:59 +0100 Subject: [PATCH] added new calcualtions to accel gyro --- UAV-ControlSystem/src/Flight/pid.c | 55 -------------------- UAV-ControlSystem/src/drivers/accel_gyro.c | 60 +++++++++------------- 2 files changed, 24 insertions(+), 91 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index cee2fc7..9b5d0cf 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -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; diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index e8c7e09..f9c40c9 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -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); } }