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

fast

Browse Source
This commit is contained in:
Jonas Holmberg 2016-11-18 14:05:18 +01:00
parent 2a87aa499e
commit 4986238d1e
4 changed files with 14 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -374,6 +374,7 @@ void pidRun(uint8_t ID)
PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
flagAccBuff = 0; flagAccBuff = 0;
throttleRate = 1;
} }
else else
{ {

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

@ -527,6 +527,7 @@ bool mpu6000_who_am_i()
/* Set the Gyro Weight for Gyro/Acc complementary filter /* Set the Gyro Weight for Gyro/Acc complementary filter
Increasing this value would reduce and delay Acc influence on the output of the 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^4) // that means a CMP_FACTOR of 1024 (2^10)
#define ACCEL_LPF_FACTOR 16
#define GetMagnitude(x) (x*x) #define GetMagnitude(x) (x*x)
#define Low_Magnitude (GetMagnitude(0.85)) #define Low_Magnitude (GetMagnitude(0.85))
@ -554,7 +555,7 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
for (int i = 0; i < 3; i ++) for (int i = 0; i < 3; i ++)
{ {
//Calculate a new smooth value based on a factor of the LPF value //Calculate a new smooth value based on a factor of the LPF value
smooth[i] = lpf_Acc[i] / 16; smooth[i] = lpf_Acc[i] / ACCEL_LPF_FACTOR;
//Save the sign(+/-) of the value //Save the sign(+/-) of the value
sign[i] = (accelConv[i]< 0) ? -1 : 1; sign[i] = (accelConv[i]< 0) ? -1 : 1;
@ -587,8 +588,13 @@ 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; 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 //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; // TODO: This is the original code. Test to set factors for them separatelly
accel->pitchAngle += (a_PitchAngle - 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;
accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR);
accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR);
} }
//Always constran the angular values within the possible ranges 90 to -90 //Always constran the angular values within the possible ranges 90 to -90

2
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -103,7 +103,7 @@ void mix()
int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array 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_Min = 0; // Stores the minimum desired command for any motor
int16_t RPY_Mix_Max = 0; // Maximum 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; if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce;

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

@ -256,9 +256,9 @@ void systemTaskSonar(void)
void systemTaskAltitude(void) void systemTaskAltitude(void)
{ {
uint8_t c[50]; // uint8_t c[50];
sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle); // sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle);
usart_transmit(&cliUsart, &c, 50, 1000000000); // usart_transmit(&cliUsart, &c, 50, 1000000000);
//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar //Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar