AP_Scripting: added throttle and height controller to aerobatic example

changed rolling circle to take the radius and number of
circles. negative radius for negative yaw rate and negative number of
circles for left roll

libraries/AP_Scripting/examples/plane_aerobatics.lua

@@ -6,24 +6,32 @@ local roll_stage = 0
 
 local ROLL_TCONST = param:get('RLL2SRV_TCONST') * 0.5
 local PITCH_TCONST = param:get('PTCH2SRV_TCONST') * 0.5
-local TRIM_THROTTLE = param:get('TRIM_THROTTLE') * 1.0
 
 DO_JUMP = 177
 NAV_WAYPOINT = 16
 
-local scr_user1_param = Parameter()
-local scr_user2_param = Parameter()
-local scr_user3_param = Parameter()
-local scr_user4_param = Parameter()
-assert(scr_user1_param:init('SCR_USER1'), 'could not find SCR_USER1 parameter')
-assert(scr_user2_param:init('SCR_USER2'), 'could not find SCR_USER2 parameter')
-assert(scr_user3_param:init('SCR_USER3'), 'could not find SCR_USER3 parameter')
-assert(scr_user4_param:init('SCR_USER4'), 'could not find SCR_USER4 parameter')
+k_throttle = 70
+
+function bind_param(name)
+   local p = Parameter()
+   assert(p:init(name), string.format('could not find %s parameter', name))
+   return p
+end
+
+local SCR_USER1 = bind_param("SCR_USER1") -- height P gain
+local SCR_USER2 = bind_param("SCR_USER2") -- height I gain
+local SCR_USER3 = bind_param("SCR_USER3") -- throttle FF from pitch
+local SCR_USER4 = bind_param("SCR_USER4") -- height knifeedge addition for pitch
+local SCR_USER5 = bind_param("SCR_USER5") -- speed P gain
+local SCR_USER6 = bind_param("SCR_USER6") -- speed I gain
+local TRIM_THROTTLE = bind_param("TRIM_THROTTLE")
+local TRIM_ARSPD_CM = bind_param("TRIM_ARSPD_CM")
 
 local last_roll_err = 0.0
 local last_id = 0
 local initial_yaw_deg = 0
 local wp_yaw_deg = 0
+local initial_height = 0
 
 -- constrain a value between limits
 function constrain(v, vmin, vmax)
@@ -73,6 +81,106 @@ function wrap_180(angle)
     return res
 end
 
+-- a PI controller implemented as a Lua object
+local function PI_controller(kP,kI,iMax)
+   -- the new instance. You can put public variables inside this self
+   -- declaration if you want to
+   local self = {}
+
+   -- private fields as locals
+   local _kP = kP or 0.0
+   local _kI = kI or 0.0
+   local _kD = kD or 0.0
+   local _iMax = iMax
+   local _last_t = nil
+   local _I = 0
+   local _P = 0
+   local _total = 0
+   local _counter = 0
+   local _target = 0
+   local _current = 0
+
+   -- update the controller.
+   function self.update(target, current)
+      local now = millis():tofloat() * 0.001
+      if not _last_t then
+         _last_t = now
+      end
+      local dt = now - _last_t
+      _last_t = now
+      local err = target - current
+      _counter = _counter + 1
+
+      local P = _kP * err
+      _I = _I + _kI * err * dt
+      if _iMax then
+         _I = constrain(_I, -_iMax, iMax)
+      end
+      local I = _I
+      local ret = P + I
+
+      _target = target
+      _current = current
+      _P = P
+      _total = ret
+      return ret
+   end
+
+   -- reset integrator to an initial value
+   function self.reset(integrator)
+      _I = integrator
+   end
+
+   function self.set_I(I)
+      _kI = I
+   end
+
+   function self.set_P(P)
+      _kP = P
+   end
+   
+   function self.set_Imax(Imax)
+      _iMax = Imax
+   end
+   
+   -- log the controller internals
+   function self.log(name, add_total)
+      -- allow for an external addition to total
+      logger.write(name,'Targ,Curr,P,I,Total,Add','ffffff',_target,_current,_P,_I,_total,add_total)
+   end
+
+   -- return the instance
+   return self
+end
+
+local function height_controller(kP_param,kI_param,KnifeEdge_param,Imax)
+   local self = {}
+   local kP = kP_param
+   local kI = kI_param
+   local KnifeEdge = KnifeEdge_param
+   local PI = PI_controller(kP:get(), kI:get(), Imax)
+
+   function self.update(target)
+      local target_pitch = PI.update(initial_height, ahrs:get_position():alt()*0.01)
+      local roll_rad = ahrs:get_roll()
+      local ke_add = math.abs(math.sin(roll_rad)) * KnifeEdge:get()
+      target_pitch = target_pitch + ke_add
+      PI.log("HPI", ke_add)
+      return target_pitch
+   end
+
+   function self.reset()
+      PI.reset(math.max(math.deg(ahrs:get_pitch()), 3.0))
+      PI.set_P(kP:get())
+      PI.set_I(kI:get())
+   end
+
+   return self
+end
+
+local height_PI = height_controller(SCR_USER1, SCR_USER2, SCR_USER4, 20.0)
+local speed_PI = PI_controller(SCR_USER5:get(), SCR_USER6:get(), 100.0)
+
 -- a controller to target a zero pitch angle and zero heading change, used in a roll
 -- output is a body frame pitch rate, with convergence over time tconst in seconds
 function pitch_controller(target_pitch_deg, target_yaw_deg, tconst)
