rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tidy up NMEA Code A Little 

Still a LOT of tidying up needed in future, of the whole GPS module
really.
This commit is contained in:
tracernz 2015-01-31 18:07:05 +13:00
parent b8b248827c
commit c226f6a412
1 changed files with 115 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

228
src/main/io/gps.c
View File

@ -69,6 +69,8 @@ extern int16_t debug[4];
#define LOG_UBLOX_POSLLH 'P' #define LOG_UBLOX_POSLLH 'P'
#define LOG_UBLOX_VELNED 'V' #define LOG_UBLOX_VELNED 'V'
#define GPS_SV_MAXSATS 16
char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
static char *gpsPacketLogChar = gpsPacketLog; static char *gpsPacketLogChar = gpsPacketLog;
// ********************** // **********************
@ -85,11 +87,11 @@ uint16_t GPS_altitude; // altitude in 0.1m
uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10 uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_numCh; // Number of channels uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS]; // Carrier to Noise Ratio (Signal Strength)
static gpsConfig_t *gpsConfig; static gpsConfig_t *gpsConfig;
@ -514,18 +516,18 @@ static uint32_t grab_fields(char *src, uint8_t mult)
return tmp; return tmp;
} }
typedef struct gpsDataNmea_s {
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
uint16_t speed;
uint16_t ground_course;
} gpsDataNmea_t;
static bool gpsNewFrameNMEA(char c) static bool gpsNewFrameNMEA(char c)
{ {
typedef struct gpsdata_s { static gpsDataNmea_t gps_Msg;
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
uint16_t speed;
uint16_t ground_course;
} gpsdata_t;
static gpsdata_t gps_Msg;
uint8_t frameOK = 0; uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0; static uint8_t param = 0, offset = 0, parity = 0;
@ -533,111 +535,111 @@ static bool gpsNewFrameNMEA(char c)
static uint8_t checksum_param, gps_frame = NO_FRAME; static uint8_t checksum_param, gps_frame = NO_FRAME;
switch (c) { switch (c) {
case '$': case '$':
param = 0; param = 0;
offset = 0; offset = 0;
parity = 0; parity = 0;
break;
case ',':
case '*':
string[offset] = 0;
if (param == 0) { //frame identification
gps_frame = NO_FRAME;
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
gps_frame = FRAME_GGA;
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
gps_frame = FRAME_RMC;
}
switch (gps_frame) {
case FRAME_GGA: //************* GPGGA FRAME parsing
switch (param) {
// case 1: // Time information
// break;
case 2:
gps_Msg.latitude = GPS_coord_to_degrees(string);
break;
case 3:
if (string[0] == 'S')
gps_Msg.latitude *= -1;
break;
case 4:
gps_Msg.longitude = GPS_coord_to_degrees(string);
break;
case 5:
if (string[0] == 'W')
gps_Msg.longitude *= -1;
break;
case 6:
if (string[0] > '0') {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
break;
case 9:
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
break;
}
break; break;
case FRAME_RMC: //************* GPRMC FRAME parsing case ',':
switch (param) { case '*':
case 7: string[offset] = 0;
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis if (param == 0) { //frame identification
break; gps_frame = NO_FRAME;
case 8: if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10 gps_frame = FRAME_GGA;
break; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
gps_frame = FRAME_RMC;
} }
break;
}
param++; switch (gps_frame) {
offset = 0; case FRAME_GGA: //************* GPGGA FRAME parsing
if (c == '*') switch(param) {
checksum_param = 1; // case 1: // Time information
else // break;
parity ^= c; case 2:
break; gps_Msg.latitude = GPS_coord_to_degrees(string);
case '\r': break;
case '\n': case 3:
if (checksum_param) { //parity checksum if (string[0] == 'S')
shiftPacketLog(); gps_Msg.latitude *= -1;
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); break;
if (checksum == parity) { case 4:
*gpsPacketLogChar = LOG_IGNORED; gps_Msg.longitude = GPS_coord_to_degrees(string);
GPS_packetCount++; break;
switch (gps_frame) { case 5:
case FRAME_GGA: if (string[0] == 'W')
*gpsPacketLogChar = LOG_NMEA_GGA; gps_Msg.longitude *= -1;
frameOK = 1; break;
if (STATE(GPS_FIX)) { case 6:
GPS_coord[LAT] = gps_Msg.latitude; if (string[0] > '0') {
GPS_coord[LON] = gps_Msg.longitude; ENABLE_STATE(GPS_FIX);
GPS_numSat = gps_Msg.numSat; } else {
GPS_altitude = gps_Msg.altitude; DISABLE_STATE(GPS_FIX);
}
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
break;
case 9:
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
break;
} }
break; break;
case FRAME_RMC: case FRAME_RMC: //************* GPRMC FRAME parsing
*gpsPacketLogChar = LOG_NMEA_RMC; switch(param) {
GPS_speed = gps_Msg.speed; case 7:
GPS_ground_course = gps_Msg.ground_course; gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
break;
case 8:
gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10
break;
}
break; break;
} // end switch
} else {
*gpsPacketLogChar = LOG_ERROR;
} }
}
checksum_param = 0; param++;
break; offset = 0;
default: if (c == '*')
if (offset < 15) checksum_param = 1;
string[offset++] = c; else
if (!checksum_param) parity ^= c;
parity ^= c; break;
case '\r':
case '\n':
if (checksum_param) { //parity checksum
shiftPacketLog();
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
if (checksum == parity) {
*gpsPacketLogChar = LOG_IGNORED;
GPS_packetCount++;
switch (gps_frame) {
case FRAME_GGA:
*gpsPacketLogChar = LOG_NMEA_GGA;
frameOK = 1;
if (STATE(GPS_FIX)) {
GPS_coord[LAT] = gps_Msg.latitude;
GPS_coord[LON] = gps_Msg.longitude;
GPS_numSat = gps_Msg.numSat;
GPS_altitude = gps_Msg.altitude;
}
break;
case FRAME_RMC:
*gpsPacketLogChar = LOG_NMEA_RMC;
GPS_speed = gps_Msg.speed;
GPS_ground_course = gps_Msg.ground_course;
break;
} // end switch
} else {
*gpsPacketLogChar = LOG_ERROR;
}
}
checksum_param = 0;
break;
default:
if (offset < 15)
string[offset++] = c;
if (!checksum_param)
parity ^= c;
} }
return frameOK; return frameOK;
} }