Browse Source

Hotfix: GPS MAVLink transmission fixes

sbg
Lorenz Meier 12 years ago
parent
commit
556a017444
  1. 18
      apps/mavlink/orb_listener.c

18
apps/mavlink/orb_listener.c

@ -229,6 +229,13 @@ l_vehicle_gps_position(const struct listener *l) @@ -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) @@ -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,

Loading…
Cancel
Save