Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2043 → Rev 2044

/branches/dongfang_FC_rewrite/ubx.c
26,34 → 26,34
 
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
int32_t Frac; // ns remainder of rounded ms above
int32_t frac; // ns remainder of rounded ms above
int16_t week; // GPS week
uint8_t GPSfix; // GPSfix Type, range 0..6
uint8_t Flags; // Navigation Status Flags
uint8_t flags; // Navigation Status Flags
int32_t ECEF_X; // cm ECEF X coordinate
int32_t ECEF_Y; // cm ECEF Y coordinate
int32_t ECEF_Z; // cm ECEF Z coordinate
uint32_t PAcc; // cm 3D Position Accuracy Estimate
uint32_t pAcc; // cm 3D Position Accuracy Estimate
int32_t ECEFVX; // cm/s ECEF X velocity
int32_t ECEFVY; // cm/s ECEF Y velocity
int32_t ECEFVZ; // cm/s ECEF Z velocity
uint32_t SAcc; // cm/s Speed Accuracy Estimate
uint32_t sAcc; // cm/s Speed Accuracy Estimate
uint16_t PDOP; // 0.01 Position DOP
uint8_t res1; // reserved
uint8_t numSV; // Number of SVs used in navigation solution
uint32_t res2; // reserved
Status_t Status;
Status_t status;
} UBX_SOL_t;
 
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
int32_t LON; // 1e-07 deg Longitude
int32_t LAT; // 1e-07 deg Latitude
int32_t HEIGHT; // mm Height above Ellipsoid
int32_t lon; // 1e-07 deg Longitude
int32_t lat; // 1e-07 deg Latitude
int32_t height; // mm Height above Ellipsoid
int32_t HMSL; // mm Height above mean sea level
uint32_t Hacc; // mm Horizontal Accuracy Estimate
uint32_t Vacc; // mm Vertical Accuracy Estimate
Status_t Status;
uint32_t hAcc; // mm Horizontal Accuracy Estimate
uint32_t vAacc; // mm Vertical Accuracy Estimate
Status_t status;
} UBX_POSLLH_t;
 
typedef struct {
61,52 → 61,51
int32_t VEL_N; // cm/s NED north velocity
int32_t VEL_E; // cm/s NED east velocity
int32_t VEL_D; // cm/s NED down velocity
uint32_t Speed; // cm/s Speed (3-D)
uint32_t GSpeed; // cm/s Ground Speed (2-D)
int32_t Heading; // 1e-05 deg Heading 2-D
uint32_t SAcc; // cm/s Speed Accuracy Estimate
uint32_t CAcc; // deg Course / Heading Accuracy Estimate
Status_t Status;
uint32_t speed; // cm/s Speed (3-D)
uint32_t gSpeed; // cm/s Ground Speed (2-D)
int32_t heading; // 1e-05 deg Heading 2-D
uint32_t sAcc; // cm/s Speed Accuracy Estimate
uint32_t cAcc; // deg Course / Heading Accuracy Estimate
Status_t status;
} UBX_VELNED_t;
 
UBX_SOL_t UbxSol =
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
UBX_POSLLH_t UbxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };
UBX_VELNED_t UbxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
UBX_SOL_t ubxSol = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
UBX_POSLLH_t ubxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };
UBX_VELNED_t ubxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
 
volatile uint8_t GPSTimeout = 0;
 
void updateGPSInfo(void) {
if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA)
&& (UbxVelNed.Status == NEWDATA)) {
if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA)
&& (ubxVelNed.status == NEWDATA)) {
//RED_FLASH;
// DebugOut.Digital ....blah...
if (GPSInfo.status != NEWDATA) {
GPSInfo.status = INVALID;
// NAV SOL
GPSInfo.flags = UbxSol.Flags;
GPSInfo.satfix = UbxSol.GPSfix;
GPSInfo.satnum = UbxSol.numSV;
GPSInfo.PAcc = UbxSol.PAcc;
GPSInfo.VAcc = UbxSol.SAcc;
GPSInfo.flags = ubxSol.flags;
GPSInfo.satfix = ubxSol.GPSfix;
GPSInfo.satnum = ubxSol.numSV;
GPSInfo.PAcc = ubxSol.pAcc;
GPSInfo.VAcc = ubxSol.sAcc;
// NAV POSLLH
GPSInfo.longitude = UbxPosLlh.LON;
GPSInfo.latitude = UbxPosLlh.LAT;
GPSInfo.altitude = UbxPosLlh.HEIGHT;
GPSInfo.longitude = ubxPosLlh.lon;
GPSInfo.latitude = ubxPosLlh.lat;
GPSInfo.altitude = ubxPosLlh.height;
 
GPSInfo.veleast = UbxVelNed.VEL_E;
GPSInfo.velnorth = UbxVelNed.VEL_N;
GPSInfo.veltop = -UbxVelNed.VEL_D;
GPSInfo.velground = UbxVelNed.GSpeed;
GPSInfo.veleast = ubxVelNed.VEL_E;
GPSInfo.velnorth = ubxVelNed.VEL_N;
GPSInfo.veltop = -ubxVelNed.VEL_D;
GPSInfo.velground = ubxVelNed.gSpeed;
 
GPSInfo.status = NEWDATA;
}
 
// set state to collect new data
UbxSol.Status = PROCESSED; // never update old data
UbxPosLlh.Status = PROCESSED; // never update old data
UbxVelNed.Status = PROCESSED; // never update old data
ubxSol.status = PROCESSED; // never update old data
ubxPosLlh.status = PROCESSED; // never update old data
ubxVelNed.status = PROCESSED; // never update old data
}
}
 
142,21 → 141,21
case UBXSTATE_CLASS: // check message identifier
switch (c) {
case UBX_ID_POSLLH: // geodetic position
ubxP = (int8_t *) &UbxPosLlh; // data start pointer
ubxEp = (int8_t *) (&UbxPosLlh + 1); // data end pointer
ubxSp = (int8_t *) &UbxPosLlh.Status; // status pointer
ubxP = (int8_t *) &ubxPosLlh; // data start pointer
ubxEp = (int8_t *) (&ubxPosLlh + 1); // data end pointer
ubxSp = (int8_t *) &ubxPosLlh.status; // status pointer
break;
 
case UBX_ID_SOL: // navigation solution
ubxP = (int8_t *) &UbxSol; // data start pointer
ubxEp = (int8_t *) (&UbxSol + 1); // data end pointer
ubxSp = (int8_t *) &UbxSol.Status; // status pointer
ubxP = (int8_t *) &ubxSol; // data start pointer
ubxEp = (int8_t *) (&ubxSol + 1); // data end pointer
ubxSp = (int8_t *) &ubxSol.status; // status pointer
break;
 
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxP = (int8_t *) &UbxVelNed; // data start pointer
ubxEp = (int8_t *) (&UbxVelNed + 1); // data end pointer
ubxSp = (int8_t *) &UbxVelNed.Status; // status pointer
ubxP = (int8_t *) &ubxVelNed; // data start pointer
ubxEp = (int8_t *) (&ubxVelNed + 1); // data end pointer
ubxSp = (int8_t *) &ubxVelNed.status; // status pointer
break;
 
default: // unsupported identifier
227,4 → 226,3
break;
}
}