diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 9dd7ee4a32..8e471c9bc9 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -989,6 +989,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			break;
 		}
 #endif
+*/
 #if HIL_MODE == HIL_MODE_SENSORS
 
     case MAVLINK_MSG_ID_RAW_IMU:
@@ -1036,7 +1037,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 #endif // HIL_MODE
-*/
 	} // end switch
 } // end handle mavlink
 
diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h
index 9bc57ca801..00a2a79e67 100644
--- a/ArduCopter/Mavlink_Common.h
+++ b/ArduCopter/Mavlink_Common.h
@@ -154,14 +154,14 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
     
     mavlink_msg_rc_channels_scaled_send(
         chan,
-        10000 * g.rc_1.norm_output(),
+        g.rc_1.servo_out,
+        g.rc_2.servo_out,
+        g.rc_3.radio_out,
+        g.rc_4.servo_out,
+		10000 * g.rc_1.norm_output(),
         10000 * g.rc_2.norm_output(),
         10000 * g.rc_3.norm_output(),
         10000 * g.rc_4.norm_output(),
-        0,
-        0,
-        0,
-        0,
         rssi);
         
      #endif