Add stall backflip maneuver and elevator override mechanism

ArduPlane/Attitude.cpp

@@ -406,8 +406,14 @@ void Plane::stabilize()
         // scripting is in control of roll and pitch rates and throttle
         const float speed_scaler = get_speed_scaler();
         const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler);
-        const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
+        float elevator = 0;
+        elevator = nav_scripting.elevator_offset_pct * 45;
+        if (nav_scripting.run_pitch_rate_controller) {
+            elevator += pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
+        } else {
+            pitchController.reset_I();
+        }
         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
         float rudder = 0;
         if (yawController.rate_control_enabled()) {

ArduPlane/Plane.h

@@ -561,6 +561,8 @@ class Plane : public AP_Vehicle {
         uint32_t current_ms;
         float rudder_offset_pct;
         bool run_yaw_rate_controller;
+        float elevator_offset_pct;
+        bool run_pitch_rate_controller;
     } nav_scripting;
 #endif
 
@@ -1212,6 +1214,7 @@ class Plane : public AP_Vehicle {
     // rates. For use with scripting controllers
     void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override;
     void set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller) override;
+    void set_elevator_offset(float elelvator_pct, bool run_pitch_rate_controller) override;
     bool nav_scripting_enable(uint8_t mode) override;
 #endif
 

ArduPlane/commands_logic.cpp

@@ -1215,6 +1215,8 @@ bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd)
             nav_scripting.enabled = false;
             nav_scripting.rudder_offset_pct = 0;
             nav_scripting.run_yaw_rate_controller = true;
+            nav_scripting.elevator_offset_pct = 0;
+            nav_scripting.run_pitch_rate_controller = true;
         }
     }
     return !nav_scripting.enabled;
@@ -1229,6 +1231,8 @@ bool Plane::nav_scripting_active(void)
         nav_scripting.current_ms = 0;
         nav_scripting.rudder_offset_pct = 0;
         nav_scripting.run_yaw_rate_controller = true;
+        nav_scripting.elevator_offset_pct = 0;
+        nav_scripting.run_pitch_rate_controller = true;
         gcs().send_text(MAV_SEVERITY_INFO, "NavScript time out");
     }
     if (control_mode == &mode_auto &&
@@ -1280,6 +1284,13 @@ void Plane::set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller)
     nav_scripting.run_yaw_rate_controller = run_yaw_rate_controller;
 }
 
