fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ubx: SVINFO parsing optimized and message rate increased, CPU consumption reduced in 6 times, ~0.3% now.

Browse Source
This commit is contained in:
Anton Babushkin
2013-07-11 18:17:36 +04:00
parent 4685871c83
commit 0ccf50bca3
1 changed files with 19 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ubx.cpp
+19 -23
View File
@@ -56,8 +56,9 @@
#include "ubx.h" #include "ubx.h"
#define UBX_CONFIG_TIMEOUT 100 #define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
#define UBX_PACKET_TIMEOUT 2 #define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
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),
@@ -214,7 +215,7 @@ UBX::configure(unsigned &baudrate)
return 1; return 1;
} }
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 10); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
@@ -280,9 +281,11 @@ UBX::receive(unsigned timeout)
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
/* /*
* We are here because poll says there is some data, so this * We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are * won't block even on a blocking device. But don't read immediately
* available, we'll go back to poll() again... * by 1-2 bytes, wait for some more data to save expensive read() calls.
* If more bytes are available, we'll go back to poll() again.
*/ */
usleep(UBX_WAIT_BEFORE_READ * 1000);
count = read(_fd, buf, sizeof(buf)); count = read(_fd, buf, sizeof(buf));
/* pass received bytes to the packet decoder */ /* pass received bytes to the packet decoder */
@@ -459,45 +462,39 @@ UBX::handle_message()
} }
case UBX_MESSAGE_NAV_SVINFO: { case UBX_MESSAGE_NAV_SVINFO: {
// printf("GOT NAV_SVINFO\n"); //printf("GOT NAV_SVINFO\n");
// TODO avoid memcpy
const int length_part1 = 8; const int length_part1 = 8;
char _rx_buffer_part1[length_part1]; gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
const int length_part2 = 12; const int length_part2 = 12;
gps_bin_nav_svinfo_part2_packet_t *packet_part2; gps_bin_nav_svinfo_part2_packet_t *packet_part2;
char _rx_buffer_part2[length_part2]; // for temporal storage
uint8_t satellites_used = 0; uint8_t satellites_used = 0;
int i; int i;
// printf("Number of Channels: %d\n", packet_part1->numCh); //printf("Number of Channels: %d\n", packet_part1->numCh);
for (i = 0; i < packet_part1->numCh; i++) { //for each channel for (i = 0; i < packet_part1->numCh; i++) {
/* set pointer to sattelite_i information */
/* get satellite information from the buffer */ packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
/* write satellite information to global storage */ /* write satellite information to global storage */
uint8_t sv_used = packet_part2->flags & 0x01; uint8_t sv_used = packet_part2->flags & 0x01;
if (sv_used) { if (sv_used) {
// Count SVs used for NAV. /* count SVs used for NAV */
satellites_used++; satellites_used++;
} }
// Record info for all channels, whether or not the SV is used for NAV. /* record info for all channels, whether or not the SV is used for NAV */
_gps_position->satellite_used[i] = sv_used; _gps_position->satellite_used[i] = sv_used;
_gps_position->satellite_snr[i] = packet_part2->cno; _gps_position->satellite_snr[i] = packet_part2->cno;
_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
_gps_position->satellite_prn[i] = packet_part2->svid; _gps_position->satellite_prn[i] = packet_part2->svid;
//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
} }
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused for (i = packet_part1->numCh; i < 20; i++) {
/* Unused channels have to be set to zero for e.g. MAVLink */ /* unused channels have to be set to zero for e.g. MAVLink */
_gps_position->satellite_prn[i] = 0; _gps_position->satellite_prn[i] = 0;
_gps_position->satellite_used[i] = 0; _gps_position->satellite_used[i] = 0;
_gps_position->satellite_snr[i] = 0; _gps_position->satellite_snr[i] = 0;
@@ -507,7 +504,6 @@ UBX::handle_message()
_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
/* set timestamp if any sat info is available */
if (packet_part1->numCh > 0) { if (packet_part1->numCh > 0) {
_gps_position->satellite_info_available = true; _gps_position->satellite_info_available = true;