Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 780 → Rev 781

/branches/V0.68d Code Redesign killagreg/GPS.c
19,15 → 19,14
int16_t GPS_Roll = 0;
 
int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
 
 
 
typedef struct
{
int32_t Northing;
int32_t Easting;
int32_t Longitude;
int32_t Latitude;
uint8_t Status;
 
} GPS_Pos_t;
 
// GPS coordinates for hold position
38,15 → 37,37
 
// ---------------------------------------------------------------------------------
 
// set home position to current hold positon
// set home position to current positon
void GPS_SetHomePosition(void)
{
HomePosition.Northing = HoldPosition.Northing;
HomePosition.Easting = HoldPosition.Easting;
HomePosition.Status = HoldPosition.Status;
if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
{
HomePosition.Longitude = GPSInfo.longitude;
HomePosition.Latitude = GPSInfo.latitude;
HomePosition.Status = VALID;
BeepTime = 1000; // signal if new home position was set
}
else
{
HomePosition.Status = INVALID;
}
}
 
// set hold position to current positon
void GPS_SetHoldPosition(void)
{
if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
{
HoldPosition.Longitude = GPSInfo.longitude;
HoldPosition.Latitude = GPSInfo.latitude;
HoldPosition.Status = VALID;
}
else
{
HoldPosition.Status = INVALID;
}
}
 
// clear home position
void GPS_ClearHomePosition(void)
{
64,13 → 85,27
void GPS_PDController(GPS_Pos_t *TargetPos)
{
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;
static int32_t cos_target_latitude = 1;
 
if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
 
if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
{
GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting;
// 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)
{
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;
 
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
158,7 → 193,7
BeepTime = 100; // beep if signal is neccesary
}
break;
case PROCESSED: // if gps data are already processed
case PROCESSED: // if gps data are already processed do nothing
// downcount timeout
if(GPSTimeout) GPSTimeout--;
// if no new data arrived within timeout set current data invalid
170,21 → 205,19
}
break;
case VALID: // new valid data from gps device
// if the gps data quality is sufficient
// if the gps data quality is good
if (GPSInfo.satfix == SATFIX_3D)
{
switch(GPS_Task) // check what's to do
{
case TSK_IDLE:
// update hold point to current gps position
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = VALID;
// update hold position to current gps position
GPS_SetHoldPosition(); // can get invalid if gps signal is bad
// disable gps control
GPS_Neutral();
break; // eof TSK_IDLE
case TSK_HOLD:
if(HoldPosition.Status == VALID)
if(HoldPosition.Status != INVALID)
{
// if sticks are centered (no manual control)
if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
194,22 → 227,23
else // MK controlled by user
{
// update hold point to current gps position
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = GPSInfo.status;
GPS_SetHoldPosition();
// disable gps control
GPS_Neutral();
}
}
else // invalid Hold Position
{ // try to catch a valid hold position from gps data input
GPS_SetHoldPosition();
GPS_Neutral();
}
break; // eof TSK_HOLD
case TSK_HOME:
if(HomePosition.Status == VALID)
if(HomePosition.Status != INVALID)
{
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = GPSInfo.status;
GPS_SetHoldPosition();
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
{
224,13 → 258,22
{
BeepTime = 50; // signal invalid home position
// try to hold at least the position as a fallback option
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
 
if (HoldPosition.Status != INVALID)
{
GPS_PDController(&HoldPosition);
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
{
GPS_PDController(&HoldPosition);
}
else // manual control or no reference position
{
GPS_Neutral();
}
}
else // manual control or no rteference position
{
else
{ // try to catch a valid hold position
GPS_SetHoldPosition();
GPS_Neutral();
}
}
240,7 → 283,6
break; // eof default
} // eof switch GPS_Task
} // eof 3D-FIX
 
else // no 3D-SATFIX
{ // disable gps control
GPS_Neutral();