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 |