ardupilot/libraries/AP_Scripting/applets/Heli_idle_control.lua

-- idle_control.lua: a closed loop control throttle control while on ground idle (trad-heli)

local PARAM_TABLE_KEY = 73
local PARAM_TABLE_PREFIX = 'IDLE_'
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table')

function bind_add_param(name, idx, default_value)
    assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
    return Parameter(PARAM_TABLE_PREFIX .. name)
end

-- parameters for idle control

IDLE_GAIN_I = bind_add_param('GAIN_I', 1, 0.05)
IDLE_GAIN_P = bind_add_param('GAIN_P', 2, 0.25)
IDLE_GAIN_MAX = bind_add_param('GAIN_MAX', 3, 1)
IDLE_MAX = bind_add_param('MAX', 4, 17)
IDLE_RANGE = bind_add_param('RANGE', 5, 300)
IDLE_SETPOINT = bind_add_param('SETPOINT', 6, 600)
IDLE_RPM_ENABLE = bind_add_param('RPM_ENABLE', 7, 0)

-- internal variables

local thr_out = nil
local idle_control_active = false
local idle_control_active_last = false
local last_scaled_output = nil
local idle_default = nil
local ramp_up_complete = false
local idle_adjusted = false
local last_idc_time = nil
local time_now = nil
local pv = nil
local thr_ctl = nil
local thr_out_last = nil
local pot_input = rc:find_channel_for_option(301)
local switch_rsc = rc:find_channel_for_option(32)
local rsc_output = SRV_Channels:find_channel(31)
local SERVO_MAX = Parameter('SERVO' .. (rsc_output+1) .. '_MAX')
local SERVO_MIN = Parameter('SERVO' .. (rsc_output+1) .. '_MIN')
local SERVO_REV = Parameter('SERVO' .. (rsc_output+1) .. '_REVERSED')
local servo_range = SERVO_MAX:get() - SERVO_MIN:get()
local H_RSC_IDLE = Parameter('H_RSC_IDLE')
local H_RSC_RUNUP_TIME = Parameter('H_RSC_RUNUP_TIME')

-- map function

function map(x, in_min, in_max, out_min, out_max)
   return out_min + (x - in_min)*(out_max - out_min)/(in_max - in_min)
end

-- constrain function

local function constrain(v, vmin, vmax)
   if v < vmin then
      v = vmin
   end
   if v > vmax then
      v = vmax
   end
   return v
end

-- PI controller function
local function PI_controller(kP,kI,iMax,min,max)

   local self = {}

   local _kP = kP
   local _kI = kI
   local _iMax = iMax
   local _min = min
   local _max = max
   local _last_t = nil
   local _I = 0
   local _total = 0
   local _counter = 0

   function self.update(target, current)
      local now = millis()
      if not _last_t then
         _last_t = now
      end
      local dt = (now - _last_t):tofloat()*0.001
      _last_t = now
      local err = target - current
      _counter = _counter + 1
      local P = _kP:get() * err
      if ((_total < _max and _total > _min) or (_total >= _max and err < 0) or (_total <= _min and err > 0)) then
         _I = _I + _kI:get() * err * dt
      end
      if _iMax:get() > 0 then
         _I = constrain(_I, -_iMax:get(), iMax:get())
      end
      local ret = P + _I
      _total = ret
      return ret
   end

   function self.reset(integrator)
      _I = integrator
   end

   return self
end

local thr_PI = PI_controller(IDLE_GAIN_P, IDLE_GAIN_I, IDLE_GAIN_MAX, 0, 1)

-- main update function

