fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 02:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

U-blox driver rework, step 1

Browse Source 
Handle u-blox 7+8 GNSS, too.
Disabled NAV-SVINFO for now (will be rewritten as an optional feature in
a later step).
Reduced parser buffer size from 300 to 80.
This commit is contained in:
Kynos
2014-05-29 21:23:57 +02:00
parent 58c9d7a723
commit 6df2fe9aeb
2 changed files with 75 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ubx.cpp
+28 -11
View File
@@ -219,12 +219,14 @@ UBX::configure(unsigned &baudrate)
return 1;
}
#ifdef UBX_ENABLE_NAV_SVINFO
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SVINFO");
return 1;
}
#endif
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
@@ -386,7 +388,6 @@ UBX::parse_char(uint8_t b)
if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
/* compare checksum */
if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
decode_init();
return 1; // message received successfully
} else {
@@ -399,7 +400,7 @@ UBX::parse_char(uint8_t b)
_rx_count++;
} else {
warnx("buffer full");
warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size);
decode_init();
return -1;
}
@@ -450,6 +451,7 @@ UBX::handle_message()
_gps_position->s_variance_m_s = packet->sAcc;
_gps_position->p_variance_m = packet->pAcc;
_gps_position->timestamp_variance = hrt_absolute_time();
_gps_position->satellites_visible = packet->numSV;
ret = 1;
break;
@@ -486,6 +488,7 @@ UBX::handle_message()
break;
}
#ifdef UBX_ENABLE_NAV_SVINFO
case UBX_MESSAGE_NAV_SVINFO: {
//printf("GOT NAV_SVINFO\n");
const int length_part1 = 8;
@@ -527,7 +530,9 @@ UBX::handle_message()
_gps_position->satellite_azimuth[i] = 0;
}
_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
/* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */
/* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones
/* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */
if (packet_part1->numCh > 0) {
_gps_position->satellite_info_available = true;
@@ -541,6 +546,7 @@ UBX::handle_message()
ret = 1;
break;
}
#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */
case UBX_MESSAGE_NAV_VELNED: {
// printf("GOT NAV_VELNED\n");
@@ -573,23 +579,34 @@ UBX::handle_message()
break;
}
case UBX_CLASS_MON: {
case UBX_CLASS_MON:
switch (_message_id) {
case UBX_MESSAGE_MON_HW: {
struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
_gps_position->noise_per_ms = p->noisePerMS;
_gps_position->jamming_indicator = p->jamInd;
case UBX_MESSAGE_MON_HW:
switch (_payload_size) {
case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE: /* u-blox 6 msg format */
_gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS;
_gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd;
ret = 1;
break;
case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE: /* u-blox 7+ msg format */
_gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS;
_gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd;
ret = 1;
break;
default: // unexpected payload size:
ret = 1; // ignore but don't disable msg
break;
}
break;
default:
break;
}
}
break;
default:
break;
@@ -597,7 +614,7 @@ UBX::handle_message()
if (ret == 0) {
/* message not handled */
warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size);
hrt_abstime t = hrt_absolute_time();
src/drivers/gps/ubx.h
+47 -8
View File
@@ -48,6 +48,10 @@
#include "gps_helper.h"
/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */
/* TODO: make this a command line option, allocate buffer dynamically */
#undef UBX_ENABLE_NAV_SVINFO
#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
@@ -60,10 +64,8 @@
/* MessageIDs (the ones that are used) */
#define UBX_MESSAGE_NAV_POSLLH 0x02
//#define UBX_MESSAGE_NAV_DOP 0x04
#define UBX_MESSAGE_NAV_SOL 0x06
#define UBX_MESSAGE_NAV_VELNED 0x12
//#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
#define UBX_MESSAGE_NAV_SVINFO 0x30
#define UBX_MESSAGE_ACK_NAK 0x00
@@ -72,9 +74,22 @@
#define UBX_MESSAGE_CFG_MSG 0x01
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_MESSAGE_CFG_NAV5 0x24
#define UBX_MESSAGE_MON_HW 0x09
/* Rx msg payload sizes */
#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE 28 /**< NAV_POSLLH Rx msg payload size */
#define UBX_NAV_SOL_RX_PAYLOAD_SIZE 52 /**< NAV_SOL Rx msg payload size */
#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE 36 /**< NAV_VELNED Rx msg payload size */
#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE 20 /**< NAV_TIMEUTC Rx msg payload size */
#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE 68 /**< MON_HW Rx msg payload size for u-blox 6 and below */
#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE 60 /**< MON_HW Rx msg payload size for u-blox 7 and above */
#define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */
/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */
#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */
#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */
/* CFG class Tx msg defs */
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
@@ -87,7 +102,6 @@
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
#define UBX_CFG_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
@@ -98,7 +112,6 @@
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
#define UBX_MAX_PAYLOAD_LENGTH 500
// ************
/** the structures of the binary packets */
@@ -140,7 +153,7 @@ typedef struct {
uint32_t sAcc;
uint16_t pDOP;
uint8_t reserved1;
uint8_t numSV;
uint8_t numSV; /**< Number of SVs used in Nav Solution */
uint32_t reserved2;
uint8_t ck_a;
uint8_t ck_b;
@@ -213,7 +226,7 @@ typedef struct {
uint8_t ck_b;
} gps_bin_nav_velned_packet_t;
struct gps_bin_mon_hw_packet {
struct gps_bin_mon_hw_ubx6_packet {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
@@ -233,6 +246,25 @@ struct gps_bin_mon_hw_packet {
uint32_t pullL;
};
struct gps_bin_mon_hw_ubx7_packet {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t __reserved1;
uint32_t usedMask;
uint8_t VP[17];
uint8_t jamInd;
uint16_t __reserved3;
uint32_t pinIrq;
uint32_t pulLH;
uint32_t pullL;
};
//typedef struct {
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
@@ -343,7 +375,14 @@ typedef enum {
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */
/* TODO: make this a command line option, allocate buffer dynamically */
#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */
#ifdef UBX_ENABLE_NAV_SVINFO
#define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
#else
#define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
#endif
class UBX : public GPS_Helper
{