Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 811 → Rev 812

/branches/V0.68d Code Redesign killagreg/GPS.c
14,13 → 14,11
 
#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_D_LIMIT_DIST 500 // for position deviations greater than 500cm the D-Part of the PD-Controller is limited
#define GPS_D_LIMIT 50 // PD-Controntroller D-Limit.
#define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral
 
int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;
 
int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
int16_t GPS_Pitch = 0, GPS_Roll = 0;
int32_t GPS_P_Factor = 0, GPS_I_Factor = 0, GPS_D_Factor = 0;
 
 
 
96,13 → 94,16
// calculates the GPS control stick values from the deviation to target position
// if the pointer to the target positin is NULL or is the target position invalid
// then the P part of the controller is deactivated.
void GPS_PDController(GPS_Pos_t *pTargetPos)
void GPS_PIDController(GPS_Pos_t *pTargetPos)
{
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;
int32_t PD_North = 0, PD_East = 0;
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
int32_t PID_North = 0, PID_East = 0;
uint8_t GPS_Stick_Limited = 0;
static int32_t cos_target_latitude = 1;
static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
static GPS_Pos_t *pLastTargetPos = 0;
 
// if GPS data and Compass are ok
if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) )
111,58 → 112,62
if(pTargetPos != NULL) // if there is a target position
{
if(pTargetPos->Status != INVALID) // and the position data are valid
{ // calculate position deviation from latitude and longitude differences
GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
// recalculate the target latitude projection only if the target data are updated
// to save time
if (pTargetPos->Status != PROCESSED)
{
// if the target data are updated or the target pointer has changed
if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) )
{
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
// recalculate latitude projection
cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
// remember last target pointer
pLastTargetPos = pTargetPos;
// mark data as processed
pTargetPos->Status = PROCESSED;
}
// calculate position deviation from latitude and longitude differences
GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
// calculate latitude projection
GPSPosDev_East *= cos_target_latitude;
GPSPosDev_East /= 8192;
 
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
//DebugOut.Analog[12] = GPSInfo.velnorth;
//DebugOut.Analog[13] = GPSInfo.veleast;
 
//Calculate P-components of the controller (negative sign for compensation)
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
}
else // not valid target position available
else // no valid target position available
{
// reset error
GPSPosDev_North = 0;
GPSPosDev_East = 0;
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
}
else // not target position available
else // no target position available
{
// reset error
GPSPosDev_North = 0;
GPSPosDev_East = 0;
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
 
//Calculate PD-components of the controller (negative sign for compensation)
//Calculate PID-components of the controller (negative sign for compensation)
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/2048;
I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/2048;
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
// limit D-Part if position deviation is significant of the target position
// this accelerates flying to the target position
if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) || (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
{
if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT;
else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT;
}
// PD-controller
PD_North = P_North + D_North;
PD_East = P_East + D_East;
PID_North = P_North + I_North + D_North;
PID_East = P_East + I_East + D_East;
 
// GPS to pitch and roll settings
 
180,22 → 185,55
 
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
GPS_Roll = (int16_t)((coscompass * PID_East - sincompass * PID_North) / 8192);
GPS_Pitch = -1*(int16_t)((sincompass * PID_East + coscompass * PID_North) / 8192);
 
// limit GPS controls
if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT;
else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT;
else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT;
if (GPS_Pitch > GPS_STICK_LIMIT)
{
GPS_Pitch = GPS_STICK_LIMIT;
GPS_Stick_Limited = 1;
}
else if (GPS_Pitch < -GPS_STICK_LIMIT)
{
GPS_Pitch = -GPS_STICK_LIMIT;
GPS_Stick_Limited = 1;
}
if (GPS_Roll > GPS_STICK_LIMIT)
{
GPS_Roll = GPS_STICK_LIMIT;
GPS_Stick_Limited = 1;
}
else if (GPS_Roll < -GPS_STICK_LIMIT)
{
GPS_Roll = -GPS_STICK_LIMIT;
GPS_Stick_Limited = 1;
}
 
if(!GPS_Stick_Limited) // prevent further growing if error integrals if control limit is reached
{
// calculate position error integrals
GPSPosDevIntegral_North += GPSPosDev_North/4;
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/4;
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;
}
 
}
else // invalid GPS data
{
GPS_Neutral(); // do nothing
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
}
 
 
 
 
void GPS_Main(uint8_t ctrl)
{
static uint8_t GPS_Task = TSK_IDLE;
257,9 → 295,9
{ // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
GPS_P_Delay++;
GPS_SetHoldPosition(); // update hold point to current gps position
GPS_PDController(NULL); // activates only the D-Part
GPS_PIDController(NULL); // activates only the D-Part
}
else GPS_PDController(&HoldPosition);// activates the P&D-Part
else GPS_PIDController(&HoldPosition);// activates the P&D-Part
}
}
else // invalid Hold Position
280,7 → 318,7
}
else // GPS control active
{
GPS_PDController(&HomePosition);
GPS_PIDController(&HomePosition);
}
}
else // bad home position
296,7 → 334,7
}
else // GPS control active
{
GPS_PDController(&HoldPosition);
GPS_PIDController(&HoldPosition);
}
}
else