Plane: added HIL_SERVOS option

this allows for real servo output in HIL

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--;

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;
 

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

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),

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()

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

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;
 

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);

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
 }
 

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);
+}

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();
         }