81,8 → 81,10 |
GPS_Roll = 0; |
} |
|
// calculates the GPS control sticks values from the deviation to target position |
void GPS_PDController(GPS_Pos_t *TargetPos) |
// 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) |
{ |
int32_t coscompass, sincompass; |
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
90,35 → 92,55 |
int32_t PD_North = 0, PD_East = 0; |
static int32_t cos_target_latitude = 1; |
|
// if GPS data and Compass are ok |
if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) ) |
{ |
|
if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D)) |
{ |
// calculate position deviation from latitude and longitude differences |
GPSPosDev_North = (GPSInfo.latitude - TargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
GPSPosDev_East = (GPSInfo.longitude - TargetPos->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 (TargetPos->Status != PROCESSED) |
if(pTargetPos != NULL) // if there is a target position |
{ |
cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L)); |
TargetPos->Status = PROCESSED; |
} |
// calculate latitude projection |
GPSPosDev_East *= cos_target_latitude; |
GPSPosDev_East /= 8192; |
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) |
{ |
cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
pTargetPos->Status = PROCESSED; |
} |
// calculate latitude projection |
GPSPosDev_East *= cos_target_latitude; |
GPSPosDev_East /= 8192; |
|
DebugOut.Analog[12] = GPSPosDev_North; |
DebugOut.Analog[13] = GPSPosDev_East; |
DebugOut.Analog[12] = GPSPosDev_North; |
DebugOut.Analog[13] = GPSPosDev_East; |
//DebugOut.Analog[12] = GPSInfo.velnorth; |
//DebugOut.Analog[13] = GPSInfo.veleast; |
|
//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 |
{ |
GPSPosDev_North = 0; |
GPSPosDev_East = 0; |
} |
} |
else // not target position available |
{ |
GPSPosDev_North = 0; |
GPSPosDev_East = 0; |
} |
|
//Calculate PD-components of the controller (negative sign for compensation) |
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
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; |
126,7 → 148,6 |
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; |
133,9 → 154,8 |
|
// GPS to pitch and roll settings |
|
//A positive pitch angle moves head downwards (flying forward). |
//A positive roll angle tilts left side downwards (flying left). |
|
// A positive pitch angle moves head downwards (flying forward). |
// A positive roll angle tilts left side downwards (flying left). |
// If compass heading is 0 the head of the copter is in north direction. |
// A positive pitch angle will fly to north and a positive roll angle will fly to west. |
// In case of a positive north deviation/velocity the |
142,23 → 162,15 |
// copter should fly to south (negative pitch). |
// In case of a positive east position deviation and a positive east velocity the |
// copter should fly to west (positive roll). |
|
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
|
// rotation transformation to compass heading to match any copter orientation |
if (CompassHeading < 0) // no valid compass data |
{ // disable GPS |
GPS_Neutral(); |
} |
else |
{ |
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); |
} |
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); |
|
// 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; |
165,10 → 177,9 |
if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
} |
else // invalid input data |
else // invalid GPS data |
{ |
BeepTime = 50; |
GPS_Neutral(); |
GPS_Neutral(); // do nothing |
} |
} |
|
176,6 → 187,7 |
void GPS_Main(uint8_t ctrl) |
{ |
static uint8_t GPS_Task = TSK_IDLE; |
static uint8_t GPS_P_Delay = 0; |
int16_t satbeep; |
|
// ctrl enables the gps feature |
222,7 → 234,13 |
// if sticks are centered (no manual control) |
if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
{ |
GPS_PDController(&HoldPosition); |
if(GPS_P_Delay<7) |
{ // 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 |
} |
else GPS_PDController(&HoldPosition);// activates the P&D-Part |
} |
else // MK controlled by user |
{ |
230,6 → 248,7 |
GPS_SetHoldPosition(); |
// disable gps control |
GPS_Neutral(); |
GPS_P_Delay = 0; |
} |
} |
else // invalid Hold Position |