Plane: tailsitter rate limit second half of VTOL transition.

ArduPlane/mode_qstabilize.cpp

@@ -64,6 +64,8 @@ void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const flo
 
     // angle max for tailsitter pitch
     plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
+
+    plane.quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd);
 }
 
 // set the desired roll and pitch for normal quadplanes, also limited by forward flight limtis

ArduPlane/quadplane.cpp

@@ -884,8 +884,15 @@ void QuadPlane::hold_stabilize(float throttle_in)
 // run the multicopter Z controller
 void QuadPlane::run_z_controller(void)
 {
+    if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED ) {
+        return;
+    }
     const uint32_t now = AP_HAL::millis();
-    if (!pos_control->is_active_z()) {
+    if (tailsitter.in_vtol_transition(now)) {
+        // never run Z controller in tailsitter transtion
+        return;
+    }
+    if ((now - last_pidz_active_ms) > 20) {
         // set vertical speed and acceleration limits
         pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
 
@@ -906,7 +913,7 @@ void QuadPlane::relax_attitude_control()
 {
     // disable roll and yaw control for vectored tailsitters
     // if not a vectored tailsitter completely disable attitude control
-    attitude_control->relax_attitude_controllers(tailsitter._is_vectored);
+    attitude_control->relax_attitude_controllers(!tailsitter.relax_pitch());
 }
 
 /*
@@ -1864,7 +1871,8 @@ void QuadPlane::motors_output(bool run_rate_controller)
         return;
     }
 
-    if (tailsitter.in_vtol_transition() && !assisted_flight) {
+    const uint32_t now = AP_HAL::millis();
+    if (tailsitter.in_vtol_transition(now) && !assisted_flight) {
         /*
           don't run the motor outputs while in tailsitter->vtol
           transition. That is taken care of by the fixed wing
@@ -1873,7 +1881,6 @@ void QuadPlane::motors_output(bool run_rate_controller)
         return;
     }
 
-    const uint32_t now = AP_HAL::millis();
     if (run_rate_controller) {
         if (now - last_att_control_ms > 100) {
             // relax if have been inactive
@@ -2319,7 +2326,7 @@ void QuadPlane::vtol_position_controller(void)
     case QPOS_POSITION1: {
         setup_target_position();
 
-        if (tailsitter.enabled() && tailsitter.in_vtol_transition()) {
+        if (tailsitter.enabled() && tailsitter.in_vtol_transition(now_ms)) {
             break;
         }
 
@@ -2357,17 +2364,23 @@ void QuadPlane::vtol_position_controller(void)
         // run fixed wing navigation
         plane.nav_controller->update_waypoint(plane.current_loc, loc);
 
-        Vector2f target_speed_xy;
+        Vector2f target_speed_xy_cms;
         if (distance > 0.1) {
-            target_speed_xy = diff_wp.normalized() * target_speed;
+            target_speed_xy_cms = diff_wp.normalized() * target_speed * 100;
+        }
+        if (!tailsitter.enabled()) {
+            // this method ignores pos-control velocity/accel limtis
+            pos_control->set_vel_desired_xy_cms(target_speed_xy_cms);
+
+            // reset position controller xy target to current position
+            // because we only want velocity control (no position control)
+            const Vector2f& curr_pos = inertial_nav.get_position_xy_cm();
+            pos_control->set_pos_target_xy_cm(curr_pos.x, curr_pos.y);
+            pos_control->set_accel_desired_xy_cmss(Vector2f());
+        } else {
+            // tailsitters use input shaping and abide by velocity limits
+            pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f());
         }
-        pos_control->set_vel_desired_xy_cms(target_speed_xy * 100);
-
-        // reset position controller xy target to current position
-        // because we only want velocity control (no position control)
-        const Vector2f& curr_pos = inertial_nav.get_position_xy_cm();
-        pos_control->set_pos_target_xy_cm(curr_pos.x, curr_pos.y);
-        pos_control->set_accel_desired_xy_cmss(Vector2f());
 
         // run horizontal velocity controller
         run_xy_controller();
@@ -2488,7 +2501,7 @@ void QuadPlane::vtol_position_controller(void)
         }
         break;
     case QPOS_POSITION1:
-        if (tailsitter.in_vtol_transition()) {
+        if (tailsitter.in_vtol_transition(now_ms)) {
             pos_control->relax_z_controller(0);
             break;
         }

ArduPlane/tailsitter.cpp

@@ -685,6 +685,14 @@ void Tailsitter::speed_scaling(void)
     }
 }
 
+// return true if pitch control should be relaxed
+// on vectored belly sitters the pitch control is not relaxed in order to keep motors pointing and avoid risk of props hitting the ground
+// always relax after a transition
+bool Tailsitter::relax_pitch()
+{
+    return !enabled() || !_is_vectored || (transition->vtol_limit_start_ms != 0);
+}
+
 /*
   update for transition from quadplane to fixed wing mode
  */
@@ -757,6 +765,11 @@ void Tailsitter_Transition::VTOL_update()
         if (!quadplane.tailsitter.transition_vtol_complete()) {
             return;
         }
+        // transition to VTOL complete, if armed set vtol rate limit starting point
+        if (hal.util->get_soft_armed()) {
+            vtol_limit_start_ms = now;
+            vtol_limit_initial_pitch = quadplane.ahrs_view->pitch_sensor;
+        }
     }
     restart();
 }
