diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-22 14:39:49 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-22 14:39:49 +0200 |
| commit | a817dedf11f60cafee262e81531208a260b7f13a (patch) | |
| tree | d7664a2b2947f5d85c2ddb8ae35506f82a0a7674 /apps | |
| parent | 82d2ab677ea6bfccdc211a6e0c77a7904175e0c0 (diff) | |
| download | px4-firmware-a817dedf11f60cafee262e81531208a260b7f13a.tar.gz px4-firmware-a817dedf11f60cafee262e81531208a260b7f13a.tar.bz2 px4-firmware-a817dedf11f60cafee262e81531208a260b7f13a.zip | |
Fixed bug in MAVLink HIL interface, now consistent with in-flight results in the field
Diffstat (limited to 'apps')
| -rw-r--r-- | apps/mavlink/mavlink_receiver.c | 14 |
1 files changed, 9 insertions, 5 deletions
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 588b3e07e..14631d503 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -485,13 +485,17 @@ handle_message(mavlink_message_t *msg) // 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.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 - 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); + + /* 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; + 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; - hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad + /* COG (course over ground) is speced as 0..360 degrees (compass) */ + hil_gps.cog_rad = cog_rad + M_PI_F; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; |