diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index eb6fc774ef..e1f1230e55 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -629,7 +629,12 @@ static void set_servos(void)
         throttle_slew_limit(last_throttle);
     }
 
-#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
+#if HIL_MODE != HIL_MODE_DISABLED
+    if (!g.hil_servos) {
+        return;
+    }
+#endif
+
     // send values to the PWM timers for output
     // ----------------------------------------
     hal.rcout->write(CH_1, g.channel_roll.radio_out);     // send to Servos
@@ -646,7 +651,6 @@ static void set_servos(void)
     g.rc_10.output_ch(CH_10);
     g.rc_11.output_ch(CH_11);
  # endif
-#endif
 }
 
 static bool demoing_servos;
@@ -656,13 +660,11 @@ static void demo_servos(uint8_t i) {
     while(i > 0) {
         gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
         demoing_servos = true;
-#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
-        hal.rcout->write(1, 1400);
+        servo_write(1, 1400);
         mavlink_delay(400);
-        hal.rcout->write(1, 1600);
+        servo_write(1, 1600);
         mavlink_delay(200);
-        hal.rcout->write(1, 1500);
-#endif
+        servo_write(1, 1500);
         demoing_servos = false;
         mavlink_delay(400);
         i--;
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 5aa520978e..8d75c30a4d 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -339,7 +339,24 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
 
 static void NOINLINE send_radio_out(mavlink_channel_t chan)
 {
-#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
+#if HIL_MODE != HIL_MODE_DISABLED
+    if (!g.hil_servos) {
+        extern RC_Channel* rc_ch[8];
+        mavlink_msg_servo_output_raw_send(
+            chan,
+            micros(),
+            0,     // port
+            rc_ch[0]->radio_out,
+            rc_ch[1]->radio_out,
+            rc_ch[2]->radio_out,
+            rc_ch[3]->radio_out,
+            rc_ch[4]->radio_out,
+            rc_ch[5]->radio_out,
+            rc_ch[6]->radio_out,
+            rc_ch[7]->radio_out);
+        return;
+    }
+#endif
     mavlink_msg_servo_output_raw_send(
         chan,
         micros(),
@@ -352,21 +369,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan)
         hal.rcout->read(5),
         hal.rcout->read(6),
         hal.rcout->read(7));
-#else
-    extern RC_Channel* rc_ch[8];
-    mavlink_msg_servo_output_raw_send(
-        chan,
-        micros(),
-        0,     // port
-        rc_ch[0]->radio_out,
-        rc_ch[1]->radio_out,
-        rc_ch[2]->radio_out,
-        rc_ch[3]->radio_out,
-        rc_ch[4]->radio_out,
-        rc_ch[5]->radio_out,
-        rc_ch[6]->radio_out,
-        rc_ch[7]->radio_out);
-#endif
 }
 
 static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
@@ -1103,8 +1105,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_DO_SET_SERVO:
-            hal.rcout->enable_ch(packet.param1 - 1);
-            hal.rcout->write(packet.param1 - 1, packet.param2);
+            servo_write(packet.param1 - 1, packet.param2);
             result = MAV_RESULT_ACCEPTED;
             break;
 
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index ca2dfc6002..cd235f43e4 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -81,6 +81,7 @@ public:
         k_param_takeoff_throttle_min_speed,
         k_param_takeoff_throttle_min_accel,
         k_param_takeoff_heading_hold,
+        k_param_hil_servos,
 
         // 110: Telemetry control
         //
@@ -342,6 +343,9 @@ public:
     AP_Int32 min_gndspeed_cm;
     AP_Int16 pitch_trim_cd;
     AP_Int16 FBWB_min_altitude_cm;
+#if HIL_MODE != HIL_MODE_DISABLED
+    AP_Int8  hil_servos;
+#endif
 
     AP_Int8 compass_enabled;
     AP_Int8 battery_monitoring;                 // 0=disabled, 3=voltage only, 4=voltage and current
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 0e59381534..00c62f8727 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -624,6 +624,15 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     GSCALAR(inverted_flight_ch,     "INVERTEDFLT_CH", 0),
 
+#if HIL_MODE != HIL_MODE_DISABLED
+    // @Param: HIL_SERVOS
+    // @DisplayName: HIL Servos enable
+    // @Description: This controls whether real servo controls are used in HIL mode. If you enable this then the APM will control the real servos in HIL mode. If disabled it will report servo values, but will not output to the real servos. Be careful that your motor and propeller are not connected if you enable this option.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    GSCALAR(hil_servos,            "HIL_SERVOS",      0),
+#endif
+
     // barometer ground calibration. The GND_ prefix is chosen for
     // compatibility with previous releases of ArduPlane
     GOBJECT(barometer, "GND_", AP_Baro),
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index c49ce582ed..c7d246228e 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -596,8 +596,7 @@ static void do_set_home()
 
 static void do_set_servo()
 {
-    hal.rcout->enable_ch(next_nonnav_command.p1 - 1);
-    hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
+    servo_write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
 }
 
 static void do_set_relay()
diff --git a/ArduPlane/config.h b/ArduPlane/config.h
index e37186be4a..75d96e219d 100644
--- a/ArduPlane/config.h
+++ b/ArduPlane/config.h
@@ -794,11 +794,6 @@
  # define SCALING_SPEED          15.0
 #endif
 
-// use this to enable servos in HIL mode
-#ifndef HIL_SERVOS
- # define HIL_SERVOS DISABLED
-#endif
-
 // use this to completely disable the CLI
 #ifndef CLI_ENABLED
  # define CLI_ENABLED ENABLED
diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde
index b9240d3de4..1baca4af70 100644
--- a/ArduPlane/events.pde
+++ b/ArduPlane/events.pde
@@ -109,9 +109,9 @@ static void update_events(void)
         case EVENT_TYPE_SERVO:
             hal.rcout->enable_ch(event_state.rc_channel);
             if (event_state.repeat & 1) {
-                hal.rcout->write(event_state.rc_channel, event_state.undo_value);                 
+                servo_write(event_state.rc_channel, event_state.undo_value);
             } else {
-                hal.rcout->write(event_state.rc_channel, event_state.servo_value);                 
+                servo_write(event_state.rc_channel, event_state.servo_value);                 
             }
             break;
 
diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde
index f60d3946e2..0a3a766da9 100644
--- a/ArduPlane/failsafe.pde
+++ b/ArduPlane/failsafe.pde
@@ -44,7 +44,7 @@ void failsafe_check(uint32_t tnow)
             start_ch = 1;
         }
         for (uint8_t ch=start_ch; ch<4; ch++) {
-            hal.rcout->write(ch, hal.rcin->read(ch));
+            servo_write(ch, hal.rcin->read(ch));
         }
         RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
         RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true);
diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde
index 12688c49be..4701a5e1af 100644
--- a/ArduPlane/radio.pde
+++ b/ArduPlane/radio.pde
@@ -52,20 +52,20 @@ static void init_rc_out()
     enable_aux_servos();
 
     // Initialization of servo outputs
-    hal.rcout->write(CH_1,   g.channel_roll.radio_trim);
-    hal.rcout->write(CH_2,   g.channel_pitch.radio_trim);
-    hal.rcout->write(CH_3,   g.channel_throttle.radio_min);
-    hal.rcout->write(CH_4,   g.channel_rudder.radio_trim);
+    servo_write(CH_1, g.channel_roll.radio_trim);
+    servo_write(CH_2,   g.channel_pitch.radio_trim);
+    servo_write(CH_3,   g.channel_throttle.radio_min);
+    servo_write(CH_4,   g.channel_rudder.radio_trim);
 
-    hal.rcout->write(CH_5,   g.rc_5.radio_trim);
-    hal.rcout->write(CH_6,   g.rc_6.radio_trim);
-    hal.rcout->write(CH_7,   g.rc_7.radio_trim);
-    hal.rcout->write(CH_8,   g.rc_8.radio_trim);
+    servo_write(CH_5,   g.rc_5.radio_trim);
+    servo_write(CH_6,   g.rc_6.radio_trim);
+    servo_write(CH_7,   g.rc_7.radio_trim);
+    servo_write(CH_8,   g.rc_8.radio_trim);
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM2
-    hal.rcout->write(CH_9,   g.rc_9.radio_trim);
-    hal.rcout->write(CH_10,  g.rc_10.radio_trim);
-    hal.rcout->write(CH_11,  g.rc_11.radio_trim);
+    servo_write(CH_9,   g.rc_9.radio_trim);
+    servo_write(CH_10,  g.rc_10.radio_trim);
+    servo_write(CH_11,  g.rc_11.radio_trim);
 #endif
 }
 
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index ed3cbcd255..5760ce5abe 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -641,3 +641,20 @@ static void print_comma(void)
 }
 
 
+/*
+  write to a servo
+ */
+static void servo_write(uint8_t ch, uint16_t pwm)
+{
+#if HIL_MODE != HIL_MODE_DISABLED
+    if (!g.hil_servos) {
+        extern RC_Channel *rc_ch[8];
+        if (ch < 8) {
+            rc_ch[ch]->radio_out = pwm;
+        }
+        return;
+    }
+#endif
+    hal.rcout->enable_ch(ch);
+    hal.rcout->write(ch, pwm);
+}
diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde
index c2a7fa76fa..f816bb7809 100644
--- a/ArduPlane/test.pde
+++ b/ArduPlane/test.pde
@@ -145,7 +145,7 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
             for(int16_t i = 0; i < 8; i++) {
                 cliSerial->print(hal.rcin->read(i));        // Print channel values
                 print_comma();
-                hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos
+                servo_write(i, hal.rcin->read(i)); // Copy input to Servos
             }
             cliSerial->println();
         }