diff --git a/src/gps.c b/src/gps.c index 77bfd4212..10f05a635 100644 --- a/src/gps.c +++ b/src/gps.c @@ -928,24 +928,46 @@ typedef struct { uint32_t speed_2d; int32_t heading_2d; uint32_t speed_accuracy; - uint32_t heading_accuracy; -} ubx_nav_velned; - -enum { - PREAMBLE1 = 0xb5, - PREAMBLE2 = 0x62, + 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, CLASS_NAV = 0x01, CLASS_ACK = 0x05, CLASS_CFG = 0x06, MSG_ACK_NACK = 0x00, MSG_ACK_ACK = 0x01, MSG_POSLLH = 0x2, - MSG_STATUS = 0x3, - MSG_SOL = 0x6, - MSG_VELNED = 0x12, - MSG_CFG_PRT = 0x00, - MSG_CFG_RATE = 0x08, - MSG_CFG_SET_RATE = 0x01, + 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, MSG_CFG_NAV_SETTINGS = 0x24 } ubs_protocol_bytes; @@ -986,13 +1008,14 @@ static uint8_t _disable_counter; // Receive buffer static union { ubx_nav_posllh posllh; - ubx_nav_status status; - ubx_nav_solution solution; - ubx_nav_velned velned; - uint8_t bytes[64]; -} _buffer; - -void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) + ubx_nav_status status; + ubx_nav_solution solution; + ubx_nav_velned velned; + 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) { while (len--) { *ck_a += *data; @@ -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; @@ -1093,12 +1117,23 @@ static bool UBLOX_parse_gps(void) case MSG_VELNED: // speed_3d = _buffer.velned.speed_3d; // cm/s GPS_speed = _buffer.velned.speed_2d; // cm/s - GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - _new_speed = true; - break; - default: - return false; - } + 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; + } // we only return true when we get new position and speed data // this ensures we don't use stale data diff --git a/src/mw.c b/src/mw.c index a2b172cf5..6de1e0aed 100755 --- a/src/mw.c +++ b/src/mw.c @@ -44,12 +44,17 @@ int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for uint16_t GPS_ground_course = 0; // degrees * 10 uint8_t GPS_Present = 0; // Checksum from Gps serial 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 - -// Automatic ACC Offset Calibration -uint16_t InflightcalibratingA = 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; int16_t AccInflightCalibrationArmed; uint16_t AccInflightCalibrationMeasurementDone = 0; uint16_t AccInflightCalibrationSavetoEEProm = 0; diff --git a/src/mw.h b/src/mw.h index ef4168956..551548f1d 100755 --- a/src/mw.h +++ b/src/mw.h @@ -354,12 +354,17 @@ extern int16_t GPS_angle[2]; // it's the angles extern uint16_t GPS_ground_course; // degrees*10 extern uint8_t GPS_Present; // Checksum from Gps serial 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 master_t mcfg; -extern config_t cfg; +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; extern flags_t f; extern sensor_t acc; extern sensor_t gyro; diff --git a/src/serial.c b/src/serial.c index db4ac12d6..eb3dc6222 100755 --- a/src/serial.c +++ b/src/serial.c @@ -47,12 +47,13 @@ #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 // Additional commands that are not compatible with MultiWii -#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 INBUF_SIZE 64 - +#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 + struct box_t { const uint8_t boxIndex; // this is from boxnames enum const char *boxName; // GUI-readable box name @@ -565,13 +566,22 @@ static void evaluateCommand(void) case MSP_UID: headSerialReply(12); serialize32(U_ID_0); - serialize32(U_ID_1); - serialize32(U_ID_2); - break; - - default: // we do not know how to handle the (valid) message, indicate error MSP $M! - headSerialError(0); - break; + 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; } tailSerialReply(); }