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

Compass starting to be better

Browse Source
This commit is contained in:
Lennart Eriksson 2016-11-25 11:36:11 +01:00
parent dc2bc678e4
commit 6eabf72e8f
3 changed files with 83 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
UAV-ControlSystem/.custom.cfg Normal file
View File

@ -0,0 +1,13 @@
# This is an .custom board with a single STM32F411RETx chip.
# Generated by System Workbench for STM32
source [find interface/stlink-v2-1.cfg]
set WORKAREASIZE 0x20000
transport select "hla_swd"
source [find target/stm32f4x_stlink.cfg]
# use hardware reset, connect under reset
reset_config srst_only srst_nogate

18
UAV-ControlSystem/inc/drivers/arduino_com.h
View File

@ -19,11 +19,11 @@
* INFORMATION: Contains the whole compass message *
***********************************************************************/
typedef struct compass_data_t {
uint8_t header;
int16_t x;
int16_t y;
int16_t z;
uint8_t crc;
uint8_t header __attribute__((packed));
int16_t x __attribute__((packed));
int16_t y __attribute__((packed));
int16_t z __attribute__((packed));
uint8_t crc __attribute__((packed));
} compass_data_t;
/***********************************************************************
@ -31,10 +31,10 @@ typedef struct compass_data_t {
* INFORMATION: Contains the whole gps data message *
***********************************************************************/
typedef struct gps_data_t {
uint8_t header;
float latitude;
float longitude;
uint8_t crc;
uint8_t header __attribute__((packed));
float latitude; __attribute__((packed))
float longitude __attribute__((packed));
uint8_t crc __attribute__((packed));
} gps_data_t;
/* An instance of the GPS data read from Arduino Com */

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

