From 0ce318d2e731ceb4bd0984f1fd8f0a40cc8a2ac8 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 24 Nov 2016 17:11:08 +0100 Subject: [PATCH 1/2] Last fix --- UAV-ControlSystem/src/drivers/accel_gyro.c | 43 ++++++++++++++++++---- UAV-ControlSystem/src/drivers/motormix.c | 2 +- 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 00be629..2a1879d 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -10,6 +10,7 @@ #include "utilities.h" #include "math.h" #include "drivers/system_clock.h" +#include "config/cli.h" spi_profile mpu6000_spi_profile; uint8_t num_failed_receive = 0; @@ -519,14 +520,29 @@ bool mpu6000_who_am_i() return false; } +#define degToRad(x) ((x/180)*M_PI) +#define radToDeg(x) ((x/M_PI)*180) +void gyroEstimation(float p, float q, float r, float dT, accel_t* accel) +{ + p = degToRad(p); + q = degToRad(q); + r = degToRad(r); + float radPitchAngle = degToRad(accel->pitchAngle); + float radRollAngle = degToRad(accel->rollAngle); + float pitchAngleRate = p + q*sinf(radPitchAngle)*tanf(radRollAngle) + r*cosf(radPitchAngle)*tanf(radRollAngle); + float rollAngleRate = q*cosf(radPitchAngle) - r*sinf(radPitchAngle); + + accel->rollAngle = accel->rollAngle + (radToDeg(rollAngleRate)/1000000)*dT; + accel->pitchAngle = accel->pitchAngle + (radToDeg(pitchAngleRate)/1000000)*dT; +} /* Set the Gyro Weight for Gyro/Acc complementary filter Increasing this value would reduce and delay Acc influence on the output of the filter*/ -#define GYRO_ACC_DIV_FACTOR (2^16) // that means a CMP_FACTOR of 1024 (2^10) +#define GYRO_ACC_DIV_FACTOR (float)(2^30) // that means a CMP_FACTOR of 1024 (2^10) #define ACCEL_LPF_FACTOR 16 #define GetMagnitude(x) (x*x) @@ -540,6 +556,8 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) uint32_t current_micros = clock_get_us(); uint32_t delta_t = current_micros - last_micros; last_micros = current_micros; + float a_RollAngle = 0; + float a_PitchAngle = 0; float deltaGyroAngleFloat[3] = {0}; static float lpf_Acc[3] = {0}; @@ -577,21 +595,32 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) deltaGyroAngleFloat[2] = (delta_t * (float)gyro->gyroZ / 1000000.0); //First integrate the gyro and add that to the angle calculation - accel->rollAngle += deltaGyroAngleFloat[1]; - accel->pitchAngle += deltaGyroAngleFloat[0]; +// accel->rollAngle += deltaGyroAngleFloat[1]; +// accel->pitchAngle += deltaGyroAngleFloat[0]; + + //New implementatioin ot apprximate the ange from gyro data + gyroEstimation((float)gyro->gyroX,(float)gyro->gyroY,(float)gyro->gyroZ, delta_t, accel); + //If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation + /*Use complementary filter to add gyro together with acc data to avoid drift over time + * fliter looks like: angle = 0.98*(angle + gyro * dt) + 0.02 * (accdata))*/ if (Low_Magnitude < magnitude && magnitude < High_Magnitude) { //Calculate the pure angle given by the accelerometer data - float a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI; - float a_PitchAngle = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI; + a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI; + a_PitchAngle = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI; //Check how much the accelerometer angle differs from the current calculated ange with the gyro. Add calculated factor to the real angle value - accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; - accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; + //accel->rollAngle += (a_RollAngle - accel->rollAngle) / (float)4000; + //accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / (float)4000; + //Combine the gyro approximation with the accelerometer data to avoid drift + float ratio = 0.98; + accel->rollAngle = (accel->rollAngle * ratio) + (a_RollAngle * (float)(1-ratio)); + accel->pitchAngle = (accel->pitchAngle * ratio) + (a_PitchAngle * (float)(1-ratio)); } + } diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 25ba171..acfd7a6 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -103,7 +103,7 @@ void mix() int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor - int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]*throttleRate; + int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE];//*throttleRate; if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce; From 366bf09086dfce3d17a06c5b810e739430a281ff Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 29 Nov 2016 09:52:55 +0100 Subject: [PATCH 2/2] small changes to settings --- UAV-ControlSystem/src/Flight/pid.c | 2 +- UAV-ControlSystem/src/drivers/accel_gyro.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 6e7a73c..125b7d2 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -30,7 +30,7 @@ #define BAROMETER_SCALE 5 #define RADIO_RANGE 500 /*Radio range input*/ -#define BAROMETER_RANGE 10 /*Determines the range of the maximum height (limits the rc input)*/ +#define BAROMETER_RANGE 4 /*Determines the range of the maximum height (limits the rc input)*/ #define ACCELEROMETER_RANGE 30 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 2a1879d..5dba96e 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -617,7 +617,7 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) //accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / (float)4000; //Combine the gyro approximation with the accelerometer data to avoid drift - float ratio = 0.98; + float ratio = 0.9996; accel->rollAngle = (accel->rollAngle * ratio) + (a_RollAngle * (float)(1-ratio)); accel->pitchAngle = (accel->pitchAngle * ratio) + (a_PitchAngle * (float)(1-ratio)); }