Plane: implement tilt bias for QHOVER, QSTABILIZE and QACRO modes

add @RebootRequired to metadata for param Q_TILT_BIAS_CH scale manual tilt bias to Q_TILT_MAX
ArduPilot · Jun 1, 2019 · 39df0e9 · 39df0e9
1 parent 7e2f807
commit 39df0e9
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 10 deletions.
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
@@ -448,6 +448,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @DisplayName: Tilt bias channel
     // @Description: RC channel for manual forward bias of motor tilt.
     // @Range 0 16
+    // @RebootRequired: True
     AP_GROUPINFO("TILT_BIAS_CH", 14, QuadPlane, tilt.bias_chan, 0),
 
     AP_GROUPEND
@@ -705,6 +706,10 @@ bool QuadPlane::setup(void)
             SRV_Channels::set_range(SRV_Channel::k_tiltMotorLeft,  1000);
             SRV_Channels::set_range(SRV_Channel::k_tiltMotorRight, 1000);
         }
+
+        if (tilt.bias_chan > 0) {
+            tilt.rc_bias_ch = RC_Channels::rc_channel(tilt.bias_chan-1);
+        }
     }
 
 

diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
@@ -431,6 +431,7 @@ class QuadPlane
         float current_tilt;
         float current_throttle;
         bool motors_active:1;
+        RC_Channel *rc_bias_ch;
     } tilt;
 
     enum tailsitter_input {

diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp
@@ -90,28 +90,46 @@ void QuadPlane::tiltrotor_continuous_update(void)
     tilt.current_throttle = constrain_float(motors_throttle,
                                             tilt.current_throttle-max_change,
                                             tilt.current_throttle+max_change);
-    
+
     /*
       we are in a VTOL mode. We need to work out how much tilt is
-      needed. There are 3 strategies we will use:
+      needed. There are 4 strategies we will use:
 
-      1) in QSTABILIZE or QHOVER the angle will be set to zero. This
-         enables these modes to be used as a safe recovery mode.
+      1a) In QAUTOTUNE mode the angle will be set to zero.
+      1b) In QHOVER, QSTABILIZE and QACRO modes with rc_bias_ch null, the angle will be set to zero.
+            This enables these modes to be used as a safe recovery mode.
 
-      2) in fixed wing assisted flight or velocity controlled modes we
+      2) In QHOVER, QSTABILIZE and QACRO modes with rc_bias_ch assigned to a valid RC channel:
+           The tilt angle is determined by RC input.
+
+      3) In fixed wing assisted flight or velocity controlled modes we
          will set the angle based on the demanded forward throttle,
          with a maximum tilt given by Q_TILT_MAX. This relies on
-         Q_VFWD_GAIN being set
+         Q_VFWD_GAIN being set.
 
-      3) if we are in TRANSITION_TIMER mode then we are transitioning
+      4) if we are in TRANSITION_TIMER mode then we are transitioning
          to forward flight and should put the rotors all the way forward
     */
-    if (plane.control_mode == &plane.mode_qstabilize ||
-        plane.control_mode == &plane.mode_qhover ||
-        plane.control_mode == &plane.mode_qautotune) {
+
+    if (plane.control_mode == &plane.mode_qautotune) {
+        // case 1a
         tiltrotor_slew(0);
         return;
     }
+    if (plane.control_mode == &plane.mode_qhover ||
+        plane.control_mode == &plane.mode_qstabilize ||
+        plane.control_mode == &plane.mode_qacro) {
+        if (tilt.rc_bias_ch == nullptr) {
+            // case 1b
+            tiltrotor_slew(0);
+        } else {
+            // case 2) manual control of motor tilt
+            float settilt = constrain_float((1.0f + tilt.rc_bias_ch->norm_input()) / 2, 0, 1);
+            settilt *= tilt.max_angle_deg / 90.0f;
+            tiltrotor_slew(settilt);
+        }
+        return;
+    }
 
     if (assisted_flight &&
         transition_state >= TRANSITION_TIMER) {
@@ -138,6 +156,7 @@ void QuadPlane::tiltrotor_binary_slew(bool forward)
     SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, forward?1000:0);
 
     // rate limiting current_tilt has the effect of delaying throttle in tiltrotor_binary_update
+    // The rate used here is (tilt.max_rate_down_dps > 0) ? tilt.max_rate_down_dps, tilt.max_rate_up_dps
     float max_change = tilt_max_change(!forward);
     if (forward) {
         tilt.current_tilt = constrain_float(tilt.current_tilt+max_change, 0, 1);