Rev 806 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!! */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software Nutzungsbedingungen (english version: see below)
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
// + des Mitverschuldens offen.
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
// + Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software LICENSING TERMS
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
// + The Software may only be used with the Licensor's products.
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
// + agreement shall be the property of the Licensor.
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
// + features that can be used to identify the program may not be altered or defaced by the customer.
// + The customer shall be responsible for taking reasonable precautions
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
// + #### END OF LICENSING TERMS ####
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <string.h>
#include "91x_lib.h"
#include "uart0.h"
#include "uart1.h"
#include "ubx.h"
#include "main.h"
#include "led.h"
#include "timer1.h"
#include "GPS.h"
// ------------------------------------------------------------------------------------------------
// defines
#define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist!
#define DAYS_PER_YEAR 365
#define DAYS_PER_LEAPYEAR 366
#define DAYS_PER_4YEARS 1461 //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years
#define DAYS_PER_100YEARS 36524 //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years
#define DAYS_PER_400YEARS 146097 //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years
#define SECONDS_PER_MINUTE 60
#define MINUTES_PER_HOUR 60
#define HOURS_PER_DAY 24
#define DAYS_PER_WEEK 7
#define SECONDS_PER_HOUR 3600 //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR)
#define SECONDS_PER_DAY 86400 //(SECONDS_PER_HOUR * HOURS_PER_DAY)
#define SECONDS_PER_WEEK 604800 //(SECONDS_PER_DAY * DAYS_PER_WEEK)
// days per month in normal and leap years
const u32 Leap
[ 13 ] = { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 };
const u32 Normal
[ 13 ] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 };
#define LEAP_SECONDS_FROM_1980 16
// message sync bytes
#define UBX_SYNC1_CHAR 0xB5
#define UBX_SYNC2_CHAR 0x62
// protocoll identifiers
// navigation class
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_MON 0x0A
#define UBX_ID_POSLLH 0x02
#define UBX_ID_SOL 0x06
#define UBX_ID_VELNED 0x12
#define UBX_ID_MON_VER 0x04
// ------------------------------------------------------------------------------------------------
// typedefs
// ubx parser state
typedef enum
{
UBXSTATE_IDLE
,
UBXSTATE_SYNC1
,
UBXSTATE_SYNC2
,
UBXSTATE_CLASS
,
UBXSTATE_LEN1
,
UBXSTATE_LEN2
,
UBXSTATE_DATA
,
UBXSTATE_CKA
,
UBXSTATE_CKB
} ubxState_t
;
typedef struct
{
u32 itow
; // ms GPS Millisecond Time of Week
s32 frac
; // ns remainder of rounded ms above
s16 week
; // GPS week
u8 GPSfix
; // GPSfix Type, range 0..6
u8 Flags
; // Navigation Status Flags
s32 ECEF_X
; // cm ECEF X coordinate
s32 ECEF_Y
; // cm ECEF Y coordinate
s32 ECEF_Z
; // cm ECEF Z coordinate
u32 PAcc
; // cm 3D Position Accuracy Estimate
s32 ECEFVX
; // cm/s ECEF X velocity
s32 ECEFVY
; // cm/s ECEF Y velocity
s32 ECEFVZ
; // cm/s ECEF Z velocity
u32 SAcc
; // cm/s Speed Accuracy Estimate
u16 PDOP
; // 0.01 Position DOP
u8 res1
; // reserved
u8 numSV
; // Number of SVs used in navigation solution
u32 res2
; // reserved
u8 Status
; // invalid/newdata/processed
} __attribute__
((packed
)) ubx_nav_sol_t
;
typedef struct
{
u32 itow
; // ms GPS Millisecond Time of Week
s32 VEL_N
; // cm/s NED north velocity
s32 VEL_E
; // cm/s NED east velocity
s32 VEL_D
; // cm/s NED down velocity
u32 Speed
; // cm/s Speed (3-D)
u32 GSpeed
; // cm/s Ground Speed (2-D)
s32 Heading
; // 1e-05 deg Heading 2-D
u32 SAcc
; // cm/s Speed Accuracy Estimate
u32 CAcc
; // deg Course / Heading Accuracy Estimate
u8 Status
; // invalid/newdata/processed
} __attribute__
((packed
)) ubx_nav_velned_t
;
typedef struct
{
u32 itow
; // ms GPS Millisecond Time of Week
s32 LON
; // 1e-07 deg Longitude
s32 LAT
; // 1e-07 deg Latitude
s32 HEIGHT
; // mm Height above Ellipsoid
s32 HMSL
; // mm Height above mean sea level
u32 Hacc
; // mm Horizontal Accuracy Estimate
u32 Vacc
; // mm Vertical Accuracy Estimate
u8 Status
; // invalid/newdata/processed
} __attribute__
((packed
)) ubx_nav_posllh_t
;
//------------------------------------------------------------------------------------
// global variables
// local buffers for the incomming ubx messages
ubx_nav_sol_t UbxSol
= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID
};
ubx_nav_posllh_t UbxPosLlh
= {0,0,0,0,0,0,0, INVALID
};
ubx_nav_velned_t UbxVelNed
= {0,0,0,0,0,0,0,0,0, INVALID
};
ubxmsg_t UbxMsg
;
// shared buffer
gps_data_t GPSData
= {200,{0,0,0,INVALID
},0,0,0,0,0,0,0, INVALID
};
DateTime_t GPSDateTime
= {0,0,0,0,0,0,0, INVALID
};
#define UBX_TIMEOUT 500 // 500 ms
u32 UBX_Timeout
= 0;
//------------------------------------------------------------------------------------
// functions
u8 IsLeapYear
(u16 year
)
{
if((year
%400 == 0) || ( (year
%4 == 0) && (year
%100 != 0) ) ) return 1;
else return 0;
}
/********************************************************/
/* Calculates the UTC Time from the GPS week and tow */
/********************************************************/
void SetGPSTime
(DateTime_t
* pTimeStruct
)
{
u32 Days
, Seconds
, Week
;
u16 YearPart
;
u32
* MonthDayTab
= 0;
u8 i
;
// if GPS data show valid time data
if((UbxSol.
Status != INVALID
) && (UbxSol.
Flags & FLAG_WKNSET
) && (UbxSol.
Flags & FLAG_TOWSET
) )
{
Seconds
= UbxSol.
itow / 1000L;
Week
= (u32
)UbxSol.
week;
// correct leap seconds since 1980
if(Seconds
< LEAP_SECONDS_FROM_1980
)
{
Week
--;
Seconds
= SECONDS_PER_WEEK
- LEAP_SECONDS_FROM_1980
+ Seconds
;
}
else Seconds
-= LEAP_SECONDS_FROM_1980
;
Days
= DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980
;
Days
+= (Week
* DAYS_PER_WEEK
);
Days
+= Seconds
/ SECONDS_PER_DAY
; // seperate days from GPS seconds of week
pTimeStruct
->Year
= 1;
YearPart
= (u16
)(Days
/ DAYS_PER_400YEARS
);
pTimeStruct
->Year
+= YearPart
* 400;
Days
= Days
% DAYS_PER_400YEARS
;
YearPart
= (u16
)(Days
/ DAYS_PER_100YEARS
);
pTimeStruct
->Year
+= YearPart
* 100;
Days
= Days
% DAYS_PER_100YEARS
;
YearPart
= (u16
)(Days
/ DAYS_PER_4YEARS
);
pTimeStruct
->Year
+= YearPart
* 4;
Days
= Days
% DAYS_PER_4YEARS
;
if(Days
< (3* DAYS_PER_YEAR
)) YearPart
= (u16
)(Days
/ DAYS_PER_YEAR
);
else YearPart
= 3;
pTimeStruct
->Year
+= YearPart
;
// calculate remaining days of year
Days
-= (u32
)(YearPart
* DAYS_PER_YEAR
);
Days
+= 1;
// check if current year is a leap year
if(IsLeapYear
(pTimeStruct
->Year
)) MonthDayTab
= (u32
*)Leap
;
else MonthDayTab
= (u32
*)Normal
;
// seperate month and day from days of year
for ( i
= 0; i
< 12; i
++ )
{
if ( (MonthDayTab
[i
]< Days
) && (Days
<= MonthDayTab
[i
+1]) )
{
pTimeStruct
->Month
= i
+1;
pTimeStruct
->Day
= Days
- MonthDayTab
[i
];
i
= 12;
}
}
Seconds
= Seconds
% SECONDS_PER_DAY
; // remaining seconds of current day
pTimeStruct
->Hour
= (u8
)(Seconds
/ SECONDS_PER_HOUR
);
Seconds
= Seconds
% SECONDS_PER_HOUR
; // remaining seconds of current hour
pTimeStruct
->Min
= (u8
)(Seconds
/ SECONDS_PER_MINUTE
);
Seconds
= Seconds
% SECONDS_PER_MINUTE
; // remaining seconds of current minute
pTimeStruct
->Sec
= (u8
)(Seconds
);
pTimeStruct
->mSec
= (u16
)(UbxSol.
itow % 1000L);
pTimeStruct
->Valid
= 1;
}
else
{
pTimeStruct
->Valid
= 0;
}
}
/********************************************************/
/* Initialize UBX Parser */
/********************************************************/
void UBX_Init
(void)
{
// mark msg buffers invalid
UbxSol.
Status = INVALID
;
UbxPosLlh.
Status = INVALID
;
UbxVelNed.
Status = INVALID
;
UbxMsg.
Status = INVALID
;
GPSData.
Status = INVALID
;
UBX_Timeout
= SetDelay
(5000);
}
/********************************************************/
/* Upate GPS data stcructure */
/********************************************************/
void Update_GPSData
(void)
{
static u32 last_itow
= 0;
static u32 milliseconds
= 0;
// if a new set of ubx messages was collected
if((UbxSol.
Status == NEWDATA
) && (UbxPosLlh.
Status == NEWDATA
) && (UbxVelNed.
Status == NEWDATA
))
{ // and the itow is equal (same time base)
UART_VersionInfo.
Flags |= NC_VERSION_FLAG_GPS_PRESENT
;
if((UbxSol.
itow == UbxPosLlh.
itow) && (UbxPosLlh.
itow == UbxVelNed.
itow))
{
UBX_Timeout
= SetDelay
(UBX_TIMEOUT
);
DebugOut.
Analog[9]++;
// update GPS data only if the status is INVALID or PROCESSED
if(GPSData.
Status != NEWDATA
)
{ // wait for new data at all neccesary ubx messages
CountNewGpsDataIn5Sec
++;
GPSData.
Status = INVALID
;
// update message cycle time
GPSData.
MsgCycleTime = (u16
)(UbxSol.
itow-last_itow
);
milliseconds
+= GPSData.
MsgCycleTime;
last_itow
= UbxSol.
itow; // update last itow
// NAV SOL
GPSData.
Flags = (GPSData.
Flags & 0xf0) | (UbxSol.
Flags & 0x0f); // we take only the lower bits
GPSData.
NumOfSats = UbxSol.
numSV;
GPSData.
SatFix = UbxSol.
GPSfix;
//if(Parameter.User3 > 100) { GPSData.NumOfSats = 0; GPSData.SatFix = 0;}
//if(Parameter.User3 > 100) { GPSData.SatFix = 0;}
//DebugOut.Analog[] = UbxPosLlh.Hacc;
//DebugOut.Analog[] = UbxPosLlh.Vacc;
GPSData.
Position_Accuracy = UbxSol.
PAcc; // in steps of 1cm
GPSData.
Speed_Accuracy = UbxSol.
SAcc; // in steps of 1cm/s
SetGPSTime
(&SystemTime
); // update system time
// NAV VELNED
GPSData.
Speed_East = UbxVelNed.
VEL_E; // in steps of 1cm/s
GPSData.
Speed_North = UbxVelNed.
VEL_N; // in steps of 1cm/s
GPSData.
Speed_Top = -UbxVelNed.
VEL_D; // in steps of 1cm/s
GPSData.
Speed_Ground = UbxVelNed.
GSpeed; // in steps of 1cm/s
GPSData.
Heading = UbxVelNed.
Heading; //in steps of 1E-5 deg
// NAV POSLLH
GPSData.
Position.
Status = INVALID
;
if(!(SimulationFlags
& SIMULATION_ACTIVE
))
{
GPSData.
Position.
Longitude = UbxPosLlh.
LON; // in steps of 1E-7 deg
GPSData.
Position.
Latitude = UbxPosLlh.
LAT; // in steps of 1E-7 deg
GPSData.
Position.
Altitude = UbxPosLlh.
HMSL; // in steps of 1 mm
}
else // simulation active
{
if(GPSData.
SatFix != SATFIX_3D
|| GPSData.
NumOfSats < 6) // simulate satfix
{
GPSData.
SatFix = SATFIX_3D
; // Simulation
GPSData.
Flags |= FLAG_GPSFIXOK
; // Simulation
GPSData.
NumOfSats = 8; // Simulation
if(!SystemTime.
Valid)
{
UbxSol.
Status = 1;
UbxSol.
Flags |= FLAG_WKNSET
;
UbxSol.
Flags |= FLAG_TOWSET
;
UbxSol.
week = 1043; // starts in year 2000
UbxSol.
itow = milliseconds
;
SetGPSTime
(&SystemTime
); // update system time
SystemTime.
Valid = 1; // use the time that is given by the GPS-Module
}
}
}
GPSData.
Position.
Status = NEWDATA
;
GPSData.
Status = NEWDATA
; // new data available
} // EOF if(GPSData.Status != NEWDATA)
// set state to collect new data
UbxSol.
Status = PROCESSED
; // ready for new data
UbxPosLlh.
Status = PROCESSED
; // ready for new data
UbxVelNed.
Status = PROCESSED
; // ready for new data
} // EOF all itow are equal
} // EOF all ubx messages received
//++++++++++++++++++++++++++++++++
// Please do not delete
// This helps me for testing
//++++++++++++++++++++++++++++++++
//GPSData.Position.Longitude = 1517409123L; // Hamilton, Australia
//GPSData.Position.Latitude = -329294773L; // Hamilton, Australia
//++++++++
//GPSData.Position.Longitude =-1556010020L; // Alaska
//GPSData.Position.Latitude = 629581270L; // Alaska
//++++++++
//GPSData.Position.Longitude =-584343419L; // Buenos aires
//GPSData.Position.Latitude = -345464421L; // Buenos aires
//++++++++
//GPSData.Position.Longitude =1683362691L; // Neuseeland
//GPSData.Position.Latitude = -465945926L; // Neuseeland
//++++++++
//GPSData.Position.Longitude = 194140605L; // Afrika
//GPSData.Position.Latitude = -345384656L; // Afrika
//++++++++
//GPSData.Position.Longitude =-740443840L; // Liberty Staue davor
//GPSData.Position.Latitude = 406888880L; // Liberty Staue
//GPSData.Position.Longitude =-740451660L; // Liberty Staue daneben
//GPSData.Position.Latitude = 406891880L; // Liberty Staue
//GPSData.Position.Longitude =-740446540L; // Liberty Staue direkt
//GPSData.Position.Latitude = 406891590L; // Liberty Staue 1
//GPSData.Position.Longitude =-1142878694L; // Flori
//GPSData.Position.Latitude = 483712102L; //
//GPSData.Position.Longitude =1251674613L; // Bubble
//GPSData.Position.Latitude = 466058365L; //
}
u8 UbxVersionParser
(void)
{
// HW:00000040: Antaris
// HW:00040001: Antaris-4
// HW:80040001: Antaris-4
// HW:00040005: u-blox 5 (nicht von uns verbaut)
// HW:00040006: u-blox 6
// HW:00040007: u-blox 6
// HW:00070000: u-blox 7 (nicht von uns verbaut)
// HW:00080000: u-blox M8
// MKGPS V10 -> 1500 -> LEA-4H-0-000 -> ubxsw == 5.00 HW:00040001
// MKGPS V20 -> 2602 -> LEA-6S-0-000 -> ubxsw == 6.02 HW:00040007
// MKGPS V20 -> 2703 -> LEA-6S-0-001 -> ubxsw == 7.03 HW:00040007
// MKGPS V30 -> 3101 -> NEO-M8N-0-01 -> ubxsw == 2.01 HW:00080000 (Flash- Variante) kann nur 5Hz bei MAX-Sat:15
// MKGPS V31 -> 3201 -> NEO-M8Q-0-00 -> ubxsw == 2.01 HW:00080000 (ROM Variante)
// MKGPS V40 -> 4301 -> NEO-M8Q-0-10 -> ubxsw == 3.01 HW:00080000 (Message:"ROM CORE 3.01 (107888))
u8 retval
= 0xFF;
if(UbxMsg.
Data[33] == '4' && UbxMsg.
Data[37] == '1') // LEA-4
{
GPS_Version
= 1000 + (UbxMsg.
Data[0] - '0') * 100 + (UbxMsg.
Data[2] - '0') * 10 + (UbxMsg.
Data[3] - '0');
retval
= 10; // MKGPS V1
}
else
if(UbxMsg.
Data[33] == '4' && UbxMsg.
Data[37] == '7') // LEA-6
{
GPS_Version
= 2000 + (UbxMsg.
Data[0] - '0') * 100 + (UbxMsg.
Data[2] - '0') * 10 + (UbxMsg.
Data[3] - '0');
retval
= 20; // MKGPS V2
}
else
if(UbxMsg.
Data[33] == '8' && UbxMsg.
Data[37] == '0') // NEO-8
{
retval
= 30; // MKGPS V3
if(UbxMsg.
Data[100] == 'F') // Message[100]:"FIS 0xEF4015 (73171)" bei der Flash-Variante
{
GPS_Version
= 3000 - 100 + (UbxMsg.
Data[0] - '0') * 100 + (UbxMsg.
Data[2] - '0') * 10 + (UbxMsg.
Data[3] - '0');
retval
= 30; // MKGPS V3 Flash
}
else
if(UbxMsg.
Data[0] >= '0' && UbxMsg.
Data[0] <= '9') // Steht da gleich zu Anfang eine Zahl?
{
GPS_Version
= 3000 + (UbxMsg.
Data[0] - '0') * 100 + (UbxMsg.
Data[2] - '0') * 10 + (UbxMsg.
Data[3] - '0');
retval
= 31; // MKGPS V3 ROM
}
else
if(UbxMsg.
Data[9] >= '0' && UbxMsg.
Data[9] <= '9') // Message:"ROM CORE 3.01 (107888)"
{
GPS_Version
= 4000 + (UbxMsg.
Data[9] - '0') * 100 + (UbxMsg.
Data[11] - '0') * 10 + (UbxMsg.
Data[12] - '0');
retval
= 40; // MKGPS V4 (Galileo)
}
}
return(retval
);
}
/********************************************************/
/* UBX Parser */
/********************************************************/
void UBX_RxParser
(u8 c
)
{
static ubxState_t ubxState
= UBXSTATE_IDLE
;
static ubxmsghdr_t RxHdr
;
static u8 RxData
[UBX_MSG_DATA_SIZE
];
static u16 RxBytes
= 0;
static u8 cka
, ckb
;
//state machine
switch (ubxState
) // ubx message parser
{
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
RxHdr.
Class = c
;
ubxState
= UBXSTATE_CLASS
;
break;
case UBXSTATE_CLASS
: // check message identifier
RxHdr.
Id = c
;
ubxState
= UBXSTATE_LEN1
;
cka
= RxHdr.
Class + RxHdr.
Id;
ckb
= RxHdr.
Class + cka
;
break;
case UBXSTATE_LEN1
: // 1st message length byte
RxHdr.
Length = (u16
)c
; // lowbyte first
cka
+= c
;
ckb
+= cka
;
ubxState
= UBXSTATE_LEN2
;
break;
case UBXSTATE_LEN2
: // 2nd message length byte
RxHdr.
Length += ((u16
)c
)<<8; // high byte last
if (RxHdr.
Length >= UBX_MSG_DATA_SIZE
)
{
ubxState
= UBXSTATE_IDLE
;
}
else
{
cka
+= c
;
ckb
+= cka
;
RxBytes
= 0; // reset data byte counter
ubxState
= UBXSTATE_DATA
;
}
break;
case UBXSTATE_DATA
: // collecting data
if (RxBytes
< UBX_MSG_DATA_SIZE
)
{
RxData
[RxBytes
++] = c
; // copy curent data byte if any space is left
cka
+= c
;
ckb
+= cka
;
if (RxBytes
>= RxHdr.
Length) ubxState
= UBXSTATE_CKA
; // switch to next state if all data have been received
}
else // rx buffer overrun
{
ubxState
= UBXSTATE_IDLE
;
}
break;
case UBXSTATE_CKA
:
if (c
== cka
) ubxState
= UBXSTATE_CKB
;
else
{
ubxState
= UBXSTATE_IDLE
;
}
break;
case UBXSTATE_CKB
:
if (c
== ckb
)
{ // checksum is ok
switch(RxHdr.
Class)
{
case UBX_CLASS_NAV
:
switch(RxHdr.
Id)
{
case UBX_ID_POSLLH
: // geodetic position
memcpy((u8
*)&UbxPosLlh
, RxData
, RxHdr.
Length);
UbxPosLlh.
Status = NEWDATA
;
break;
case UBX_ID_VELNED
: // velocity vector in tangent plane
memcpy((u8
*)&UbxVelNed
, RxData
, RxHdr.
Length);
UbxVelNed.
Status = NEWDATA
;
break;
case UBX_ID_SOL
: // navigation solution
memcpy((u8
*)&UbxSol
, RxData
, RxHdr.
Length);
UbxSol.
Status = NEWDATA
;
break;
default:
break;
} // EOF switch(Id)
Update_GPSData
();
break;
case UBX_CLASS_MON
: // version
switch(RxHdr.
Id)
{
case UBX_ID_MON_VER
:
if(UbxMsg.
Hdr.
Length <= UBX_MSG_DATA_SIZE
) memcpy(UbxMsg.
Data, RxData
, RxHdr.
Length);
UbxVersionParser
();
break;
default:
break;
}
default:
break;
} // EOF switch(class)
// check generic msg filter
if(UbxMsg.
Status != NEWDATA
)
{ // msg buffer is free
if(((UbxMsg.
Hdr.
Class&UbxMsg.
ClassMask) == (RxHdr.
Class&UbxMsg.
ClassMask)) && ((UbxMsg.
Hdr.
Id&UbxMsg.
IdMask) == (RxHdr.
Id&UbxMsg.
IdMask)))
{ // msg matches to the filter criteria
UbxMsg.
Status = INVALID
;
UbxMsg.
Hdr.
Class = RxHdr.
Class;
UbxMsg.
Hdr.
Id = RxHdr.
Id;
UbxMsg.
Hdr.
Length = RxHdr.
Length;
if(UbxMsg.
Hdr.
Length <= UBX_MSG_DATA_SIZE
)
{ // copy data block
memcpy(UbxMsg.
Data, RxData
, RxHdr.
Length);
UbxMsg.
Status = NEWDATA
;
}
} // EOF filter matches
} // EOF != INVALID
}// EOF crc ok
// else DebugOut.Analog[13]++; // CRC Error -> since 2.10e (6.2015) -> removed 11.2016 (V2.16)
ubxState
= UBXSTATE_IDLE
; // ready to parse new data
break;
default: // unknown ubx state
ubxState
= UBXSTATE_IDLE
;
break;
}
}
u8 UBX_CreateMsg
(Buffer_t
* pBuff
, u8
* pData
, u16 Len
)
{
u16 i
;
u8 cka
= 0, ckb
= 0;
// check if buffer is available
if(pBuff
->Locked
== TRUE
) return(0);
// check if buffer size is sufficient
if(pBuff
->Size
< 8 + Len
) return(0);
// lock the buffer
pBuff
->Locked
= TRUE
;
// start at begin
pBuff
->Position
= 0;
pBuff
->pData
[pBuff
->Position
++] = UBX_SYNC1_CHAR
;
pBuff
->pData
[pBuff
->Position
++] = UBX_SYNC2_CHAR
;
for(i
=0;i
<Len
;i
++)
{
pBuff
->pData
[pBuff
->Position
++] = pData
[i
];
}
// calculate checksum
for(i
=2;i
<pBuff
->Position
;i
++)
{
cka
+= pBuff
->pData
[i
];
ckb
+= cka
;
}
pBuff
->pData
[pBuff
->Position
++] = cka
;
pBuff
->pData
[pBuff
->Position
++] = ckb
;
pBuff
->DataBytes
= pBuff
->Position
;
pBuff
->Position
= 0; // reset buffer position for transmision
return(1);
}
/*
switch(ubxclass)
{
case UBX_CLASS_NAV:
switch(ubxid)
{
case UBX_ID_POSLLH: // geodetic position
ubxSp = (u8 *)&UbxPosLlh; // data start pointer
ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
ubxStP = (u8 *)&UbxPosLlh.Status; // status pointer
break;
case UBX_ID_SOL: // navigation solution
ubxSp = (u8 *)&UbxSol; // data start pointer
ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
ubxStP = (u8 *)&UbxSol.Status; // status pointer
break;
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxSp = (u8 *)&UbxVelNed; // data start pointer
ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
ubxStP = (u8 *)&UbxVelNed.Status; // status pointer
break;
default: // unsupported identifier
ubxState = UBXSTATE_IDLE;
return;
}
break;
default: // other classes
if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE;
else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask)))
{ // buffer is free and message matches to filter criteria
UbxMsg.Status = INVALID;
UbxMsg.Hdr.Class = ubxclass;
UbxMsg.Hdr.Id = ubxid;
UbxMsg.Hdr.Length = msglen;
ubxSp = (u8 *)&(UbxMsg.Data); // data start pointer
ubxEp = (u8 *)(&UbxMsg + 1); // data end pointer
ubxStP = (u8 *)&UbxMsg.Status; // status pointer
}
else ubxState = UBXSTATE_IDLE;
break;
}
*/