diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-09-21 10:17:00 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-09-21 10:17:00 +0200 |
| commit | 32fbf80ab84d3d8ebcb93ec1d7f20470ebb4db1f (patch) | |
| tree | 18ec574254adf2792118542912d2d7b739a82a36 /src/modules | |
| parent | 42f930908fc3f60e6305257f25b625830a020a80 (diff) | |
| download | px4-firmware-32fbf80ab84d3d8ebcb93ec1d7f20470ebb4db1f.tar.gz px4-firmware-32fbf80ab84d3d8ebcb93ec1d7f20470ebb4db1f.tar.bz2 px4-firmware-32fbf80ab84d3d8ebcb93ec1d7f20470ebb4db1f.zip | |
mavlink: EPH/EPV casting issue fixed
Diffstat (limited to 'src/modules')
| -rw-r--r-- | src/modules/mavlink/orb_listener.c | 23 |
1 files changed, 20 insertions, 3 deletions
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d11a67fc6..ed02b17e1 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -99,6 +99,8 @@ struct listener { uintptr_t arg; }; +uint16_t cm_uint16_from_m_float(float m); + static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); static void l_vehicle_gps_position(const struct listener *l); @@ -150,6 +152,19 @@ static const struct listener listeners[] = { static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return m * 0.01f; +} + void l_sensor_combined(const struct listener *l) { @@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l) /* GPS COG is 0..2PI in degrees * 1e2 */ float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; @@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - gps.eph_m * 1e2f, // from m to cm - gps.epv_m * 1e2f, // from m to cm + cm_uint16_from_m_float(gps.eph_m), + cm_uint16_from_m_float(gps.epv_m), gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from rad to deg * 100 + cog_deg * 1e2f, // from deg to deg * 100 gps.satellites_visible); /* update SAT info every 10 seconds */ |