diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index cfcc815fd9..fd0477726d 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1851,6 +1851,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #if MAVLINK10 != ENABLED // This is used both as a sensor and to pass the location // in HIL_ATTITUDE mode. +#if MAVLINK10 == ENABLED + case MAVLINK_MSG_ID_GPS_RAW_INT: + { + // decode + mavlink_gps_raw_int_t packet; + mavlink_msg_gps_raw_int_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.time_usec/1000.0,packet.lat/1.0e7,packet.lon/1.0e7,packet.alt/1000, + packet.vel/100,packet.cog/100,0,0); + if (gps_base_alt == 0) { + gps_base_alt = packet.alt/10; + } + current_loc.lng = packet.lon; + current_loc.lat = packet.lat; + current_loc.alt = g_gps->altitude - gps_base_alt; + if (!home_is_set) { + init_home(); + } + break; + } +#else case MAVLINK_MSG_ID_GPS_RAW: //32 { // decode @@ -1859,7 +1881,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set gps hil sensor g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, - packet.v,packet.hdg,0,0); + packet.v,packet.hdg,0,0); if (gps_base_alt == 0) { gps_base_alt = packet.alt*100; }