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

Hotfix: GPS MAVLink transmission fixes

This commit is contained in:
Lorenz Meier 2013-04-26 17:41:46 +02:00
parent afbb4d55b3
commit 556a017444
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
apps/mavlink/orb_listener.c
View File

@ -229,6 +229,13 @@ l_vehicle_gps_position(const struct listener *l)
/* copy gps data into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
/* 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;
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
gps.timestamp_position,
@ -236,13 +243,14 @@ l_vehicle_gps_position(const struct listener *l)
gps.lat,
gps.lon,
gps.alt,
(uint16_t)(gps.eph_m * 1e2f), // from m to cm
(uint16_t)(gps.epv_m * 1e2f), // from m to cm
(uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s
(uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100
gps.eph_m * 1e2f, // from m to cm
gps.epv_m * 1e2f, // from m to cm
gps.vel_m_s * 1e2f, // from m/s to cm/s
cog_deg * 1e2f, // from rad to deg * 100
gps.satellites_visible);
if (gps.satellite_info_available && (gps_counter % 4 == 0)) {
/* update SAT info every 10 seconds */
if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
mavlink_msg_gps_status_send(MAVLINK_COMM_0,
gps.satellites_visible,
gps.satellite_prn,