TradHeli: script for idle control

libraries/AP_Scripting/applets/Heli_idle_control.lua

@@ -0,0 +1,212 @@
+-- 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()

libraries/AP_Scripting/applets/Heli_idle_control.md

@@ -0,0 +1,17 @@
+# Idle Control
+
+Allows manual or automatic rpm control for heli while on ground idle condition
+
+# Parameters
+
+IDLE_GAIN_I = integrative gain of the controller
+IDLE_GAIN_P = proportional gain of the controller
+IDLE_GAIN_MAX = IMAX for integrative
+IDLE_MAX = maximum throttle position, shall be set low enough to prevent clutch engagement/slipping or lower than first point of throttle curve
+IDLE_RANGE = rpm range of operation for the idle control
+IDLE_SETPOINT = desired rpm setpoint
+IDLE_RPM_ENABLE = 0 for manual throttle control // 1 for RPM1 targeting // 2 for RPM2 targeting
+
+# How To Use
+
+set RCx_OPTION to 301 to enable idle control from an auxiliary potentiometer