From ed9fbbce5946d22ded1519ddec1b5ff6a8f2e511 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Apr 2013 17:25:42 +0200 Subject: HIL bugfixing --- apps/drivers/gps/ubx.cpp | 18 ++++++++++----- apps/examples/kalman_demo/KalmanNav.cpp | 2 ++ apps/mavlink/mavlink_receiver.c | 14 +++++++----- apps/uORB/topics/vehicle_gps_position.h | 39 +++++++++++++++++---------------- 4 files changed, 43 insertions(+), 30 deletions(-) diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 0d4894b8d..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 #include @@ -633,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/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 6df454a55..4ef150da1 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -355,6 +355,8 @@ int KalmanNav::predictState(float dt) float LDot = vN / R; float lDot = vE / (cosLSing * R); float rotRate = 2 * omega + lDot; + + // XXX position prediction using speed float vNDot = fN - vE * rotRate * sinL + vD * LDot; float vDDot = fD - vE * rotRate * cosL - diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index a30f0bf3c..22c2fcdad 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -477,25 +477,27 @@ handle_message(mavlink_message_t *msg) /* gps */ hil_gps.timestamp_position = gps.time_usec; -// hil_gps.counter = hil_counter++; hil_gps.time_gps_usec = gps.time_usec; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; -// hil_gps.counter_pos_valid = hil_counter++; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - /* gps.cog is in degrees 0..360 * 100, heading is -PI..PI */ - float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f - M_PI_F; + /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ + float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ + if (heading_rad > M_PI_F) + heading_rad -= 2.0f * M_PI_F; hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); hil_gps.vel_d_m_s = 0.0f; - /* COG (course over ground) is speced as 0..360 degrees (compass) */ - hil_gps.cog_rad = heading_rad + M_PI_F; // from deg*100 to rad + hil_gps.vel_ned_valid = true; + /* COG (course over ground) is speced as -PI..+PI */ + hil_gps.cog_rad = heading_rad; hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index 5463a460d..0a7fb4e9d 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -55,38 +55,39 @@ */ struct vehicle_gps_position_s { - uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + uint64_t timestamp_position; /**< Timestamp for position information */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + float p_variance_m; /**< position accuracy estimate m */ + float c_variance_rad; /**< course accuracy estimate rad */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ - float vel_m_s; /**< GPS ground speed (m/s) */ - float vel_n_m_s; /**< GPS ground speed in m/s */ - float vel_e_m_s; /**< GPS ground speed in m/s */ - float vel_d_m_s; /**< GPS ground speed in m/s */ - float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */ - bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ + float vel_m_s; /**< GPS ground speed (m/s) */ + float vel_n_m_s; /**< GPS ground speed in m/s */ + float vel_e_m_s; /**< GPS ground speed in m/s */ + float vel_d_m_s; /**< GPS ground speed in m/s */ + float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ + uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ uint8_t satellite_prn[20]; /**< Global satellite ID */ uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - bool satellite_info_available; /**< 0 for no info, 1 for info available */ + bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** -- cgit v1.2.3