From 88f09eef257488a414d8a49b59009597a6bfecdf Mon Sep 17 00:00:00 2001 From: philsson Date: Wed, 2 Nov 2016 09:22:22 +0100 Subject: [PATCH] First commit to new acc implementation --- UAV-ControlSystem/inc/drivers/accel_gyro.h | 3 + UAV-ControlSystem/src/Flight/pid.c | 71 +++++++++---------- UAV-ControlSystem/src/drivers/accel_gyro.c | 82 ++++++++++++++++++++++ 3 files changed, 117 insertions(+), 39 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h index 805964d..d1c6560 100644 --- a/UAV-ControlSystem/inc/drivers/accel_gyro.h +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -114,6 +114,7 @@ typedef struct accel_t { int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */ int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */ uint16_t accel1G; /* Sensitivity factor */ + float rollAngle, pitchAngle; } accel_t; /*********************************************************************** @@ -164,6 +165,8 @@ int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out); ***********************************************************************/ bool mpu6000_who_am_i(); +void mpu6000_read_angle(accel_t* accel, gyro_t* gyro); + #endif /* DRIVERS_ACCEL_GYRO_H_ */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index c16c8ec..cee2fc7 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -65,6 +65,7 @@ float accPitchFineTune = 0; +//TODO: Remove as implemented in accel_gyro /************************************************************************** * BRIEF: Calculates angle from accelerometer * * INFORMATION: * @@ -151,56 +152,48 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_ACCELEROMETER: - mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ - -// -// if (calcGravity(accelProfile) > 1.15) -// { -// -// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT; -// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT; -// -// } -// else -// { + 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); +// 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; +// 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]; +// oldSensorValueRoll[i] = X_roll; +// oldSensorValuePitch[i] = X_pitch; // - oldSensorValue[0] = sensorValues[ROLL]; - oldSensorValue[1] = sensorValues[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); diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 759dcc5..e8c7e09 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -7,6 +7,8 @@ #include #include "drivers/spi.h" +#include "utilities.h" +#include "math.h" spi_profile mpu6000_spi_profile; uint8_t num_failed_receive = 0; @@ -514,3 +516,83 @@ bool mpu6000_who_am_i() return false; } + + +/* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */ +void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) +{ + static uint32_t last_micros = 0; // Static stores micros measured from last iteration + uint32_t current_micros = clock_get_us(); + uint32_t delta_t = current_micros - last_micros; + last_micros = current_micros; + + static float lpf_Acc[3] = {0}; + static float smooth[3] = {0}; + + /* We read the accelerometer to get fresh data */ + mpu6000_read_accel(accel); + + /* 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]; + + accel->accelXconv = smooth[0] * smooth[0]; + accel->accelYconv = smooth[1] * smooth[1]; + accel->accelZconv = smooth[2] * smooth[2]; + + /* The total magnitude of the acceleration */ + float magnitude = sqrtf( \ + ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ + ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ + ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ + ); + + /* 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; + + + // 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; + } + +}