From 16b05ee20668ac9d0396f8ceb81e2f50e12264ec Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 21 Nov 2016 11:17:00 +0100 Subject: [PATCH] Improved angle update to accelerometer --- UAV-ControlSystem/inc/Flight/pid.h | 2 ++ UAV-ControlSystem/src/Flight/pid.c | 14 ++++++++------ UAV-ControlSystem/src/drivers/accel_gyro.c | 14 +++----------- UAV-ControlSystem/src/tasks_main.c | 14 +------------- 4 files changed, 14 insertions(+), 30 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index ac014ac..fd6d62a 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -84,6 +84,8 @@ void pidInit(); **************************************************************************/ void pidRun(uint8_t ID); +void readAcc(void); + void pidEproom(void); #endif /* FLIGHT_PID_H_ */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 366a9a8..6e7a73c 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -31,7 +31,7 @@ #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 10 /*Determines the range of the maximum height (limits the rc input)*/ -#define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ +#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)*/ @@ -69,7 +69,7 @@ float accRollFineTune = 0; float accPitchFineTune = 0; float throttleRate = 1; -int HoverForce = 1475;/*Struct profile for input data from sensor*/ +int HoverForce = 1475; /*Struct profile for input data from sensor*/ /************************************************************************** * BRIEF: Scales data from input range to output range * @@ -109,8 +109,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_ACCELEROMETER: - mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/ - sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; @@ -245,7 +243,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, } } - /* -----calculate I component ---- */ float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis]; @@ -341,7 +338,6 @@ void pidAccelerometer(void) PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; counterAcc +=1; } - } /************************************************************************** @@ -418,6 +414,12 @@ void pidRun(uint8_t ID) } } +void readAcc(void) +{ + /*Reads data from accelerometer*/ + mpu6000_read_angle(&accelProfile,&gyroProfile); +} + /*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/ /************************************************************************** diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 2a40d42..f0b87cc 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -526,7 +526,7 @@ bool mpu6000_who_am_i() /* 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^4) // that means a CMP_FACTOR of 1024 (2^10) +#define GYRO_ACC_DIV_FACTOR (2^16) // that means a CMP_FACTOR of 1024 (2^10) #define ACCEL_LPF_FACTOR 16 #define GetMagnitude(x) (x*x) @@ -588,16 +588,8 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) float 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 -// TODO: This is the original code. Test to set factors for them separatelly -// accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; -// accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; - - - accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR); - accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR); + accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; + accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; } - //Always constran the angular values within the possible ranges 90 to -90 - accel->rollAngle = constrainf(accel->rollAngle, -90,90); - accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 09e657d..d5ab7b5 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -46,18 +46,10 @@ void systemTaskGyroPid(void) { - //Read gyro and update PID and finally update the motors. The most important task in the system - - //Update Gyro - - //Convert? //PID Gyro pidRun(PID_ID_GYRO); - //MIX GO - - //call the motor mix mix(); } @@ -65,12 +57,8 @@ void systemTaskGyroPid(void) void systemTaskAccelerometer(void) { + readAcc(); pidRun(PID_ID_ACCELEROMETER); - //update the accelerometer data -// uint8_t c = 97; -// usart_transmit(&cliUsart, &c, 1, 1000000000); - - } void systemTaskAttitude(void)