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

Merge remote-tracking branch 'refs/remotes/origin/master' into Compass

Browse Source
This commit is contained in:
Lennart Eriksson 2016-11-29 15:01:51 +01:00
commit 26982bf938
3 changed files with 38 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -34,7 +34,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)*/

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

@ -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.9996;
accel->rollAngle = (accel->rollAngle * ratio) + (a_RollAngle * (float)(1-ratio));
accel->pitchAngle = (accel->pitchAngle * ratio) + (a_PitchAngle * (float)(1-ratio));
}
}

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_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;