From 10a6fcad7a69c861e91eac1e94b6f587e89b3823 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Tue, 5 Dec 2023 07:07:10 +0000 Subject: [PATCH] AP_Scripting: script for idle control (gas helicopters) allows manual and/or automatic engine rpm control during ground idling fix for conversion to float rename fix --- .../applets/Heli_idle_control.lua | 212 ++++++++++++++++++ .../AP_Scripting/applets/Heli_idle_control.md | 17 ++ 2 files changed, 229 insertions(+) create mode 100644 libraries/AP_Scripting/applets/Heli_idle_control.lua create mode 100644 libraries/AP_Scripting/applets/Heli_idle_control.md diff --git a/libraries/AP_Scripting/applets/Heli_idle_control.lua b/libraries/AP_Scripting/applets/Heli_idle_control.lua new file mode 100644 index 0000000000000..16e7de0a0056b --- /dev/null +++ b/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() diff --git a/libraries/AP_Scripting/applets/Heli_idle_control.md b/libraries/AP_Scripting/applets/Heli_idle_control.md new file mode 100644 index 0000000000000..39fb88684e020 --- /dev/null +++ b/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