Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 701 → Rev 702

/branches/V0.68d Code Redesign killagreg/ubx.c
1,5 → 1,7
#include <inttypes.h>
#include "ubx.h"
#include "main.h"
#include <avr/io.h>
 
// ubx protocol parser state machine
#define UBXSTATE_IDLE 0
30,7 → 32,7
uint8_t res; // reserved
uint32_t TTFF; // Time to first fix (millisecond time tag)
uint32_t MSSS; // Milliseconds since Startup / Reset
uint8_t packetStatus;
uint8_t Status;
} GPS_STATUS_t;
 
typedef struct {
41,7 → 43,7
int32_t HMSL; // mm Height above mean sea level
uint32_t Hacc; // mm Horizontal Accuracy Estimate
uint32_t Vacc; // mm Vertical Accuracy Estimate
uint8_t packetStatus;
uint8_t Status;
} GPS_POSLLH_t;
 
typedef struct {
51,7 → 53,7
int32_t ALT; // cm altitude
uint8_t ZONE; // UTM zone number
uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)
uint8_t packetStatus;
uint8_t Status;
} GPS_POSUTM_t;
 
typedef struct {
64,7 → 66,7
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
uint8_t packetStatus;
uint8_t Status;
} GPS_VELNED_t;
 
 
73,36 → 75,40
GPS_POSUTM_t GpsPosUtm;
GPS_VELNED_t GpsVelNed;
 
GPS_INFO_t actualPos;
GPS_INFO_t GPSInfo;
 
void UpdateGPSInfo (void)
{
if (GpsStatus.packetStatus == VALID) // valid packet
if (GpsStatus.Status == VALID) // valid packet
{
actualPos.satfix = GpsStatus.GPSfix;
GpsStatus.packetStatus = PROCESSED; // never update old data
GPSInfo.satfix = GpsStatus.GPSfix;
GpsStatus.Status = PROCESSED; // never update old data
}
if (GpsPosLlh.packetStatus == VALID) // valid packet
if (GpsPosLlh.Status == VALID) // valid packet
{
actualPos.longitude = GpsPosLlh.LON;
actualPos.latitude = GpsPosLlh.LAT;
actualPos.altitude = GpsPosLlh.HEIGHT;
GpsPosLlh.packetStatus = PROCESSED; // never update old data
GPSInfo.longitude = GpsPosLlh.LON;
GPSInfo.latitude = GpsPosLlh.LAT;
GPSInfo.altitude = GpsPosLlh.HEIGHT;
GpsPosLlh.Status = PROCESSED; // never update old data
}
if (GpsPosUtm.packetStatus == VALID) // valid packet
if (GpsPosUtm.Status == VALID) // valid packet
{
actualPos.utmeast = GpsPosUtm.EAST;
actualPos.utmnorth = GpsPosUtm.NORTH;
actualPos.utmalt = GpsPosUtm.ALT;
GpsPosUtm.packetStatus = PROCESSED; // never update old data
GPSInfo.utmeast = GpsPosUtm.EAST;
GPSInfo.utmnorth = GpsPosUtm.NORTH;
GPSInfo.utmalt = GpsPosUtm.ALT;
GpsPosUtm.Status = PROCESSED; // never update old data
}
if (GpsVelNed.packetStatus == VALID) // valid packet
if (GpsVelNed.Status == VALID) // valid packet
{
actualPos.veleast = GpsVelNed.VEL_E;
actualPos.velnorth = GpsVelNed.VEL_N;
actualPos.veltop = -GpsVelNed.VEL_D;
GpsPosUtm.packetStatus = PROCESSED; // never update old data
GPSInfo.veleast = GpsVelNed.VEL_E;
GPSInfo.velnorth = GpsVelNed.VEL_N;
GPSInfo.veltop = -GpsVelNed.VEL_D;
GpsPosUtm.Status = PROCESSED; // never update old data
}
if (GpsStatus.Status | GpsVelNed.Status | GpsPosLlh.Status | GpsPosUtm.Status)
{
GPSInfo.status = VALID;
}
}
 
// this function should be called within the UART RX ISR
136,7 → 142,7
case UBX_ID_POSUTM: // utm position
ubxP = (int8_t *)&GpsPosUtm; // data start pointer
ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
ubxSp = (int8_t *)&GpsPosUtm.packetStatus; // status pointer
ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
 
break;
 
143,19 → 149,19
case UBX_ID_POSLLH: // geodetic position
ubxP = (int8_t *)&GpsStatus; // data start pointer
ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer
ubxSp = (int8_t *)&GpsStatus.Status; // status pointer
break;
 
case UBX_ID_STATUS: // receiver status
ubxP = (int8_t *)&GpsStatus; // data start pointer
ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer
ubxSp = (int8_t *)&GpsStatus.Status; // status pointer
break;
 
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxP = (int8_t *)&GpsVelNed; // data start pointer
ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer
ubxSp = (int8_t *)&GpsVelNed.packetStatus; // status pointer
ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer
break;
 
default: // unsupported identifier
208,6 → 214,8
{
*ubxSp = VALID; // new data are availabe
UpdateGPSInfo(); //update GPS info respectively
ROT_FLASH;
 
}
ubxstate = UBXSTATE_IDLE; // ready to parse new data
break;