diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-23 15:08:33 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-23 15:08:33 +0200 |
commit | c4d79b84b4eb3b4d255706f29972cd3f31689749 (patch) | |
tree | fa0bc213731474cbebcbb32e441c5b6ad4d1f4d5 /src/drivers/gps | |
parent | 7bf1f82f615b05bb37b8c4c45891bbde96745a4b (diff) | |
download | px4-firmware-c4d79b84b4eb3b4d255706f29972cd3f31689749.tar.gz px4-firmware-c4d79b84b4eb3b4d255706f29972cd3f31689749.tar.bz2 px4-firmware-c4d79b84b4eb3b4d255706f29972cd3f31689749.zip |
bugfixes in UBX driver, emit new SNR, Jamming and noise count indices
Diffstat (limited to 'src/drivers/gps')
-rw-r--r-- | src/drivers/gps/gps.cpp | 6 | ||||
-rw-r--r-- | src/drivers/gps/gps_helper.cpp | 4 | ||||
-rw-r--r-- | src/drivers/gps/gps_helper.h | 8 | ||||
-rw-r--r-- | src/drivers/gps/ubx.cpp | 25 | ||||
-rw-r--r-- | src/drivers/gps/ubx.h | 28 |
5 files changed, 62 insertions, 9 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6195cd6ea..5342ccf78 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -448,10 +448,10 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, - _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, + _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); + warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 2360ff39b..3b92f1bf4 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate() return _rate_vel; } -float +void GPS_Helper::reset_update_rates() { _rate_count_vel = 0; @@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates() _interval_rate_start = hrt_absolute_time(); } -float +void GPS_Helper::store_update_rates() { _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index cfb9e0d43..d14a95afe 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -46,13 +46,17 @@ class GPS_Helper { public: + + GPS_Helper() {}; + virtual ~GPS_Helper() {}; + virtual int configure(unsigned &baud) = 0; virtual int receive(unsigned timeout) = 0; int set_baudrate(const int &fd, unsigned baud); float get_position_update_rate(); float get_velocity_update_rate(); - float reset_update_rates(); - float store_update_rates(); + void reset_update_rates(); + void store_update_rates(); protected: uint8_t _rate_count_lat_lon; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f4a065c19..19cf5beec 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -226,6 +226,13 @@ UBX::configure(unsigned &baudrate) return 1; } + configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: MON HW"); + return 1; + } + _configured = true; return 0; } @@ -566,6 +573,24 @@ UBX::handle_message() break; } + 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; + + ret = 1; + break; + } + + default: + break; + } + } + default: break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 79a904f4a..5cf47b60b 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -56,6 +56,7 @@ //#define UBX_CLASS_RXM 0x02 #define UBX_CLASS_ACK 0x05 #define UBX_CLASS_CFG 0x06 +#define UBX_CLASS_MON 0x0A /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 @@ -72,6 +73,8 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_MON_HW 0x09 + #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -210,6 +213,27 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_velned_packet_t; +struct gps_bin_mon_hw_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[25]; + 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 */ // int16_t week; /**< Measurement GPS week number */ @@ -319,7 +343,7 @@ typedef enum { //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; #pragma pack(pop) -#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer +#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer class UBX : public GPS_Helper { @@ -383,7 +407,7 @@ private: uint8_t _message_class; uint8_t _message_id; unsigned _payload_size; - uint8_t _disable_cmd_last; + hrt_abstime _disable_cmd_last; }; #endif /* UBX_H_ */ |