Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 828 → Rev 829

/branches/V0.68d Code Redesign killagreg/GPS.c
15,6 → 15,7
#define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function
#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes
#define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral
#define MAX_VELOCITY 250 // max ground speed in cm/s during position control
 
 
int16_t GPS_Pitch = 0, GPS_Roll = 0;
96,7 → 97,7
// then the P part of the controller is deactivated.
void GPS_PIDController(GPS_Pos_t *pTargetPos)
{
int32_t PID_Stick, PID_Pitch, PID_Roll;
int32_t temp, temp1, PID_Pitch, PID_Roll;
int32_t coscompass, sincompass;
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
159,13 → 160,44
}
 
//Calculate PID-components of the controller (negative sign for compensation)
 
// P-Part
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
 
//limit P-part to limit the max velocity
temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the P-Part limit
temp = (int32_t)c_sqrt(P_North*P_North + P_East*P_East); // the current P-Part
if(temp > temp1) // P-Part limit is reached
{
// normalize P-part components to the P-Part limit
P_North = (P_North * temp1)/temp;
P_East = (P_East * temp1)/temp;
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
else // P-Part under its limit
{
// calculate position error integrals
GPSPosDevIntegral_North += GPSPosDev_North/16;
if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT;
else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT;
GPSPosDevIntegral_East += GPSPosDev_East/16;
if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;
else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;
}
 
// I-Part
I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192;
I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192;
 
// D-Part
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
// PD-controller
 
 
// combine P- I- D-Part
PID_North = P_North + I_North + D_North;
PID_East = P_East + I_East + D_East;
 
188,24 → 220,14
PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192;
PID_Pitch = -1*((sincompass * PID_East + coscompass * PID_North) / 8192);
 
// limit GPS control vector
PID_Stick = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch);
if (PID_Stick > GPS_STICK_LIMIT)
// limit resulting GPS control vector
temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch);
if (temp > GPS_STICK_LIMIT)
{
// normalize control vector components to the limit
PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/PID_Stick;
PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/PID_Stick;
PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/temp;
PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/temp;
}
else // prevent further growing of error integrals if control limit is reached
{
// calculate position error integrals
GPSPosDevIntegral_North += GPSPosDev_North/16;
if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT;
else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT;
GPSPosDevIntegral_East += GPSPosDev_East/16;
if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;
else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;
}
 
GPS_Roll = (int16_t)PID_Roll;
GPS_Pitch = (int16_t)PID_Pitch;