Skip to content

Commit b23ded8

Browse files
GeoracerGeoracer
committed
AP_Scripting: Added VSPeak Modell flow meter driver
1 parent f07df39 commit b23ded8

File tree

2 files changed

+294
-0
lines changed

2 files changed

+294
-0
lines changed

‎libraries/AP_Scripting/drivers/VSPeak_flow_meter.lua

Lines changed: 256 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,256 @@
1+
-- VSPeak_flow_meter.lua: Driver for the VSPeak Modell fuel flow sensor.
2+
--
3+
-- Setup
4+
-- 1. Place this script in the "scripts" directory of the autopilot.
5+
-- 2. Connect the sensor to a serial port (for now referred to as SERIAL*)
6+
-- 3. Enable the scripting engine via SCR_ENABLE.
7+
-- 4. Set SERIAL*_BAUD = 19 (19200)
8+
-- 5. Set SERIAL*_PROTOCOL = 28 (Scripting)
9+
-- 6. Set EFI_TYPE=7 (Scripting)
10+
-- 7. Set BATT2_MONITOR = 27 (EFI)
11+
12+
--
13+
-- Usage
14+
-- No further action required.
15+
16+
--[[
17+
Global definitions
18+
--]]
19+
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
20+
local SCRIPT_NAME = "VSPeak Modell flow meter driver"
21+
local NUM_SCRIPT_PARAMS = 1
22+
local LOOP_RATE_HZ = 10
23+
local last_warning_time_ms = 0
24+
local WARNING_DEADTIME_MS = 1000
25+
local FRAME_LEN = 7
26+
local BUFFER_LEN = 2*FRAME_LEN - 1
27+
local HEADER_BYTE = 0xFE
28+
local INIT_DELAY_MS = 5000
29+
local uart
30+
local efi_backend
31+
local state = {}
32+
33+
34+
-- State machine states.
35+
local FSM_STATE = {
36+
INACTIVE = 0,
37+
ACTIVE = 1,
38+
}
39+
local current_state = FSM_STATE.INACTIVE
40+
local next_state = FSM_STATE.INACTIVE
41+
42+
--[[
43+
New parameter declarations
44+
--]]
45+
local PARAM_TABLE_KEY = 142 -- Ensure no other applet is using this key.
46+
local PARAM_TABLE_PREFIX = "VSPF_"
47+
48+
-- Bind a parameter to a variable.
49+
function bind_param(name)
50+
return Parameter(name)
51+
end
52+
53+
-- Add a parameter and bind it to a variable.
54+
function bind_add_param(name, idx, default_value)
55+
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
56+
return bind_param(PARAM_TABLE_PREFIX .. name)
57+
end
58+
59+
-- Add param table.
60+
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, NUM_SCRIPT_PARAMS), SCRIPT_NAME .. ': Could not add param table.')
61+
62+
--[[
63+
// @Param: ENABLE
64+
// @DisplayName: Enable this script
65+
// @Description: When set to 0 this script will not run. When set to 1 this script will run.
66+
// @Range: 0 1
67+
// @User: Standard
68+
--]]
69+
local VSPF_ENABLE = bind_add_param('ENABLE', 1, 0)
70+
71+
--[[
72+
Potential additions:
73+
- The scripting switch.
74+
--]]
75+
-- Warn the user, throttling the message rate.
76+
function warn_user(msg, severity)
77+
severity = severity or MAV_SEVERITY.WARNING -- Optional severity argument.
78+
if millis() - last_warning_time_ms > WARNING_DEADTIME_MS then
79+
gcs:send_text(severity, SCRIPT_NAME .. ": " .. msg)
80+
last_warning_time_ms = millis()
81+
end
82+
end
83+
84+
-- Get uint16 from two bytes
85+
function uint16_value(hbyte, lbyte)
86+
return ((hbyte & 0xFF) << 8) | (lbyte & 0xFF)
87+
end
88+
89+
function read_uart()
90+
local n_avail = uart:available()
91+
92+
-- Discard up to BUFFER_LEN bytes.
93+
-- These are stale data.
94+
if (n_avail - BUFFER_LEN) > 0 then
95+
n_avail = BUFFER_LEN
96+
for _ = 1, (n_avail-BUFFER_LEN) do
97+
uart:read()
98+
end
99+
end
100+
101+
-- Read in the rest of the data.
102+
for _ = 1, n_avail do
103+
local val = uart:read()
104+
table.insert(state.buffer, val)
105+
end
106+
107+
end
108+
109+
function parse_buffer()
110+
111+
-- Drop old data.
112+
for _ = 1, (#state.buffer - BUFFER_LEN) do
113+
table.remove(state.buffer, 1)
114+
end
115+
116+
-- Find the header byte and discard up to it.
117+
for _ = 1, #state.buffer do
118+
if state.buffer[1] ~= HEADER_BYTE then
119+
table.remove(state.buffer, 1)
120+
else
121+
break
122+
end
123+
end
124+
125+
-- Check if the buffer has enough data in it.
126+
if #state.buffer >= FRAME_LEN then
127+
-- Parse the data.
128+
state.flow = uint16_value(state.buffer[3], state.buffer[2])
129+
state.fuel_ml = uint16_value(state.buffer[5], state.buffer[4])
130+
state.fuel_pct = state.buffer[6]
131+
state.checksum = state.buffer[7]
132+
-- Calculate the checksum.
133+
local checksum = 0x00
134+
for i = 1, 6 do
135+
checksum = (checksum ~ state.buffer[i]) & 0xFF
136+
end
137+
138+
-- If parse successful, discard them.
139+
if checksum == state.checksum then
140+
state.updated = true
141+
for _ = 1, FRAME_LEN do
142+
table.remove(state.buffer, 1)
143+
end
144+
else
145+
warn_user("Checksum failed")
146+
state.updated = false
147+
-- Discard the header to force the corrupt data to flush on the next pass.
148+
table.remove(state.buffer, 1)
149+
end
150+
151+
end
152+
153+
end
154+
155+
function update_efi()
156+
if state.updated == false then
157+
return
158+
end
159+
160+
local efi_state = EFI_State()
161+
efi_state:fuel_consumption_rate_cm3pm(state.flow)
162+
efi_state:estimated_consumed_fuel_volume_cm3(state.fuel_ml)
163+
efi_state:last_updated_ms(millis())
164+
efi_backend:handle_scripting(efi_state)
165+
end
166+
167+
--[[
168+
Activation conditions
169+
--]]
170+
-- Check for script activating conditions here.
171+
function can_start()
172+
return VSPF_ENABLE:get() == 1 and (millis() > INIT_DELAY_MS)
173+
end
174+
175+
--[[
176+
Deactivation conditions
177+
--]]
178+
-- Check for script deactivating conditions here.
179+
function must_stop()
180+
return VSPF_ENABLE:get() == 0
181+
end
182+
183+
--[[
184+
State machine
185+
--]]
186+
-- Write the state machine transitions.
187+
function fsm_step()
188+
if current_state == FSM_STATE.INACTIVE then
189+
190+
if can_start() then
191+
setup()
192+
next_state = FSM_STATE.ACTIVE
193+
end
194+
195+
elseif current_state == FSM_STATE.ACTIVE then
196+
197+
-- Feed the buffer.
198+
read_uart()
199+
parse_buffer()
200+
update_efi()
201+
202+
if must_stop() then
203+
next_state = FSM_STATE.INACTIVE
204+
end
205+
end
206+
current_state = next_state
207+
end
208+
209+
--[[
210+
Main loop function
211+
--]]
212+
function update()
213+
-- Stuff is placed here.
214+
fsm_step()
215+
end
216+
217+
--[[
218+
Setup function
219+
--]]
220+
function setup()
221+
uart = serial:find_serial(0) -- First scripting serial
222+
if not uart then
223+
warn_user("Unable to find scripting serial", MAV_SEVERITY.ERROR)
224+
return
225+
end
226+
uart:begin(19200)
227+
228+
efi_backend = efi:get_backend(0)
229+
if not efi_backend then
230+
warn_user("Unable to find EFI backend", MAV_SEVERITY.ERROR)
231+
return
232+
end
233+
234+
state.last_read_us = 0x00
235+
state.buffer = {}
236+
state.flow = 0x00
237+
state.fuel_ml = 0x00
238+
state.fuel_pct = 0x00
239+
state.checksum = 0x00
240+
state.updated = false
241+
end
242+
243+
gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME .. string.format(" loaded."))
244+
245+
-- Wrapper around update() to catch errors.
246+
function protected_wrapper()
247+
local success, err = pcall(update)
248+
if not success then
249+
gcs:send_text(MAV_SEVERITY.EMERGENCY, "Internal Error: " .. err)
250+
return protected_wrapper, 1000
251+
end
252+
return protected_wrapper, 1000.0/LOOP_RATE_HZ
253+
end
254+
255+
-- Start running update loop
256+
return protected_wrapper()