function update()  

   local armed = arming:is_armed()

  -- aux potentiometer for manual adjusting of idle

   local pot_pos = pot_input:norm_input()
   local thr_man = map(pot_pos,-1,1,0,1)

   if armed == false then
      idle_default = H_RSC_IDLE:get()
      idle_control_active = false
      ramp_up_complete = false
      idle_adjusted = false
      thr_PI.reset(0)
   else 
      if switch_rsc:get_aux_switch_pos() == 0 and vehicle:get_likely_flying() == false then
         if H_RSC_IDLE:get()~= idle_default then
            H_RSC_IDLE:set(idle_default)
            gcs:send_text(5, "H_RSC_IDLE set to:".. tostring(H_RSC_IDLE:get()))
         end
         if IDLE_RPM_ENABLE:get() == 0 then
            ramp_up_complete = false
            idle_adjusted = false
            thr_out = H_RSC_IDLE:get() + thr_man*(IDLE_MAX:get() - H_RSC_IDLE:get())
         else
            if thr_man == 0 then
              idle_control_active = false
              if idle_control_active_last ~= idle_control_active then
                 gcs:send_text(5, "idle control: OFF")
              end
               thr_out = H_RSC_IDLE:get()
               thr_ctl = 0
               thr_PI.reset(0)
            else
               local rpm_current = RPM:get_rpm((IDLE_RPM_ENABLE:get())-1)
               ramp_up_complete = false
               idle_adjusted = false
               if rpm_current < (IDLE_SETPOINT:get() - IDLE_RANGE:get()) then
                  thr_out = H_RSC_IDLE:get() + thr_man*(IDLE_MAX:get() - H_RSC_IDLE:get())
                  thr_out_last = thr_out
               elseif rpm_current > (IDLE_SETPOINT:get() + IDLE_RANGE:get()) then 
                  thr_out = H_RSC_IDLE:get()
                  thr_out_last = thr_out
               else
                  -- throttle output set from the PI controller
                  pv = rpm_current/(IDLE_SETPOINT:get())
                  thr_ctl = thr_PI.update(1, pv)
                  thr_ctl = constrain(thr_ctl,0,1)
                  thr_out = H_RSC_IDLE:get() + thr_ctl*(IDLE_MAX:get() - H_RSC_IDLE:get())
                  if thr_out_last == nil then
                     thr_out_last = 0
                  end
                  thr_out = constrain(thr_out, thr_out_last-0.05, thr_out_last+0.05)
                  thr_out_last = thr_out
                  idle_control_active = true
               end
               if idle_control_active_last ~= idle_control_active then
                  gcs:send_text(5, "idle control: ON")
               end
             end
         end
         last_idc_time = millis()
         last_scaled_output = thr_out/100
         if SERVO_REV:get() == 0 then
            SRV_Channels:set_output_pwm_chan_timeout(rsc_output, math.floor((last_scaled_output*servo_range)+SERVO_MIN:get()), 150)
         else
            SRV_Channels:set_output_pwm_chan_timeout(rsc_output, math.floor(SERVO_MAX:get()-(last_scaled_output*servo_range)), 150)
         end
      else
         -- motor interlock disabled, armed state, flight
         idle_control_active = false
         if idle_control_active_last ~= idle_control_active then
            gcs:send_text(5, "idle control: deactivated")
         end
         if ramp_up_complete ~= true then
            time_now = millis()
            if ((time_now-last_idc_time):tofloat()*0.001) < H_RSC_RUNUP_TIME:get() then
               if idle_adjusted ~= true then
                  H_RSC_IDLE:set(last_scaled_output*100)
                  idle_adjusted = true
                  gcs:send_text(5, "H_RSC_IDLE updated for ramp up:".. tostring(H_RSC_IDLE:get()))
               end
            else
               if H_RSC_IDLE:get()~= idle_default then
                  H_RSC_IDLE:set(idle_default)
                  gcs:send_text(5, "H_RSC_IDLE default restored:".. tostring(H_RSC_IDLE:get()))
               end
               ramp_up_complete = true
            end
         end
      end
  -- update notify variable
  idle_control_active_last = idle_control_active
   end

   return update, 100 -- 10Hz rate
end

gcs:send_text(5, "idle_control_running")

return update()