-
Notifications
You must be signed in to change notification settings - Fork 16.8k
/
gen_control.lua
213 lines (179 loc) · 6.09 KB
/
gen_control.lua
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
--[[
control throttle on a generator to achieve a target battery voltage
This is meant to replace throttle governors on some hybrid drone
generators. It monitors battery voltage and controls the throttle
of the generator to maintain a target voltage using a PI controller
--]]
-- luacheck: only 0
---@diagnostic disable: need-check-nil
---@diagnostic disable: param-type-mismatch
UPDATE_RATE_HZ = 10
-- setup a parameter block
local PARAM_TABLE_KEY = 73
local PARAM_TABLE_PREFIX = 'GENCTL_'
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table')
-- bind a parameter to a variable
function bind_param(name)
local p = Parameter()
assert(p:init(name), string.format('could not find %s parameter', name))
return p
end
-- add a parameter and bind it to a variable
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 bind_param(PARAM_TABLE_PREFIX .. name)
end
GENCTL_ENABLE = bind_add_param('ENABLE', 1, 0)
if GENCTL_ENABLE:get() <= 0 then
return
end
-- MIN, MAX and IDLE PWM for throttle output
GENCTL_PWM_MIN = bind_add_param('PWM_MIN', 2, 1000)
GENCTL_PWM_MAX = bind_add_param('PWM_MAX', 3, 2000)
GENCTL_PWM_IDLE = bind_add_param('PWM_IDLE', 4, 1200)
-- P and I gains for controller
GENCTL_PID_P = bind_add_param('PID_P', 5, 0.1)
GENCTL_PID_I = bind_add_param('PID_I', 6, 0.1)
-- maximum I contribution
GENCTL_PID_IMAX = bind_add_param('PID_IMAX', 7, 1.0)
-- RCn_OPTION value for 3 position switch
GENCTL_RC_FUNC = bind_add_param('RC_FUNC', 8, 300)
-- output servo channel that we will control
GENCTL_THR_CHAN = bind_add_param('THR_CHAN', 9, 0)
-- battery index to monitor, 0 is first battery
GENCTL_BAT_IDX = bind_add_param('BAT_IDX', 10, 0)
-- target voltage
GENCTL_VOLT_TARG = bind_add_param('VOLT_TARG', 11, 0)
-- maximum slew rate in percent/second for throttle change
GENCTL_SLEW_RATE = bind_add_param('SLEW_RATE', 12, 100)
local MAV_SEVERITY_INFO = 6
local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_EMERGENCY = 0
local switch = nil
local thr_pwm = GENCTL_PWM_MIN:get()
local last_switch_pos = nil
-- constrain a value between limits
local function constrain(v, vmin, vmax)
if v < vmin then
v = vmin
end
if v > vmax then
v = vmax
end
return v
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
-- declaration if you want to
local self = {}
-- private fields as locals
local _kP = kP
local _kI = kI
local _iMax = iMax
local _min = min
local _max = max
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: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 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
-- log the controller internals
function self.log(name)
-- allow for an external addition to total
logger.write(name,'Targ,Curr,P,I,Total','fffff',_target,_current,_P,_I,_total)
end
-- return the instance
return self
end
local thr_PI = PI_controller(GENCTL_PID_P, GENCTL_PID_I, GENCTL_PID_IMAX, 0, 1)
local last_pwm = GENCTL_PWM_MIN:get()
function update()
if switch == nil then
switch = rc:find_channel_for_option(math.floor(GENCTL_RC_FUNC:get()))
end
if switch == nil or GENCTL_ENABLE:get() < 1 then
-- nothing to do
return
end
local sw_pos = switch:get_aux_switch_pos()
if sw_pos ~= last_switch_pos then
if sw_pos == 0 then
gcs:send_text(MAV_SEVERITY_INFO,"GenCtl: off")
elseif sw_pos == 1 then
gcs:send_text(MAV_SEVERITY_INFO,"GenCtl: idle")
else
gcs:send_text(MAV_SEVERITY_INFO,"GenCtl: run")
end
last_switch_pos = sw_pos
end
if sw_pos == 0 then
-- force low throttle
thr_pwm = GENCTL_PWM_MIN:get()
thr_PI.reset(0)
elseif sw_pos == 1 then
-- force idle
thr_pwm = GENCTL_PWM_IDLE:get()
thr_PI.reset(0)
else
local bat_volt = battery:voltage_resting_estimate(GENCTL_BAT_IDX:get())
local thr_out = thr_PI.update(GENCTL_VOLT_TARG:get(), bat_volt)
thr_out = constrain(thr_out, 0, 1)
thr_pwm = GENCTL_PWM_IDLE:get() + thr_out * (GENCTL_PWM_MAX:get() - GENCTL_PWM_IDLE:get())
thr_PI.log("GENC")
end
local max_change = GENCTL_SLEW_RATE:get() * (GENCTL_PWM_MAX:get() - GENCTL_PWM_MIN:get()) * 0.01 / UPDATE_RATE_HZ
thr_pwm = constrain(thr_pwm, last_pwm - max_change, last_pwm + max_change)
last_pwm = thr_pwm
if GENCTL_THR_CHAN:get() > 0 then
SRV_Channels:set_output_pwm_chan(GENCTL_THR_CHAN:get()-1, math.floor(thr_pwm))
end
end
-- wrapper around update(). This calls update() at 10Hz,
-- and if update faults then an error is displayed, but the script is not
-- stopped
function protected_wrapper()
local success, err = pcall(update)
if not success then
gcs:send_text(MAV_SEVERITY_EMERGENCY, "Internal Error: " .. err)
-- when we fault we run the update function again after 1s, slowing it
-- down a bit so we don't flood the console with errors
--return protected_wrapper, 1000
return
end
return protected_wrapper, 1000/UPDATE_RATE_HZ
end
gcs:send_text(MAV_SEVERITY_INFO,"Loaded gen_control.lua")
-- start running update loop
return protected_wrapper()