+// support for elevator offset override in aerobatic scripting
+void Plane::set_elevator_offset(float elevator_pct, bool run_pitch_rate_controller)
+{
+    nav_scripting.elevator_offset_pct = elevator_pct;
+    nav_scripting.run_pitch_rate_controller = run_pitch_rate_controller;
+}
+
 // enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands
 bool Plane::nav_scripting_enable(uint8_t mode)
 {

libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua

@@ -452,6 +452,18 @@ local function get_quat_dcm_c_y(q)
    return 2*(q3q4 + q1q2)
 end
 
+--[[ get the c.z element of a quaternion, which gives
+   the projection of the vehicle z axis in the up direction
+   This is equal to cos(pitch) when the pitch is close to vertical
+--]]
+local function get_quat_dcm_c_z_down(q)
+   local q1 = q:q1()
+   local q2 = q:q2()
+   local q3 = q:q3()
+   local q4 = q:q4()
+   return q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4
+end
+
 --[[ get the c.y element of the DCM body to earth matrix, which gives
    the projection of the vehicle y axis in the down direction
    This is equal to sin(roll)*cos(pitch)
@@ -460,6 +472,14 @@ local function get_ahrs_dcm_c_y()
    return get_quat_dcm_c_y(ahrs:get_quaternion())
 end
 
+--[[ get the c.z element of the DCM body to earth matrix, which gives
+   the projection of the vehicle z axis in the up direction
+   This is equal to cos(pitch) when the pitch is close to vertical
+--]]
+local function get_ahrs_dcm_c_z_down()
+   return get_quat_dcm_c_z_down(ahrs:get_quaternion())
+end
+
 -- a PI controller implemented as a Lua object
 local function PI_controller(kP,kI,iMax,min,max)
    -- the new instance. You can put public variables inside this self
@@ -1803,6 +1823,154 @@ function rudder_over(_direction, _min_speed)
    return self
 end
 
+--[[
+   perform a pitch over maneuver
+--]]
+function pitch_over(_direction, _min_speed)
+   local self = {}
+   local direction = _direction
+   local min_speed = _min_speed
+   local reached_speed = false
+   local kick_started = false
+   local pitch2_done = false
+   local descent_done = false
+   local target_q = nil
+   local initial_q = nil
+   local last_t = nil
+   local initial_z = nil
+   local desired_direction = nil
+
+   --[[
+      the update() method is called during the pitch over, it
+      should return true when the maneuver is completed
+   --]]
+   function self.update(path, t, target_speed)
+      if descent_done then
+         -- we're all done
+         return true
+      end
+
+      local ahrs_quat = ahrs:get_quaternion()
+      local ahrs_pos = ahrs:get_relative_position_NED_origin()
+      local ahrs_gyro = ahrs:get_gyro()
+      local now = millis():tofloat() * 0.001
+
+      if target_q == nil then
+         -- initialising
+         initial_z = ahrs_pos:z()
+         target_q = quat_copy(ahrs_quat)
+         initial_q = quat_copy(target_q)
+         last_t = now
+      end
+
+      local dt = now - last_t
+      last_t = now
+
+      local error_quat = ahrs_quat:inverse() * target_q
+      local rate_rads = Vector3f()
+      error_quat:to_axis_angle(rate_rads)
+      local tc = ROLL_CORR_TC:get()
+      local rate_dps = rate_rads:scale(math.deg(1)/tc)
+
+      -- use user set throttle for achieving the stall
+      local throttle = AEROM_STALL_THR:get()
+      local pitch_deg = math.deg(ahrs:get_pitch())
+      if reached_speed and not kick_started and math.abs(math.deg(ahrs_gyro:y())) > ACRO_PITCH_RATE:get()/3 then
+         kick_started = true
+      end
+      if kick_started then
+         -- when we have established some pitch rate cut the throttle to minimum
+         throttle = AEROM_THR_MIN:get()
+      end
+
+      vehicle:set_target_throttle_rate_rpy(throttle, rate_dps:x(), rate_dps:y(), rate_dps:z())
+
+      log_pose('POSM', ahrs_pos, ahrs:get_quaternion())
+      log_pose('POST', ahrs_pos, target_q)
+
+      local current_speed_up = -ahrs:get_velocity_NED():z()
+      if not reached_speed and current_speed_up <= min_speed then
+         reached_speed = true
+      end
+
+      if not reached_speed then
+         return false
+      end
+
+      -- integrate desired attitude through pitch
+      local q_rate_rads = makeVector3f(0,ahrs_gyro:y(),0)
+      if pitch2_done then
+         -- stop adding pitch
+         q_rate_rads:y(0)
+      end
+      local rotation = Quaternion()
+      rotation:from_angular_velocity(q_rate_rads, dt)
+      target_q = target_q * rotation
+      target_q:normalize()
+
+      --[[
+         override elevator to maximum, basing PWM on the MIN/MAX of the channel
+         according to the desired direction
+      --]]
+      if desired_direction == nil then
+         desired_direction = direction
+         if desired_direction == 0 then
+            local c_z = get_ahrs_dcm_c_z()
+            if c_z > 0 then
+               desired_direction = -1
+            else
+               desired_direction = 1
+            end
+         end
+      end
+      if not pitch2_done then
+         vehicle:set_elevator_offset(desired_direction * 100, false)
+      else
+         vehicle:set_elevator_offset(0, true)
+      end
+      if not kick_started then
+         return false
+      end
+
+      -- see if we are nose down
+      if kick_started and pitch_deg < AEROM_STALL_PIT:get() and not pitch2_done then
+         -- lock onto a descent path
+         pitch2_done = true
+         target_q = initial_q * qorient(0, 180, 0)
+         --[[ correct the attitude to the opposite correction that we
+            had at the start of the slowdown, so we fight the wind on
+            the way down
+         --]]
+         local error_q = initial_q:inverse() * qorient(0, 90, math.deg(initial_q:get_euler_yaw()))
+         local error_pitch = error_q:get_euler_pitch()
+         local error_yaw = error_q:get_euler_yaw()
+         target_q = target_q * qorient(0, math.deg(-2*error_pitch), math.deg(2*error_yaw))
+         target_q:normalize()
+         return false
+      end
+
+      if not pitch2_done or ahrs_pos:z() < initial_z then
+         -- haven't finished the descent
+         return false
+      end
+
+      -- all done, update state
+      descent_done = true
+      path_var.tangent = path_var.tangent:scale(-1)
+      path_var.path_t = path:get_next_segment_start(t)
+      path_var.accumulated_orientation_rel_ef = path_var.accumulated_orientation_rel_ef * qorient(0,180,0)
+      path_var.last_time = now
+      path_var.last_ang_rate_dps = ahrs_gyro:scale(math.deg(1))
+      path_var.pos = rotate_path(path, path_var.path_t, path_var.initial_ori, path_var.initial_ef_pos)
+      -- ensure that the path will move fwd on the next step
+      path_var.pos:z(path_var.pos:z()-10)
+
+      return false
+   end
+
+   return self
+end
+
 --[[
    stall turn is not really correct, as we don't fully stall. Needs to be
    reworked
@@ -2010,6 +2178,26 @@ function procedure_turn(radius, bank_angle, step_out, arg4)
       })
 end
 
+--[[
+   A maneuver flown by F-22 demonstration team consisting of:
+     - A pull to a vertical climb
+     - Deceleration until a stall-turn like condition
+     - A tight backwards pitch to a nose-down orientation
+     - A 180-degree roll while descending
+     - Return to level flight in the reverse of the entry direction
+--]]
+function stall_backflip(radius, height, direction, min_speed)
+   local h = height - radius
+   assert(h >= 0)
+   return make_paths("stall_backflip", {
+         { path_vertical_arc(radius, 90),          roll_angle(0) },
+         { path_straight(h),                       roll_angle(0) },
+         { path_reverse(h/4),                      roll_angle(0), rate_override=pitch_over(direction,min_speed), set_orient=qorient(0,-90,0) },
+         { path_straight(h),                       roll_angle(180), pos_corr=0.5, shift_xy=true },
+         { path_vertical_arc(-radius, 90),         roll_angle(0), set_orient=qorient(0,0,180) },
+   })
+end
+
 ---------------------------------------------------
 
 --[[
@@ -2878,6 +3066,7 @@ command_table[27]= PathFunction(straight_flight, "Straight Hold")
 command_table[28]= PathFunction(partial_circle, "Partial Circle")
 command_table[31]= PathFunction(multi_point_roll, "Multi Point Roll")
 command_table[32]= PathFunction(side_step, "Side Step")
+command_table[33]= PathFunction(stall_turn, "Stall Backflip")
 
 --[[
    a table of function available in loadable tricks
@@ -2916,6 +3105,7 @@ load_table["multi_point_roll"] = multi_point_roll
 load_table["side_step"] = side_step
 load_table["align_box"] = align_box
 load_table["align_center"] = align_center
+load_table["stall_backflip"] = stall_backflip
 
 --[[
    interpret an attribute value, coping with special cases

libraries/AP_Scripting/docs/docs.lua

@@ -2599,6 +2599,11 @@ function vehicle:set_land_descent_rate(rate) end
 ---@param run_yaw_rate_control boolean
 function vehicle:set_rudder_offset(rudder_pct, run_yaw_rate_control) end
 
+-- desc
+---@param elevator_pct number
+---@param run_pitch_rate_control boolean
+function vehicle:set_elevator_offset(elevator_pct, run_pitch_rate_control) end
+
 -- desc
 ---@return boolean
 function vehicle:has_ekf_failsafed() end

libraries/AP_Scripting/generator/description/bindings.desc

@@ -342,6 +342,7 @@ singleton AP_Vehicle method nav_script_time boolean uint16_t'Null uint8_t'Null f
 singleton AP_Vehicle method nav_script_time_done void uint16_t'skip_check
 singleton AP_Vehicle method set_target_throttle_rate_rpy void float -100 100 float'skip_check float'skip_check float'skip_check
 singleton AP_Vehicle method set_rudder_offset void float'skip_check boolean
+singleton AP_Vehicle method set_elevator_offset void float'skip_check boolean
 singleton AP_Vehicle method set_desired_turn_rate_and_speed boolean float'skip_check float'skip_check
 singleton AP_Vehicle method set_desired_speed boolean float'skip_check
 singleton AP_Vehicle method nav_scripting_enable boolean uint8_t'skip_check

libraries/AP_Vehicle/AP_Vehicle.h

@@ -192,6 +192,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     // rates. For use with scripting controllers
     virtual void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) {}
     virtual void set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller) {}
+    virtual void set_elevator_offset(float elevator_pct, bool run_pitch_rate_controller) {}
     virtual bool nav_scripting_enable(uint8_t mode) {return false;}
 
     // get target location (for use by scripting)