diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
index a782261a4d..84a64556f9 100644
--- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
+++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
@@ -17,6 +17,7 @@
 #include <AP_Motors/AP_Motors.h>
 #include <RC_Channel/RC_Channel.h>
 #include <SRV_Channel/SRV_Channel.h>
+#include <AP_ESC_Telem/AP_ESC_Telem.h>
 #include <stdio.h>
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
@@ -31,12 +32,15 @@ void update_motors();
 #define HELI_TEST       0   // set to 1 to test helicopters
 #define NUM_OUTPUTS     4   // set to 6 for hexacopter, 8 for octacopter and heli
 
+#if HAL_WITH_ESC_TELEM
+AP_ESC_Telem esc_telem;
+#endif
 SRV_Channels srvs;
 
 // uncomment the row below depending upon what frame you are using
 //AP_MotorsTri  motors(400);
 AP_MotorsMatrix   motors(400);
-//AP_MotorsHeli_Single motors(rc7, rsc, h1, h2, h3, h4, 400);
+//AP_MotorsHeli_Single motors(400);
 //AP_MotorsSingle motors(400);
 //AP_MotorsCoax motors(400);
 
@@ -53,6 +57,9 @@ void setup()
     motors.set_dt(1.0/400.0);
     motors.set_update_rate(490);
 
+#if HELI_TEST
+    motors.init(AP_Motors::MOTOR_FRAME_HELI, AP_Motors::MOTOR_FRAME_TYPE_PLUS);
+#else
 #if NUM_OUTPUTS == 8
     motors.init(AP_Motors::MOTOR_FRAME_OCTA, AP_Motors::MOTOR_FRAME_TYPE_X);
 #elif NUM_OUTPUTS == 6
@@ -60,6 +67,7 @@ void setup()
 #else
     motors.init(AP_Motors::MOTOR_FRAME_QUAD, AP_Motors::MOTOR_FRAME_TYPE_X);
 #endif
+#endif // HELI_TEST == 0
 
     char frame_and_type_string[30];
     motors.get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
@@ -76,6 +84,7 @@ void setup()
     char * const *argv;
     hal.util->commandline_arguments(argc, argv);
     if (argc > 1) {
+#if HELI_TEST == 0
         for (uint8_t i = 2; i < argc; i++) {
             const char* arg = argv[i];
 
@@ -102,6 +111,7 @@ void setup()
             }
 
         }
+#endif
         if (strcmp(argv[1],"t") == 0) {
             motor_order_test();
 
@@ -165,9 +175,11 @@ void motor_order_test()
 // stability_test
 void stability_test()
 {
+#if HELI_TEST == 0
     hal.console->printf("Throttle average max: %0.4f\n",  motors.get_throttle_avg_max());
     hal.console->printf("Yaw headroom: %i\n", motors.get_yaw_headroom());
     hal.console->printf("Thrust boost: %s\n", thrust_boost?"True":"False");
+#endif
 
     const float throttle_tests[] = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0};
     const uint8_t throttle_tests_num = ARRAY_SIZE(throttle_tests);
@@ -183,9 +195,11 @@ void stability_test()
     for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
         hal.console->printf("Mot%i,",i+1);
     }
+#if HELI_TEST == 0
     for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
         hal.console->printf("Mot%i_norm,",i+1);
     }
+#endif
     hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n");
 
     // run stability test
@@ -203,14 +217,17 @@ void stability_test()
                     motors.set_throttle(throttle_in);
                     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
                     update_motors();
+                    SRV_Channels::calc_pwm();
                     // display input and output
                     hal.console->printf("%0.2f,%0.2f,%0.2f,%0.2f,", roll_in, pitch_in, yaw_in, throttle_in);
                     for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
                         hal.console->printf("%d,",(int)hal.rcout->read(i));
                     }
+#if HELI_TEST == 0
                     for (uint8_t i=0; i<NUM_OUTPUTS; i++) {
                         hal.console->printf("%0.4f,", motors.get_thrust_rpyt_out(i));
                     }
+#endif
                     hal.console->printf("%d,%d,%d,%d,%d\n",
                             (int)motors.limit.roll,
                             (int)motors.limit.pitch,