yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

GPS velocity update rate validation for u-blox, WIP

Browse Source
This commit is contained in:
Lorenz Meier
2013-05-05 12:57:21 +02:00
parent f9c0ff0f20
commit 3466006735
7 changed files with 203 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
apps/drivers/gps/gps.cpp
View File

@@ -285,6 +285,11 @@ GPS::task_main()
unlock(); unlock();
if (_Helper->configure(_baudrate) == 0) { if (_Helper->configure(_baudrate) == 0) {
unlock(); 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) { while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock(); // lock();
/* opportunistic publishing - else invalid data would end up on the bus */ /* 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, warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); 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); usleep(100000);

27
apps/drivers/gps/gps_helper.cpp
View File

@@ -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> * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch> * Julian Oes <joes@student.ethz.ch>
* *
@@ -36,9 +36,32 @@
#include <termios.h> #include <termios.h>
#include <errno.h> #include <errno.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "gps_helper.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 int
GPS_Helper::set_baudrate(const int &fd, unsigned baud) GPS_Helper::set_baudrate(const int &fd, unsigned baud)

15
apps/drivers/gps/gps_helper.h
View File

@@ -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> * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@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 #ifndef GPS_HELPER_H
#define GPS_HELPER_H #define GPS_HELPER_H
@@ -47,6 +49,15 @@ public:
virtual int configure(unsigned &baud) = 0; virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 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 */ #endif /* GPS_HELPER_H */

4
apps/drivers/gps/mtk.cpp
View File

@@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); _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; return;
} }

162
apps/drivers/gps/ubx.cpp
View File

@@ -60,7 +60,8 @@
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd), _fd(fd),
_gps_position(gps_position), _gps_position(gps_position),
_waiting_for_ack(false) _waiting_for_ack(false),
_disable_cmd_counter(0)
{ {
decode_init(); decode_init();
} }
@@ -144,7 +145,7 @@ UBX::configure(unsigned &baudrate)
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); 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 */ /* try next baudrate */
continue; continue;
} }
@@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate)
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) { if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */ /* try next baudrate */
continue; continue;
} }
type_gps_bin_cfg_msg_packet_t cfg_msg_packet; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
/* insist of receiving the ACK for this packet */
_clsID_needed = UBX_CLASS_CFG; // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
_msgID_needed = UBX_MESSAGE_CFG_MSG; // continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
cfg_msg_packet.clsID = UBX_CLASS_CFG; UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; // /* insist of receiving the ACK for this packet */
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ // continue;
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; // /* insist of receiving the ACK for this packet */
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
if (receive(UBX_CONFIG_TIMEOUT) < 0) { UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
/* try next baudrate */ // /* insist of receiving the ACK for this packet */
continue; // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
} // continue;
// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; // 0);
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; // /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); // continue;
if (receive(UBX_CONFIG_TIMEOUT) < 0) { configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
/* try next baudrate */ 0);
continue; // /* insist of receiving the ACK for this packet */
} // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// 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) {
/* 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;
_waiting_for_ack = false; _waiting_for_ack = false;
return 0; return 0;
@@ -239,6 +207,15 @@ UBX::configure(unsigned &baudrate)
return -1; return -1;
} }
int
UBX::wait_for_ack(unsigned timeout)
{
_waiting_for_ack = true;
int ret = receive(timeout);
_waiting_for_ack = false;
return ret;
}
int int
UBX::receive(unsigned timeout) UBX::receive(unsigned timeout)
{ {
@@ -498,6 +475,8 @@ UBX::handle_message()
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m _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 _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
_rate_count_lat_lon++;
/* Add timestamp to finish the report */ /* Add timestamp to finish the report */
_gps_position->timestamp_position = hrt_absolute_time(); _gps_position->timestamp_position = hrt_absolute_time();
/* only return 1 when new position is available */ /* 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->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->vel_ned_valid = true; _gps_position->vel_ned_valid = true;
_gps_position->timestamp_velocity = hrt_absolute_time(); _gps_position->timestamp_velocity = hrt_absolute_time();
_rate_count_vel++;
} }
break; break;
@@ -693,6 +674,12 @@ UBX::handle_message()
default: //we don't know the message default: //we don't know the message
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); 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; ret = -1;
break; break;
} }
@@ -736,6 +723,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
message[length-1] = ck_b; message[length-1] = ck_b;
} }
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 void
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{ {
@@ -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? 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"); 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);
}

28
apps/drivers/gps/ubx.h
View File

@@ -78,13 +78,14 @@
#define UBX_CFG_NAV5_LENGTH 36 #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_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_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_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_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 #define UBX_MAX_PAYLOAD_LENGTH 500
@@ -92,6 +93,14 @@
/** the structures of the binary packets */ /** the structures of the binary packets */
#pragma pack(push, 1) #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 { typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t lon; /**< Longitude * 1e-7, deg */ int32_t lon; /**< Longitude * 1e-7, deg */
@@ -274,11 +283,17 @@ typedef struct {
uint16_t length; uint16_t length;
uint8_t msgClass_payload; uint8_t msgClass_payload;
uint8_t msgID_payload; uint8_t msgID_payload;
uint8_t rate[6]; uint8_t rate;
uint8_t ck_a; uint8_t ck_a;
uint8_t ck_b; uint8_t ck_b;
} type_gps_bin_cfg_msg_packet_t; } 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 // END the structures of the binary packets
// ************ // ************
@@ -376,6 +391,14 @@ private:
*/ */
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; struct vehicle_gps_position_s *_gps_position;
ubx_config_state_t _config_state; ubx_config_state_t _config_state;
@@ -390,6 +413,7 @@ private:
ubx_message_class_t _message_class; ubx_message_class_t _message_class;
ubx_message_id_t _message_id; ubx_message_id_t _message_id;
unsigned _payload_size; unsigned _payload_size;
uint8_t _disable_cmd_counter;
}; };
#endif /* UBX_H_ */ #endif /* UBX_H_ */