diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-02-18 16:46:05 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-02-18 16:46:05 +0100 |
commit | 663ca58063a281d23dbc92a6fbd19011c3fbde41 (patch) | |
tree | 5a2e9f58a8f41db94ef221e12acead09c9828233 /apps/mavlink/mavlink_receiver.c | |
parent | 104d5aa3654545b354f25750d3980181da8f6a0b (diff) | |
parent | 520a2b417410bed7db6f08a3a69f3bcccc55910b (diff) | |
download | px4-firmware-663ca58063a281d23dbc92a6fbd19011c3fbde41.tar.gz px4-firmware-663ca58063a281d23dbc92a6fbd19011c3fbde41.tar.bz2 px4-firmware-663ca58063a281d23dbc92a6fbd19011c3fbde41.zip |
Merge branch 'master' of github.com:PX4/Firmware
Diffstat (limited to 'apps/mavlink/mavlink_receiver.c')
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 24 |
1 files changed, 12 insertions, 12 deletions
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 510d2f3e4..4d9cd964d 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg) static uint64_t old_timestamp = 0; /* gps */ - hil_gps.timestamp = gps.time_usec; - hil_gps.counter = hil_counter++; + 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 = gps.eph; - hil_gps.epv = gps.epv; - hil_gps.s_variance = 100; - hil_gps.p_variance = 100; - hil_gps.vel = gps.vel; - hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_d = 0.0f; - hil_gps.cog = gps.cog; +// 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 = 100; // XXX 100 m/s variance? + hil_gps.p_variance_m = 100; // XXX 100 m variance? + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_d_m_s = 0.0f; + hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; |