dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: EPH/EPV casting issue fixed

This commit is contained in:
Anton Babushkin 2013-09-21 10:17:00 +02:00
parent 42f930908f
commit 32fbf80ab8
1 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/mavlink/orb_listener.c
View File

@ -99,6 +99,8 @@ struct listener {
uintptr_t arg; uintptr_t arg;
}; };
uint16_t cm_uint16_from_m_float(float m);
static void l_sensor_combined(const struct listener *l); static void l_sensor_combined(const struct listener *l);
static void l_vehicle_attitude(const struct listener *l); static void l_vehicle_attitude(const struct listener *l);
static void l_vehicle_gps_position(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]); 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 void
l_sensor_combined(const struct listener *l) 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 */ /* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = gps.cog_rad; float cog_deg = gps.cog_rad;
if (cog_deg > M_PI_F) if (cog_deg > M_PI_F)
cog_deg -= 2.0f * M_PI_F; cog_deg -= 2.0f * M_PI_F;
cog_deg *= M_RAD_TO_DEG_F; cog_deg *= M_RAD_TO_DEG_F;
@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l)
gps.lat, gps.lat,
gps.lon, gps.lon,
gps.alt, gps.alt,
gps.eph_m * 1e2f, // from m to cm cm_uint16_from_m_float(gps.eph_m),
gps.epv_m * 1e2f, // from m to cm cm_uint16_from_m_float(gps.epv_m),
gps.vel_m_s * 1e2f, // from m/s to cm/s 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); gps.satellites_visible);
/* update SAT info every 10 seconds */ /* update SAT info every 10 seconds */