From 46301f753d212b55e151a394bc9d4b3b787f35ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 18:28:37 +0200 Subject: Minor fixes to MAVLink --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb1ad86ef..28cdf65a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -190,8 +190,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - if (desired > buf_free) { - desired = buf_free; + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + return; } } @@ -222,6 +223,8 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), -- cgit v1.2.3 From 718206bd6d4103d8d2b1ad6a111770f65622029a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 14:37:23 +0200 Subject: ubx driver: fix unit of speed and position accuracy estimate --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..7502809bd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -447,8 +447,8 @@ UBX::handle_message() gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s + _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; -- cgit v1.2.3 From 47c9d326209034dc373ab54647d29df4e63b5285 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:23:26 +0200 Subject: ubx: disable all debug messages --- src/drivers/gps/ubx.cpp | 170 ++++++++++++++++++++++++------------------------ 1 file changed, 85 insertions(+), 85 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 7502809bd..cbfb61d00 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -219,19 +219,19 @@ UBX::configure(unsigned &baudrate) return 1; } - 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; - } - - 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; - } +// 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; +// } +// +// 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; @@ -486,61 +486,61 @@ UBX::handle_message() break; } - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - const int length_part1 = 8; - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - - uint8_t satellites_used = 0; - int i; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { - /* set pointer to sattelite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); - - /* write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - /* count SVs used for NAV */ - satellites_used++; - } - - /* record info for all channels, whether or not the SV is used for NAV */ - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _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_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++) { - /* unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - - } else { - _gps_position->satellite_info_available = false; - } - - _gps_position->timestamp_satellites = hrt_absolute_time(); - - ret = 1; - break; - } +// case UBX_MESSAGE_NAV_SVINFO: { +// //printf("GOT NAV_SVINFO\n"); +// const int length_part1 = 8; +// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; +// const int length_part2 = 12; +// gps_bin_nav_svinfo_part2_packet_t *packet_part2; +// +// uint8_t satellites_used = 0; +// int i; +// +// //printf("Number of Channels: %d\n", packet_part1->numCh); +// for (i = 0; i < packet_part1->numCh; i++) { +// /* set pointer to sattelite_i information */ +// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); +// +// /* write satellite information to global storage */ +// uint8_t sv_used = packet_part2->flags & 0x01; +// +// if (sv_used) { +// /* count SVs used for NAV */ +// satellites_used++; +// } +// +// /* record info for all channels, whether or not the SV is used for NAV */ +// _gps_position->satellite_used[i] = sv_used; +// _gps_position->satellite_snr[i] = packet_part2->cno; +// _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_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++) { +// /* unused channels have to be set to zero for e.g. MAVLink */ +// _gps_position->satellite_prn[i] = 0; +// _gps_position->satellite_used[i] = 0; +// _gps_position->satellite_snr[i] = 0; +// _gps_position->satellite_elevation[i] = 0; +// _gps_position->satellite_azimuth[i] = 0; +// } +// +// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones +// +// if (packet_part1->numCh > 0) { +// _gps_position->satellite_info_available = true; +// +// } else { +// _gps_position->satellite_info_available = false; +// } +// +// _gps_position->timestamp_satellites = hrt_absolute_time(); +// +// ret = 1; +// break; +// } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -573,23 +573,23 @@ 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; - } - } +// 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; -- cgit v1.2.3 From efb44969d584e1b2613d96a795fc94a3ef728d9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:57:48 +0200 Subject: ubx: send update only if got POSLLH & VELNED & TIMEUTC --- src/drivers/gps/ubx.cpp | 184 +++++++++++++++++++++++++----------------------- src/drivers/gps/ubx.h | 3 + 2 files changed, 100 insertions(+), 87 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index cbfb61d00..c143eeb0c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _gps_position(gps_position), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), + _got_timeutc(false), _disable_cmd_last(0) { decode_init(); @@ -219,19 +222,19 @@ UBX::configure(unsigned &baudrate) return 1; } -// 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; -// } -// -// 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; -// } + 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; + } + + 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; @@ -275,9 +278,10 @@ UBX::receive(unsigned timeout) bool handled = false; while (true) { + bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -286,7 +290,10 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; + _got_timeutc = false; return 1; } else { @@ -438,6 +445,7 @@ UBX::handle_message() _rate_count_lat_lon++; + _got_posllh = true; ret = 1; break; } @@ -482,65 +490,66 @@ UBX::handle_message() _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); + _got_timeutc = true; ret = 1; break; } -// case UBX_MESSAGE_NAV_SVINFO: { -// //printf("GOT NAV_SVINFO\n"); -// const int length_part1 = 8; -// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; -// const int length_part2 = 12; -// gps_bin_nav_svinfo_part2_packet_t *packet_part2; -// -// uint8_t satellites_used = 0; -// int i; -// -// //printf("Number of Channels: %d\n", packet_part1->numCh); -// for (i = 0; i < packet_part1->numCh; i++) { -// /* set pointer to sattelite_i information */ -// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); -// -// /* write satellite information to global storage */ -// uint8_t sv_used = packet_part2->flags & 0x01; -// -// if (sv_used) { -// /* count SVs used for NAV */ -// satellites_used++; -// } -// -// /* record info for all channels, whether or not the SV is used for NAV */ -// _gps_position->satellite_used[i] = sv_used; -// _gps_position->satellite_snr[i] = packet_part2->cno; -// _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_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++) { -// /* unused channels have to be set to zero for e.g. MAVLink */ -// _gps_position->satellite_prn[i] = 0; -// _gps_position->satellite_used[i] = 0; -// _gps_position->satellite_snr[i] = 0; -// _gps_position->satellite_elevation[i] = 0; -// _gps_position->satellite_azimuth[i] = 0; -// } -// -// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones -// -// if (packet_part1->numCh > 0) { -// _gps_position->satellite_info_available = true; -// -// } else { -// _gps_position->satellite_info_available = false; -// } -// -// _gps_position->timestamp_satellites = hrt_absolute_time(); -// -// ret = 1; -// break; -// } + case UBX_MESSAGE_NAV_SVINFO: { + //printf("GOT NAV_SVINFO\n"); + const int length_part1 = 8; + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + + uint8_t satellites_used = 0; + int i; + + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); + + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + /* count SVs used for NAV */ + satellites_used++; + } + + /* record info for all channels, whether or not the SV is used for NAV */ + _gps_position->satellite_used[i] = sv_used; + _gps_position->satellite_snr[i] = packet_part2->cno; + _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_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++) { + /* unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; + } + + _gps_position->timestamp_satellites = hrt_absolute_time(); + + ret = 1; + break; + } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -557,6 +566,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } @@ -573,23 +583,23 @@ 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; -// } -// } + 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 5cf47b60b..43d688893 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -397,6 +397,9 @@ private: struct vehicle_gps_position_s *_gps_position; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; + bool _got_timeutc; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; -- cgit v1.2.3