@ -133,7 +133,7 @@ float convertData(int inputRange, int outputRange, int offset, float value)
* BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values *
**************************************************************************/
float constrainf(float amt, int low, int high)
float constrainfu(float amt, int low, int high)
{
if (amt < (float)low)
return (float)low;
@ -181,14 +181,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
/*Checks the biggest angle */
throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - cos(sensorValues[PITCH]*M_PI/180);
break;
case PID_ID_COMPASS:
sensorValues[ROLL] = 0;
sensorValues[PITCH] = 0;
sensorValues[YAW] = 0;
break;
case PID_ID_BAROMETER:
@ -208,59 +200,62 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break;
case PID_ID_COMPASS:
{
float xMagnetFiltered = 0;
float yMagnetFiltered = 0;
float zMagnetFiltered = 0;
xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]);
yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]);
zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]);
MagnetFilteredOld[0] = xMagnetFiltered;
MagnetFilteredOld[1] = yMagnetFiltered;
MagnetFilteredOld[2] = zMagnetFiltered;
//this part is required to normalize the magnetic vector
if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; }
if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; }
if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; }
if (xMagnetFiltered<MagnetMin[0]) { MagnetMin[0] = xMagnetFiltered; }
if (yMagnetFiltered<MagnetMin[1]) { MagnetMin[1] = yMagnetFiltered; }
if (zMagnetFiltered<MagnetMin[2]) { MagnetMin[2] = zMagnetFiltered; }
float norm;
MagnetMap[0] = (float)(map(xMagnetFiltered, MagnetMin[0], MagnetMax[0], -10000, 10000)) / 10000.0;
MagnetMap[1] = (float)(map(yMagnetFiltered, MagnetMin[1], MagnetMax[1], -10000, 10000)) / 10000.0;
MagnetMap[2] = (float)(map(zMagnetFiltered, MagnetMin[2], MagnetMax[2], -10000, 10000)) / 10000.0;
//normalize the magnetic vector
norm = sqrt(sq(MagnetMap[0]) + sq(MagnetMap[1]) + sq(MagnetMap[2]));
MagnetMap[0] /= norm;
MagnetMap[1] /= norm;
MagnetMap[2] /= norm;
// compare Applications of Magnetic Sensors for Low Cost Compass Systems by Michael J. Caruso
// for the compensated Yaw equations...
// http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
Yaw = atan2(-(MagnetMap[1] * cos(oldSensorValue[0]) + MagnetMap[2] * sin(oldSensorValue[0])), MagnetMap[0] * cos(oldSensorValue[1]) + MagnetMap[1] * sin(oldSensorValue[1]) * sin(oldSensorValue[0]) + MagnetMap[2] * sin(oldSensorValue[1]) * cos(oldSensorValue[0]));
YawU = atan2(-MagnetMap[1], MagnetMap[0]);
// float XH;
// XH = compass_data.y * cos(oldSensorValue[1]) +
// compass_data.x * sin(oldSensorValue[1]) * sin(oldSensorValue[0]) +
// compass_data.z * sin(oldSensorValue[1]) * cos(oldSensorValue[0]);
// float YH;
// YH = compass_data.x * cos(oldSensorValue[0]) +
// compass_data.z * sin(oldSensorValue[0]);
readAcc();
// float xMagnetFiltered = 0;
// float yMagnetFiltered = 0;
// float zMagnetFiltered = 0;
// xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]);
// yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]);
// zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]);
//
// Yaw = atan2(-YH, XH);
// YawU = atan2(compass_data.y, compass_data.x);
// MagnetFilteredOld[0] = xMagnetFiltered;
// MagnetFilteredOld[1] = yMagnetFiltered;
// MagnetFilteredOld[2] = zMagnetFiltered;
//
//
// //this part is required to normalize the magnetic vector
// if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; }
// if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; }
// if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; }
//
// if (xMagnetFiltered<MagnetMin[0]) { MagnetMin[0] = xMagnetFiltered; }
// if (yMagnetFiltered<MagnetMin[1]) { MagnetMin[1] = yMagnetFiltered; }
// if (zMagnetFiltered<MagnetMin[2]) { MagnetMin[2] = zMagnetFiltered; }
//
// float norm;
//
// MagnetMap[0] = (float)(map(xMagnetFiltered, MagnetMin[0], MagnetMax[0], -10000, 10000)) / 10000.0;
// MagnetMap[1] = (float)(map(yMagnetFiltered, MagnetMin[1], MagnetMax[1], -10000, 10000)) / 10000.0;
// MagnetMap[2] = (float)(map(zMagnetFiltered, MagnetMin[2], MagnetMax[2], -10000, 10000)) / 10000.0;
//
// //normalize the magnetic vector
// norm = sqrt(sq(MagnetMap[0]) + sq(MagnetMap[1]) + sq(MagnetMap[2]));
// MagnetMap[0] /= norm;
// MagnetMap[1] /= norm;
// MagnetMap[2] /= norm;
//
//// compare Applications of Magnetic Sensors for Low Cost Compass Systems by Michael J. Caruso
//// for the compensated Yaw equations...
//// http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
// Yaw = atan2(-(MagnetMap[1] * cos(accelProfile.rollAngle) +
// MagnetMap[2] * sin(accelProfile.rollAngle)),
// MagnetMap[0] * cos(accelProfile.pitchAngle) +
// MagnetMap[1] * sin(accelProfile.pitchAngle) * sin(accelProfile.rollAngle) +
// MagnetMap[2] * sin(accelProfile.rollAngle) * cos(accelProfile.pitchAngle));
// YawU = atan2(MagnetMap[1], MagnetMap[0]);
float XH;
XH = (compass_data.y * cos(accelProfile.pitchAngle * (3.141592 / 180))) +
(compass_data.x * sin(accelProfile.pitchAngle * (3.141592 / 180)) * sin(accelProfile.rollAngle * (3.141592 / 180))) +
(compass_data.z * cos(accelProfile.pitchAngle * (3.141592 / 180)) * sin(accelProfile.rollAngle * (3.141592 / 180)));
float YH;
YH = (compass_data.x * cos(accelProfile.rollAngle * (3.141592 / 180))) +
(compass_data.z * sin(accelProfile.rollAngle * (3.141592 / 180)));
Yaw = atan2f(-YH, XH);
YawU = atan2f(-compass_data.x, compass_data.y);
}
break;
case PID_ID_BAROMETER:
default:
current_micros = clock_get_us();
current_micros = current_micros/1000000;
@ -360,7 +355,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/*Limits the PTerm of the Yaw axis */
if (pidProfile->yaw_p_limit)
{
PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
PTerm = constrainfu(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
}
@ -368,12 +363,12 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
ITerm = constrainfu(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
// Anti windup protection
if (motorLimitReached)
{
ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
ITerm = constrainfu(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
}
else
{
@ -409,7 +404,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
}
DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0;
DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
DTerm = constrainfu(DTerm, -PID_MAX_D, PID_MAX_D);
}
@ -420,7 +415,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
pidProfileBuff->lastITerm[axis] = 0;
pidProfileBuff->ITermLimit[axis] = 0;
}
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
pidProfile->PID_Out[axis] = constrainfu(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
}
/**************************************************************************
@ -522,11 +517,11 @@ void pidRun(uint8_t ID)
else
{
pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]);
PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
}
break;