diff options
Diffstat (limited to 'apps/drivers')
| -rw-r--r-- | apps/drivers/gps/ubx.cpp | 24 | ||||
| -rw-r--r-- | apps/drivers/l3gd20/l3gd20.cpp | 7 |
2 files changed, 21 insertions, 10 deletions
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 74cbc5aaf..c150f5271 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -33,7 +33,14 @@ * ****************************************************************************/ -/* @file U-Blox protocol implementation */ +/** + * @file ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + */ #include <unistd.h> #include <stdio.h> @@ -113,13 +120,15 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ + receive(UBX_CONFIG_TIMEOUT); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } - /* no ack is ecpected here, keep going configuring */ - /* send a CFT-RATE message to define update rate */ type_gps_bin_cfg_rate_packet_t cfg_rate_packet; memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); @@ -631,16 +640,17 @@ UBX::handle_message() } case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); if (!_waiting_for_ack) { + /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ _gps_position->cog_rad = (float)packet->heading * 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->timestamp_velocity = hrt_absolute_time(); } diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 6227df72a..c7f433dd4 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -684,9 +684,10 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; |