‎libraries/AP_Scripting/drivers/VSPeak_flow_meter.md

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
# VSPeak Modell flow meter Driver
2+
3+
This driver implements support for the VSPeak Modell flow meter sensor.
4+
5+
https://www.vspeak-modell.de/en/flow-meter
6+
7+
# Parameters
8+
9+
The script used the following parameters:
10+
11+
## VSPF_ENABLE
12+
13+
Setting this to 1 enables the driver.
14+
15+
# Setup
16+
17+
First of all, calibrate and configure the flow meter according to the
18+
manufacturer instructions. Set your configuration with the `FLOW.txt` file,
19+
placed in the SD card in the sensor itself.
20+
21+
Once this is done, perform the following steps.
22+
23+
1. Place this script in the "scripts" directory of the autopilot.
24+
2. Connect the sensor to a serial port (for now referred to as `SERIAL*`)
25+
3. Enable the scripting engine via `SCR_ENABLE`.
26+
4. Set the baud rate to 19200 with `SERIAL*_BAUD = 19`.
27+
5. Set port protocol to scripting with `SERIAL*_PROTOCOL = 28`.
28+
6. Set the EFI type to scripting with `EFI_TYPE = 7`.
29+
7. Set a battery monitor to EFI. For example, to set the 2nd battery monitor
30+
use `BATT2_MONITOR = 27`.
31+
8. Enable the script itself with `VSPF_ENABLE=1`.
32+
33+
# Operation
34+
35+
Once everything is configured correctly, the corresponding battery monitor
36+
will display in the corresponding `BATTERY_STATUS` MAVLink message:
37+
- The current fuel flow in cl/hr (centiliters per hour) in the `current_battery` field.
38+
- The current fuel already consumed in ml (milliliters) in the `current_consumed` field.

0 commit comments

Comments
 (0)