diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-25 17:25:42 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-25 17:25:42 +0200 |
| commit | ed9fbbce5946d22ded1519ddec1b5ff6a8f2e511 (patch) | |
| tree | d719793cb905d2b40b0d80b5acfb3452008c6b5f /apps/mavlink | |
| parent | d62058eccf7d3801a752aac11fb2c37f198a239c (diff) | |
| download | px4-firmware-ed9fbbce5946d22ded1519ddec1b5ff6a8f2e511.tar.gz px4-firmware-ed9fbbce5946d22ded1519ddec1b5ff6a8f2e511.tar.bz2 px4-firmware-ed9fbbce5946d22ded1519ddec1b5ff6a8f2e511.zip | |
HIL bugfixing
Diffstat (limited to 'apps/mavlink')
| -rw-r--r-- | apps/mavlink/mavlink_receiver.c | 14 |
1 files changed, 8 insertions, 6 deletions
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; |