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

Improved angle update to accelerometer

Browse Source
This commit is contained in:
johan9107 2016-11-21 11:17:00 +01:00
parent 4986238d1e
commit 16b05ee206
4 changed files with 14 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
UAV-ControlSystem/inc/Flight/pid.h
View File

@ -84,6 +84,8 @@ void pidInit();
**************************************************************************/
void pidRun(uint8_t ID);
void readAcc(void);
void pidEproom(void);
#endif /* FLIGHT_PID_H_ */

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

@ -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----------------------------------------------------------------------------------*/
/**************************************************************************

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

@ -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);
}

14
UAV-ControlSystem/src/tasks_main.c
View File

@ -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)