diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-26 17:41:46 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-26 17:41:46 +0200 |
| commit | 556a017444b809c18e2ce495a2fd00380960e0f4 (patch) | |
| tree | 6c76cc2b8898bb9280863d567b2650f498f13e69 | |
| parent | afbb4d55b3fc4e0e2a1fc1a1b052e9f4fb034d51 (diff) | |
| download | px4-firmware-556a017444b809c18e2ce495a2fd00380960e0f4.tar.gz px4-firmware-556a017444b809c18e2ce495a2fd00380960e0f4.tar.bz2 px4-firmware-556a017444b809c18e2ce495a2fd00380960e0f4.zip | |
Hotfix: GPS MAVLink transmission fixes
| -rw-r--r-- | apps/mavlink/orb_listener.c | 18 |
1 files changed, 13 insertions, 5 deletions
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 1dd3fc2d8..5f15facf8 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -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, |