@@ -797,6 +810,38 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na
     }
 }
 
+bool Tailsitter_Transition::set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd)
+{
+    if (vtol_limit_start_ms == 0) {
+        return false;
+    }
+    // prevent pitching towards 0 too quickly
+    const float pitch_change_cd = (AP_HAL::millis() - vtol_limit_start_ms) * tailsitter.transition_rate_vtol * 0.1;
+    if (pitch_change_cd > fabsf(vtol_limit_initial_pitch)) {
+        // limit has passed 0, nothing to do
+        vtol_limit_start_ms = 0;
+        return false;
+    }
+    // continue limiting while limit angle is larger than desired angle
+    if (is_negative(vtol_limit_initial_pitch)) {
+        const float pitch_limit = vtol_limit_initial_pitch + pitch_change_cd;
+        if (nav_pitch_cd > pitch_limit) {
+            nav_pitch_cd = pitch_limit;
+            nav_roll_cd = 0;
+            return true;
+        }
+    } else {
+        const float pitch_limit = vtol_limit_initial_pitch - pitch_change_cd;
+        if (nav_pitch_cd < pitch_limit) {
+            nav_pitch_cd = pitch_limit;
+            nav_roll_cd = 0;
+            return true;
+        }
+    }
+    vtol_limit_start_ms = 0;
+    return false;
+}
+
 // setup for the transition back to fixed wing
 void Tailsitter_Transition::restart()
 {

ArduPlane/tailsitter.h

@@ -62,9 +62,8 @@ friend class Plane;
     // return the transition_angle_vtol value
     int8_t get_transition_angle_vtol() const;
 
-
-    // true when flying a tilt-vectored tailsitter
-    bool _is_vectored;
+    // return true if pitch control should be relaxed
+    bool relax_pitch();
 
     // tailsitter speed scaler
     float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option
@@ -111,6 +110,9 @@ friend class Plane;
 
     bool setup_complete;
 
+    // true when flying a tilt-vectored tailsitter
+    bool _is_vectored;
+
     // refences for convenience
     QuadPlane& quadplane;
     AP_MotorsMulticopter*& motors;
@@ -150,6 +152,8 @@ friend class Tailsitter;
 
     MAV_VTOL_STATE get_mav_vtol_state() const override;
 
+    bool set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) override;
+
 private:
 
     enum {
@@ -162,6 +166,10 @@ friend class Tailsitter;
     uint32_t vtol_transition_start_ms;
     float vtol_transition_initial_pitch;
 
+    // for rate limit of VTOL flight
+    uint32_t vtol_limit_start_ms;
+    float vtol_limit_initial_pitch;
+
     // for transition to FW flight
     uint32_t fw_transition_start_ms;
     float fw_transition_initial_pitch;