Plane: added Q_TILT_TYPE for retract servo tiltrotors

ArduPlane/quadplane.cpp

@@ -328,6 +328,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist_angle, 30),
 
+    // @Param: TILT_TYPE
+    // @DisplayName: Tiltrotor type
+    // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE
+    // @Values: 0:Continuous,1:Binary
+    AP_GROUPINFO("TILT_TYPE", 46, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS),
+    
     AP_GROUPEND
 };
 

ArduPlane/quadplane.h

@@ -300,11 +300,15 @@ private:
     // time of last control log message
     uint32_t last_ctrl_log_ms;
 
+    // types of tilt mechanisms
+    enum {TILT_TYPE_CONTINUOUS=0, TILT_TYPE_BINARY=1};
+    
     // tiltrotor control variables
     struct {
         AP_Int16 tilt_mask;
         AP_Int16 max_rate_dps;
         AP_Int8  max_angle_deg;
+        AP_Int8  tilt_type;
         float current_tilt;
         float current_throttle;
         bool motors_active:1;
@@ -314,7 +318,10 @@ private:
     uint32_t last_motors_active_ms;
     
     void tiltrotor_slew(float tilt);
+    void tiltrotor_binary_slew(bool forward);
     void tiltrotor_update(void);
+    void tiltrotor_continuous_update(void);
+    void tiltrotor_binary_update(void);
     void tilt_compensate(float *thrust, uint8_t num_motors);
 
     void afs_terminate(void);

ArduPlane/tiltrotor.cpp

@@ -22,15 +22,10 @@ void QuadPlane::tiltrotor_slew(float newtilt)
 }
 
 /*
-  update motor tilt
+  update motor tilt for continuous tilt servos
  */
-void QuadPlane::tiltrotor_update(void)
+void QuadPlane::tiltrotor_continuous_update(void)
 {
-    if (tilt.tilt_mask <= 0) {
-        // no motors to tilt
-        return;
-    }
-
     // default to inactive
     tilt.motors_active = false;
 
@@ -102,6 +97,65 @@ void QuadPlane::tiltrotor_update(void)
 }
 
 
+/*
+  output a slew limited tiltrotor angle. tilt is 0 or 1
+ */
+void QuadPlane::tiltrotor_binary_slew(bool forward)
+{
+    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_motor_tilt, forward?1000:0);
+
+    float max_change = (tilt.max_rate_dps.get() * plane.G_Dt) / 90.0f;
+    if (forward) {
+        tilt.current_tilt = constrain_float(tilt.current_tilt+max_change, 0, 1);
+    } else {
+        tilt.current_tilt = constrain_float(tilt.current_tilt-max_change, 0, 1);
+    }
+
+    // setup tilt compensation
+    motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&QuadPlane::tilt_compensate, void, float *, uint8_t));
+}
+
+/*
+  update motor tilt for binary tilt servos
+ */
+void QuadPlane::tiltrotor_binary_update(void)
+{
+    // motors always active
+    tilt.motors_active = true;
+
+    if (!in_vtol_mode()) {
+        // we are in pure fixed wing mode. Move the tiltable motors
+        // all the way forward and run them as a forward motor
+        tiltrotor_binary_slew(true);
+
+        float new_throttle = plane.channel_throttle->get_servo_out()*0.01f;
+        if (tilt.current_tilt >= 1) {
+            // the motors are all the way forward, start using them for fwd thrust
+            motors->output_motor_mask(new_throttle, (uint8_t)tilt.tilt_mask.get());
+        }
+    } else {
+        tiltrotor_binary_slew(false);
+    }
+}
+
+
+/*
+  update motor tilt
+ */
+void QuadPlane::tiltrotor_update(void)
+{
+    if (tilt.tilt_mask <= 0) {
+        // no motors to tilt
+        return;
+    }
+    if (tilt.tilt_type == TILT_TYPE_BINARY) {
+        tiltrotor_binary_update();
+    } else {
+        tiltrotor_continuous_update();
+    }
+}
+
+
 /*
   compensate for tilt in a set of motor outputs