@@ -92,8 +200,8 @@ end
 -- a controller for throttle to account for pitch
 function throttle_controller(tconst)
    local pitch_rad = ahrs:get_pitch()
-   local thr_ff = scr_user3_param:get()
-   local throttle = TRIM_THROTTLE + math.sin(pitch_rad) * thr_ff
+   local thr_ff = SCR_USER3:get()
+   local throttle = TRIM_THROTTLE:get() + math.sin(pitch_rad) * thr_ff
    return constrain(throttle, 0.0, 100.0)
 end
 
@@ -102,6 +210,7 @@ function do_axial_roll(arg1, arg2)
    if not running then
       running = true
       roll_stage = 0
+      height_PI.reset()
       gcs:send_text(0, string.format("Starting roll"))
    end
    local roll_rate = arg1
@@ -123,7 +232,7 @@ function do_axial_roll(arg1, arg2)
       end
    end
    if roll_stage < 2 then
-      target_pitch = scr_user2_param:get()
+      target_pitch = height_PI.update(initial_height)
       pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST)
       vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate)
    end
@@ -171,34 +280,42 @@ function do_rolling_circle(arg1, arg2)
    if not running then
       running = true
       rolling_circle_stage = 0
-      rolling_circle_yaw = initial_yaw_deg
+      rolling_circle_yaw = 0
       rolling_circle_last_ms = millis()
+      height_PI.reset()
+
+      speed_PI.set_P(SCR_USER5:get())
+      speed_PI.set_I(SCR_USER6:get())
+      speed_PI.reset(math.max(SRV_Channels:get_output_scaled(k_throttle), TRIM_THROTTLE:get()))
       gcs:send_text(0, string.format("Starting rolling circle"))
    end
-   local roll_rate = arg1
-   local throttle = arg2
+   local radius = arg1
+   local num_rolls = arg2
    local pitch_deg = math.deg(ahrs:get_pitch())
    local roll_deg = math.deg(ahrs:get_roll())
    local yaw_deg = math.deg(ahrs:get_yaw())
    local gspeed = ahrs:groundspeed_vector():length()
-   local radius = scr_user4_param:get()
-   if radius < 1 then
-      radius = 50.0
-   end
-   local circumference = math.pi * 2.0 * radius
+   local circumference = math.abs(math.pi * 2.0 * radius)
    local circle_time = circumference / gspeed
    local yaw_rate_dps = 360.0 / circle_time
    local now_ms = millis()
    local dt = (now_ms - rolling_circle_last_ms):tofloat() * 0.001
    rolling_circle_last_ms = now_ms
 
+   if radius < 0.0 then
+      yaw_rate_dps = -yaw_rate_dps
+   end
+
+   local roll_rate = (360.0 * num_rolls) / circle_time
+
    rolling_circle_yaw = rolling_circle_yaw + yaw_rate_dps * dt
+
    if rolling_circle_stage == 0 then
-      if wrap_180(yaw_deg - initial_yaw_deg) > 45 then
+      if math.abs(rolling_circle_yaw) > 10.0 then
          rolling_circle_stage = 1
       end
    elseif rolling_circle_stage == 1 then
-      if math.abs(wrap_180(yaw_deg - initial_yaw_deg)) < 5 then
+      if math.abs(rolling_circle_yaw) >= 360.0 then
          running = false
          -- we're done
          gcs:send_text(0, string.format("Finished rollcircle r=%.1f p=%.1f", roll_deg, pitch_deg))
@@ -207,10 +324,20 @@ function do_rolling_circle(arg1, arg2)
          return
       end
    end
+
+   local target_roll = num_rolls * math.abs(rolling_circle_yaw)
+   local roll_error = wrap_180(target_roll - roll_deg)
+   local roll_error_P = 0.5
+   local roll_rate_corrected = roll_rate + roll_error * roll_error_P
+
    if rolling_circle_stage < 2 then
-      target_pitch = scr_user2_param:get()
-      pitch_rate, yaw_rate = pitch_controller(target_pitch, rolling_circle_yaw, PITCH_TCONST)
-      vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate)
+      target_pitch = height_PI.update(initial_height)
+      vel = ahrs:get_velocity_NED()
+      throttle = speed_PI.update(TRIM_ARSPD_CM:get()*0.01, vel:length())
+      throttle = constrain(throttle, 0, 100.0)
+      speed_PI.log("SPI", 0.0)
+      pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(rolling_circle_yaw+initial_yaw_deg), PITCH_TCONST)
+      vehicle:set_target_throttle_rate_rpy(throttle, roll_rate_corrected, pitch_rate, yaw_rate)
    end
 end
 
@@ -244,6 +371,7 @@ function update()
          running = false
          last_id = id
          initial_yaw_deg = math.deg(ahrs:get_yaw())
+         initial_height = ahrs:get_position():alt()*0.01
 
          -- work out yaw between previous WP and next WP
          local cnum = mission:get_current_nav_index()