diff options
| -rw-r--r-- | apps/drivers/gps/gps.cpp | 11 | ||||
| -rw-r--r-- | apps/drivers/gps/gps_helper.cpp | 27 | ||||
| -rw-r--r-- | apps/drivers/gps/gps_helper.h | 19 | ||||
| -rw-r--r-- | apps/drivers/gps/mtk.cpp | 4 | ||||
| -rw-r--r-- | apps/drivers/gps/mtk.h | 6 | ||||
| -rw-r--r-- | apps/drivers/gps/ubx.cpp | 162 | ||||
| -rw-r--r-- | apps/drivers/gps/ubx.h | 76 |
7 files changed, 203 insertions, 102 deletions
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index e35bdb944..7db257816 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -285,6 +285,11 @@ GPS::task_main() unlock(); if (_Helper->configure(_baudrate) == 0) { unlock(); + + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); + warnx("module configuration successful"); + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ @@ -372,7 +377,11 @@ GPS::print_info() warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("update rate: %6.2f Hz", (double)_rate); + 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); + + _Helper->reset_update_rates(); } usleep(100000); diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp index 9c1fad569..b03cccb45 100644 --- a/apps/drivers/gps/gps_helper.cpp +++ b/apps/drivers/gps/gps_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * @@ -36,9 +36,32 @@ #include <termios.h> #include <errno.h> #include <systemlib/err.h> +#include <drivers/drv_hrt.h> #include "gps_helper.h" -/* @file gps_helper.cpp */ +/** + * @file gps_helper.cpp + */ + +float +GPS_Helper::get_position_update_rate() +{ + _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); +} + +float +GPS_Helper::get_velocity_update_rate() +{ + _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); +} + +float +GPS_Helper::reset_update_rates() +{ + _rate_count_vel = 0; + _rate_count_lat_lon = 0; + _interval_rate_start = hrt_absolute_time(); +} int GPS_Helper::set_baudrate(const int &fd, unsigned baud) diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index f3d3bc40b..7e456a6aa 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * @@ -33,7 +33,9 @@ * ****************************************************************************/ -/* @file gps_helper.h */ +/** + * @file gps_helper.h + */ #ifndef GPS_HELPER_H #define GPS_HELPER_H @@ -44,9 +46,18 @@ class GPS_Helper { public: - virtual int configure(unsigned &baud) = 0; + virtual int configure(unsigned &baud) = 0; virtual int receive(unsigned timeout) = 0; - int set_baudrate(const int &fd, unsigned baud); + int set_baudrate(const int &fd, unsigned baud); + float get_position_update_rate(); + float get_velocity_update_rate(); + float reset_update_rates(); + +protected: + uint8_t _rate_count_lat_lon; + uint8_t _rate_count_vel; + + uint64_t _interval_rate_start; }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 4762bd503..62941d74b 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); + // Position and velocity update always at the same time + _rate_count_vel++; + _rate_count_lat_lon++; + return; } diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index d4e390b01..b5cfbf0a6 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -87,14 +87,14 @@ class MTK : public GPS_Helper public: MTK(const int &fd, struct vehicle_gps_position_s *gps_position); ~MTK(); - int receive(unsigned timeout); - int configure(unsigned &baudrate); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: /** * Parse the binary MTK packet */ - int parse_char(uint8_t b, gps_mtk_packet_t &packet); + int parse_char(uint8_t b, gps_mtk_packet_t &packet); /** * Handle the package once it has arrived diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index c150f5271..e887e8f7c 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -60,7 +60,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), _gps_position(gps_position), -_waiting_for_ack(false) +_waiting_for_ack(false), +_disable_cmd_counter(0) { decode_init(); } @@ -144,7 +145,7 @@ UBX::configure(unsigned &baudrate) cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } @@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate) cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_MSG; - - cfg_msg_packet.clsID = UBX_CLASS_CFG; - cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; - cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - /* For satelites info 1Hz is enough */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } -// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - -// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); + /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, + UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, + UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + // configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP, + // 0); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, + 0); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; _waiting_for_ack = false; return 0; @@ -240,6 +208,15 @@ UBX::configure(unsigned &baudrate) } int +UBX::wait_for_ack(unsigned timeout) +{ + _waiting_for_ack = true; + int ret = receive(timeout); + _waiting_for_ack = false; + return ret; +} + +int UBX::receive(unsigned timeout) { /* poll descriptor */ @@ -498,6 +475,8 @@ UBX::handle_message() _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _rate_count_lat_lon++; + /* Add timestamp to finish the report */ _gps_position->timestamp_position = hrt_absolute_time(); /* only return 1 when new position is available */ @@ -653,6 +632,8 @@ UBX::handle_message() _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; _gps_position->vel_ned_valid = true; _gps_position->timestamp_velocity = hrt_absolute_time(); + + _rate_count_vel++; } break; @@ -693,6 +674,12 @@ UBX::handle_message() default: //we don't know the message warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + if (_disable_cmd_counter++ == 0) { + // Don't attempt for every message to disable, some might not be disabled */ + warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id); + configure_message_rate(_message_class, _message_id, 0); + } + return ret; ret = -1; break; } @@ -737,6 +724,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length) } void +UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +{ + for (unsigned i = 0; i < length; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } +} + +void +UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) +{ + struct ubx_cfg_msg_rate msg; + msg.msg_class = msg_class; + msg.msg_id = msg_id; + msg.rate = rate; + send_message(CFG, UBX_CONFIG_STATE_RATE, &msg, sizeof(msg)); +} + +void UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; @@ -753,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? warnx("ubx: config write fail"); } + +void +UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) +{ + struct ubx_header header; + uint8_t ck_a=0, ck_b=0; + header.sync1 = UBX_SYNC1; + header.sync2 = UBX_SYNC2; + header.msg_class = msg_class; + header.msg_id = msg_id; + header.length = size; + + add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); + add_checksum((uint8_t *)msg, size, ck_a, ck_b); + + // Configure receive check + _clsID_needed = msg_class; + _msgID_needed = msg_id; + + write(_fd, (const char *)&header, sizeof(header)); + write(_fd, (const char *)msg, size); + write(_fd, (const char *)&ck_a, 1); + write(_fd, (const char *)&ck_b, 1); +} diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index e3dd1c7ea..a6cd0685d 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -65,11 +65,11 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ -#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ +#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ #define UBX_CFG_RATE_LENGTH 6 #define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ @@ -78,13 +78,14 @@ #define UBX_CFG_NAV5_LENGTH 36 -#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ +#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 */ -#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ +#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ #define UBX_CFG_MSG_LENGTH 8 #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #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 @@ -92,6 +93,14 @@ /** the structures of the binary packets */ #pragma pack(push, 1) +struct ubx_header { + uint8_t sync1; + uint8_t sync2; + uint8_t msg_class; + uint8_t msg_id; + uint16_t length; +}; + typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ int32_t lon; /**< Longitude * 1e-7, deg */ @@ -274,11 +283,17 @@ typedef struct { uint16_t length; uint8_t msgClass_payload; uint8_t msgID_payload; - uint8_t rate[6]; + uint8_t rate; uint8_t ck_a; uint8_t ck_b; } type_gps_bin_cfg_msg_packet_t; +struct ubx_cfg_msg_rate { + uint8_t msg_class; + uint8_t msg_id; + uint8_t rate; +}; + // END the structures of the binary packets // ************ @@ -341,55 +356,64 @@ class UBX : public GPS_Helper public: UBX(const int &fd, struct vehicle_gps_position_s *gps_position); ~UBX(); - int receive(unsigned timeout); - int configure(unsigned &baudrate); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: /** * Parse the binary MTK packet */ - int parse_char(uint8_t b); + int parse_char(uint8_t b); /** * Handle the package once it has arrived */ - int handle_message(void); + int handle_message(void); /** * Reset the parse state machine for a fresh start */ - void decode_init(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void add_byte_to_checksum(uint8_t); + void add_byte_to_checksum(uint8_t); /** * Add the two checksum bytes to an outgoing message */ - void add_checksum_to_message(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t* message, const unsigned length); /** * Helper to send a config packet */ - void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + + void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); + + void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); + + void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + + int wait_for_ack(unsigned timeout); - int _fd; + int _fd; struct vehicle_gps_position_s *_gps_position; ubx_config_state_t _config_state; - bool _waiting_for_ack; - uint8_t _clsID_needed; - uint8_t _msgID_needed; + bool _waiting_for_ack; + uint8_t _clsID_needed; + uint8_t _msgID_needed; ubx_decode_state_t _decode_state; - uint8_t _rx_buffer[RECV_BUFFER_SIZE]; - unsigned _rx_count; - uint8_t _rx_ck_a; - uint8_t _rx_ck_b; - ubx_message_class_t _message_class; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; + ubx_message_class_t _message_class; ubx_message_id_t _message_id; - unsigned _payload_size; + unsigned _payload_size; + uint8_t _disable_cmd_counter; }; #endif /* UBX_H_ */ |