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 * * INFORMATION: Contains the whole compass message *
***********************************************************************/ ***********************************************************************/
typedef struct compass_data_t { typedef struct compass_data_t {
uint8_t header; uint8_t header __attribute__((packed));
int16_t x; int16_t x __attribute__((packed));
int16_t y; int16_t y __attribute__((packed));
int16_t z; int16_t z __attribute__((packed));
uint8_t crc; uint8_t crc __attribute__((packed));
} compass_data_t; } compass_data_t;
/*********************************************************************** /***********************************************************************
@ -31,10 +31,10 @@ typedef struct compass_data_t {
* INFORMATION: Contains the whole gps data message * * INFORMATION: Contains the whole gps data message *
***********************************************************************/ ***********************************************************************/
typedef struct gps_data_t { typedef struct gps_data_t {
uint8_t header; uint8_t header __attribute__((packed));
float latitude; float latitude; __attribute__((packed))
float longitude; float longitude __attribute__((packed));
uint8_t crc; uint8_t crc __attribute__((packed));
} gps_data_t; } gps_data_t;
/* An instance of the GPS data read from Arduino Com */ /* 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 * * BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values * * 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) if (amt < (float)low)
return (float)low; return (float)low;
@ -181,14 +181,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
/*Checks the biggest angle */ /*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); 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; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
@ -208,59 +200,62 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break; break;
case PID_ID_COMPASS: case PID_ID_COMPASS:
{ {
float xMagnetFiltered = 0; readAcc();
float yMagnetFiltered = 0; // float xMagnetFiltered = 0;
float zMagnetFiltered = 0; // float yMagnetFiltered = 0;
xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]); // float zMagnetFiltered = 0;
yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]); // xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]);
zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]); // 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]);
// //
// Yaw = atan2(-YH, XH); // MagnetFilteredOld[0] = xMagnetFiltered;
// YawU = atan2(compass_data.y, compass_data.x); // 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; break;
case PID_ID_BAROMETER:
default: default:
current_micros = clock_get_us(); current_micros = clock_get_us();
current_micros = current_micros/1000000; current_micros = current_micros/1000000;
@ -360,7 +355,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/*Limits the PTerm of the Yaw axis */ /*Limits the PTerm of the Yaw axis */
if (pidProfile->yaw_p_limit) 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]; 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. // 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 // Anti windup protection
if (motorLimitReached) if (motorLimitReached)
{ {
ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); ITerm = constrainfu(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
} }
else 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 = 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->lastITerm[axis] = 0;
pidProfileBuff->ITermLimit[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 else
{ {
pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]); 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] = constrainfu(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], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
} }
break; break;