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

HIL bugfixing

Browse Source
This commit is contained in:
Lorenz Meier
2013-04-25 17:25:42 +02:00
parent d62058eccf
commit ed9fbbce59
4 changed files with 43 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink_receiver.c
+8 -6
View File
@@ -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;