Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +03:00
Code Issues Wiki Activity

Merge pull request #437 from tracernz/nmea-satdata

Browse source 
Add NMEA Signal Quality
This commit is contained in:
Dominic Clifton 2015-03-10 00:18:47 +01:00
commit d8cd9f239c
1 changed files with 163 additions and 113 deletions
Download patch file Download diff file Expand all files Collapse all files

76
src/main/io/gps.c
View file

@ -65,6 +65,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;
// ********************** // **********************
@ -82,10 +84,10 @@ 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;
@ -452,6 +454,7 @@ bool gpsNewFrame(uint8_t c)
#define NO_FRAME 0 #define NO_FRAME 0
#define FRAME_GGA 1 #define FRAME_GGA 1
#define FRAME_RMC 2 #define FRAME_RMC 2
#define FRAME_GSV 3
// This code is used for parsing NMEA data // This code is used for parsing NMEA data
@ -516,23 +519,25 @@ static uint32_t grab_fields(char *src, uint8_t mult)
return tmp; return tmp;
} }
static bool gpsNewFrameNMEA(char c) typedef struct gpsDataNmea_s {
{
typedef struct gpsdata_s {
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
uint8_t numSat; uint8_t numSat;
uint16_t altitude; uint16_t altitude;
uint16_t speed; uint16_t speed;
uint16_t ground_course; uint16_t ground_course;
} gpsdata_t; } gpsDataNmea_t;
static gpsdata_t gps_Msg; static bool gpsNewFrameNMEA(char c)
{
static gpsDataNmea_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;
static char string[15]; static char string[15];
static uint8_t checksum_param, gps_frame = NO_FRAME; static uint8_t checksum_param, gps_frame = NO_FRAME;
static uint8_t svMessageNum = 0;
uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
switch (c) { switch (c) {
case '$': case '$':
@ -549,13 +554,15 @@ static bool gpsNewFrameNMEA(char c)
gps_frame = FRAME_GGA; gps_frame = FRAME_GGA;
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
gps_frame = FRAME_RMC; gps_frame = FRAME_RMC;
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V')
gps_frame = FRAME_GSV;
} }
switch (gps_frame) { switch (gps_frame) {
case FRAME_GGA: //************* GPGGA FRAME parsing case FRAME_GGA: //************* GPGGA FRAME parsing
switch (param) { switch(param) {
// case 1: // Time information // case 1: // Time information
// break; // break;
case 2: case 2:
gps_Msg.latitude = GPS_coord_to_degrees(string); gps_Msg.latitude = GPS_coord_to_degrees(string);
break; break;
@ -586,7 +593,7 @@ static bool gpsNewFrameNMEA(char c)
} }
break; break;
case FRAME_RMC: //************* GPRMC FRAME parsing case FRAME_RMC: //************* GPRMC FRAME parsing
switch (param) { switch(param) {
case 7: case 7:
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
break; break;
@ -595,6 +602,49 @@ static bool gpsNewFrameNMEA(char c)
break; break;
} }
break; break;
case FRAME_GSV:
switch(param) {
/*case 1:
// Total number of messages of this type in this cycle
break; */
case 2:
// Message number
svMessageNum = grab_fields(string, 0);
break;
case 3:
// Total number of SVs visible
GPS_numCh = grab_fields(string, 0);
break;
}
if(param < 4)
break;
svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
if(svSatNum > GPS_SV_MAXSATS)
break;
switch(svSatParam) {
case 1:
// SV PRN number
GPS_svinfo_chn[svSatNum - 1] = svSatNum;
GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
break;
/*case 2:
// Elevation, in degrees, 90 maximum
break;
case 3:
// Azimuth, degrees from True North, 000 through 359
break; */
case 4:
// SNR, 00 through 99 dB (null when not tracking)
GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
break;
}
break;
} }
param++; param++;