[PATCH] GPS: Signal Strength for u-Blox only

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@334 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/gps.c

@@ -931,6 +931,27 @@ typedef struct {
     uint32_t heading_accuracy;
 } ubx_nav_velned;
 
+typedef struct
+{
+    uint8_t chn;                // Channel number, 255 for SVx not assigned to channel
+    uint8_t svid;               // Satellite ID
+    uint8_t flags;              // Bitmask
+    uint8_t quality;            // Bitfield
+    uint8_t cno;                // Carrier to Noise Ratio (Signal Strength)
+    uint8_t elev;               // Elevation in integer degrees
+    int16_t azim;               // Azimuth in integer degrees
+    int32_t prRes;              // Pseudo range residual in centimetres
+} ubx_nav_svinfo_channel;
+
+typedef struct
+{
+    uint32_t time;              // GPS Millisecond time of week
+    uint8_t numCh;              // Number of channels
+    uint8_t globalFlags;        // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
+    uint16_t reserved2;         // Reserved
+    ubx_nav_svinfo_channel channel[16];         // 16 satellites * 12 byte
+} ubx_nav_svinfo;
+
 enum {
     PREAMBLE1 = 0xb5,
     PREAMBLE2 = 0x62,
@@ -943,6 +964,7 @@ enum {
     MSG_STATUS = 0x3,
     MSG_SOL = 0x6,
     MSG_VELNED = 0x12,
+    MSG_SVINFO = 0x30,
     MSG_CFG_PRT = 0x00,
     MSG_CFG_RATE = 0x08,
     MSG_CFG_SET_RATE = 0x01,
@@ -989,7 +1011,8 @@ static union {
     ubx_nav_status status;
     ubx_nav_solution solution;
     ubx_nav_velned velned;
-    uint8_t bytes[64];
+    ubx_nav_svinfo svinfo;
+    uint8_t bytes[200];
 } _buffer;
 
 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
@@ -1068,6 +1091,7 @@ static bool GPS_UBLOX_newFrame(uint8_t data)
 
 static bool UBLOX_parse_gps(void)
 {
+    int i;
     switch (_msg_id) {
     case MSG_POSLLH:
         //i2c_dataset.time                = _buffer.posllh.time;
@@ -1096,6 +1120,17 @@ static bool UBLOX_parse_gps(void)
         GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
         _new_speed = true;
         break;
+    case MSG_SVINFO:
+        GPS_numCh = _buffer.svinfo.numCh;
+        if (GPS_numCh > 16)
+            GPS_numCh = 16;
+        for (i = 0; i < GPS_numCh; i++){
+            GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
+            GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
+            GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
+            GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
+        }
+        break;
     default:
         return false;
     }

src/mw.c

@@ -47,6 +47,11 @@ uint8_t GPS_Enable = 0;
 int16_t nav[2];
 int16_t nav_rated[2];               // Adding a rate controller to the navigation to make it smoother
 int8_t nav_mode = NAV_MODE_NONE;    // Navigation mode
+uint8_t  GPS_numCh;                 // Number of channels
+uint8_t  GPS_svinfo_chn[16];        // Channel number
+uint8_t  GPS_svinfo_svid[16];       // Satellite ID
+uint8_t  GPS_svinfo_quality[16];    // Bitfield Qualtity
+uint8_t  GPS_svinfo_cno[16];        // Carrier to Noise Ratio (Signal Strength)
 
 // Automatic ACC Offset Calibration
 uint16_t InflightcalibratingA = 0;

src/mw.h

@@ -357,6 +357,11 @@ extern uint8_t  GPS_Enable;
 extern int16_t  nav[2];
 extern int8_t   nav_mode;                                    // Navigation mode
 extern int16_t  nav_rated[2];                                // Adding a rate controller to the navigation to make it smoother
+extern uint8_t  GPS_numCh;                                   // Number of channels
+extern uint8_t  GPS_svinfo_chn[16];                          // Channel number
+extern uint8_t  GPS_svinfo_svid[16];                         // Satellite ID
+extern uint8_t  GPS_svinfo_quality[16];                      // Bitfield Qualtity
+extern uint8_t  GPS_svinfo_cno[16];                          // Carrier to Noise Ratio (Signal Strength)
 
 extern master_t mcfg;
 extern config_t cfg;

src/serial.c

@@ -50,6 +50,7 @@
 #define MSP_UID                  160    //out message         Unique device ID
 #define MSP_ACC_TRIM             240    //out message         get acc angle trim values
 #define MSP_SET_ACC_TRIM         239    //in message          set acc angle trim values
+#define MSP_GPSSVINFO            164    //out message         get Signal Strength (only U-Blox)
 
 #define INBUF_SIZE 64
 
@@ -568,7 +569,16 @@ static void evaluateCommand(void)
         serialize32(U_ID_1);
         serialize32(U_ID_2);
         break;
-
+    case MSP_GPSSVINFO:
+        headSerialReply(1 + (GPS_numCh * 4));
+        serialize8(GPS_numCh);
+           for (i = 0; i < GPS_numCh; i++){
+               serialize8(GPS_svinfo_chn[i]);
+               serialize8(GPS_svinfo_svid[i]);
+               serialize8(GPS_svinfo_quality[i]);
+               serialize8(GPS_svinfo_cno[i]);
+            }
+        break;
     default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
         headSerialError(0);
         break;