Subversion Repositories FlightCtrl

#include <inttypes.h>
#include "ubx.h"
#include "main.h"
#include <avr/io.h>

#include "uart.h"
#include "timer0.h"

// ubx protocol parser state machine
#define UBXSTATE_IDLE   0
#define UBXSTATE_SYNC1  1
#define UBXSTATE_SYNC2  2
#define UBXSTATE_CLASS  3
#define UBXSTATE_LEN1   4
#define UBXSTATE_LEN2   5
#define UBXSTATE_DATA   6
#define UBXSTATE_CKA    7
#define UBXSTATE_CKB    8

// ublox protocoll identifier
#define UBX_CLASS_NAV   0x01
#define UBX_ID_POSLLH   0x02
#define UBX_ID_POSUTM   0x08
#define UBX_ID_STATUS   0x03
#define UBX_ID_VELNED   0x12
#define UBX_SYNC1_CHAR  0xB5
#define UBX_SYNC2_CHAR  0x62


typedef struct {
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
        uint8_t                 GPSfix;         // GPSfix Type, range 0..6
        uint8_t                 Flags;          // Navigation Status Flags
        uint8_t                 DiffS;          // Differential Status
        uint8_t                 res;            // reserved
        uint32_t                TTFF;           // Time to first fix (millisecond time tag)
        uint32_t                MSSS;           // Milliseconds since Startup / Reset
        uint8_t                 Status;
} GPS_STATUS_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                 HMSL;           // mm Height above mean sea level
        uint32_t                Hacc;           // mm Horizontal Accuracy Estimate
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
        uint8_t                 Status;
} GPS_POSLLH_t;

typedef struct {
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
        int32_t                 EAST;           // cm  UTM Easting
        int32_t                 NORTH;          // cm  UTM Nording
        int32_t                 ALT;            // cm  altitude
        uint8_t                 ZONE;           // UTM zone number
        uint8_t                 HEM;            // Hemisphere Indicator (0=North, 1=South)
        uint8_t                 Status;
} GPS_POSUTM_t;

typedef struct {
        uint32_t                ITOW;           // ms  GPS Millisecond Time of Week
        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
        uint8_t                 Status;
} GPS_VELNED_t;


GPS_STATUS_t    GpsStatus;
GPS_POSLLH_t    GpsPosLlh;
GPS_POSUTM_t    GpsPosUtm;
GPS_VELNED_t    GpsVelNed;

GPS_INFO_t GPSInfo;

void UpdateGPSInfo (void)
{
        if (GpsStatus.Status == VALID)                  // valid packet
        {
                GPSInfo.satfix = GpsStatus.GPSfix;
                GpsStatus.Status = PROCESSED;                   // never update old data
        }
        if (GpsPosLlh.Status == VALID)                  // valid packet
        {
                GPSInfo.longitude = GpsPosLlh.LON;
                GPSInfo.latitude = GpsPosLlh.LAT;
                GPSInfo.altitude = GpsPosLlh.HEIGHT;
                GpsPosLlh.Status = PROCESSED;                   // never update old data
        }
        if (GpsPosUtm.Status == VALID)                  // valid packet
        {
                GPSInfo.utmeast = GpsPosUtm.EAST;
                GPSInfo.utmnorth = GpsPosUtm.NORTH;
                GPSInfo.utmalt = GpsPosUtm.ALT;
                GpsPosUtm.Status = PROCESSED;                   // never update old data
        }
        if (GpsVelNed.Status == VALID)                  // valid packet
        {
                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 != INVALID)
        {
                GPSInfo.status = VALID;               // set valid if data are updated
        }
}

// this function should be called within the UART RX ISR
void ubx_parser(uint8_t c)
{
        static uint8_t ubxstate = UBXSTATE_IDLE;
        static uint8_t cka, ckb;
        static uint16_t msglen;
        static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered

        switch(ubxstate)
        {
                case UBXSTATE_IDLE: // check 1st sync byte
                        if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
                        break;

                case UBXSTATE_SYNC1: // check 2nd sync byte
                        if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
                        break;

                case UBXSTATE_SYNC2: // check msg class to be NAV
                        if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
                        else ubxstate = UBXSTATE_IDLE; // unsupported message class
                        break;

                case UBXSTATE_CLASS: // check message identifier
                        switch(c)
                        {
                                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.Status; // status pointer
                                        break;

                                case UBX_ID_POSLLH: // geodetic position
                                        ubxP =  (int8_t *)&GpsPosLlh; // data start pointer
                                        ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer
                                        ubxSp = (int8_t *)&GpsPosLlh.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.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.Status; // status pointer
                                        break;

                                default:                        // unsupported identifier
                                        ubxstate = UBXSTATE_IDLE;
                                        break;
                        }
                        if (ubxstate != UBXSTATE_IDLE)
                        {
                                ubxstate = UBXSTATE_LEN1;
                                cka = UBX_CLASS_NAV + c;
                                ckb = UBX_CLASS_NAV + cka;
                        }
                        break;

                case UBXSTATE_LEN1: // 1st message length byte
                        msglen = c;
                        cka += c;
                        ckb += cka;
                        ubxstate = UBXSTATE_LEN2;
                        break;

                case UBXSTATE_LEN2: // 2nd message length byte
                        msglen += ((uint16_t)c)<<8;
                        cka += c;
                        ckb += cka;
                        // if the old data are not processed so far then break parsing now
                        // to avoid writing new data in ISR during reading by another function
                        if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE;
                        else // data invalid or allready processd
                        {
                                *ubxSp = INVALID;
                                ubxstate = UBXSTATE_DATA;
                        }
                        break;

                case UBXSTATE_DATA:
                        if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
                        cka += c;
                        ckb += cka;
                        if (--msglen == 0)      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
                        break;

                case UBXSTATE_CKA:
                        if (c == cka) ubxstate = UBXSTATE_CKB;
                        else
                        {
                                *ubxSp = INVALID;
                                ubxstate = UBXSTATE_IDLE;
                        }
                        break;

                case UBXSTATE_CKB:
                        if (c == ckb)
                        {
                                *ubxSp = VALID; // new data are valid
                                ROT_FLASH;
                                UpdateGPSInfo(); //update GPS info respectively
                        }
                        else
                        {       // if checksum not fit then set data invalid
                                *ubxSp = INVALID;
                        }
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
                        break;

                default: // unknown ubx state
                        ubxstate = UBXSTATE_IDLE;
                        break;
        }

}