|
1 |
| -MSP_PID_FORMAT = { |
2 |
| - read = 112, -- MSP_PID |
3 |
| - write = 202, -- MSP_SET_PID |
4 |
| - minBytes = 8, |
5 |
| - fields = { |
6 |
| - -- P |
7 |
| - { vals = { 1 } }, |
8 |
| - { vals = { 4 } }, |
9 |
| - { vals = { 7 } }, |
10 |
| - -- I |
11 |
| - { vals = { 2 } }, |
12 |
| - { vals = { 5 } }, |
13 |
| - { vals = { 8 } }, |
14 |
| - -- D |
15 |
| - { vals = { 3 } }, |
16 |
| - { vals = { 6 } }, |
17 |
| - }, |
18 |
| -} |
19 |
| - |
20 |
| -MSP_PID_ADVANCED_FORMAT = { |
21 |
| - read = 94, -- MSP_PID_ADVANCED |
22 |
| - write = 95, -- MSP_SET_PID_ADVANCED |
23 |
| - minBytes = 23, |
24 |
| - fields = { |
25 |
| - -- weight |
26 |
| - { vals = { 10 }, scale = 100 }, |
27 |
| - -- transition |
28 |
| - { vals = { 9 }, scale = 100 }, |
29 |
| - }, |
30 |
| -} |
31 |
| - |
32 |
| -local INTRO_DELAY = 1600 |
33 |
| -local READOUT_DELAY = 500 |
34 |
| - |
35 |
| -function extractMspValues(cmd, rx_buf, msgFormat, msgValues) |
36 |
| - if cmd == nil or rx_buf == nil then |
37 |
| - return |
38 |
| - end |
39 |
| - if cmd ~= msgFormat.read then |
40 |
| - return |
41 |
| - end |
42 |
| - if #(rx_buf) > 0 then |
43 |
| - msgValues.raw = {} |
44 |
| - for i=1,#(rx_buf) do |
45 |
| - msgValues.raw[i] = rx_buf[i] |
46 |
| - end |
47 |
| - |
48 |
| - msgValues.values = {} |
49 |
| - for i=1,#(msgFormat.fields) do |
50 |
| - if (#(msgValues.raw) or 0) >= msgFormat.minBytes then |
51 |
| - local f = msgFormat.fields[i] |
52 |
| - if f.vals then |
53 |
| - local value = 0; |
54 |
| - for idx=1, #(f.vals) do |
55 |
| - local raw_val = msgValues.raw[f.vals[idx]] |
56 |
| - raw_val = bit32.lshift(raw_val, (idx-1)*8) |
57 |
| - value = bit32.bor(value, raw_val) |
58 |
| - end |
59 |
| - msgValues.values[i] = value/(f.scale or 1) |
60 |
| - end |
61 |
| - end |
62 |
| - end |
63 |
| - end |
64 |
| -end |
65 |
| - |
66 |
| -function readoutMsp(msgFormat, msg) |
67 |
| - local t = getTime() |
68 |
| - if msg.lastTrigger == nil or msg.lastTrigger + INTRO_DELAY <= t then |
69 |
| - playFile(msg.intro) |
70 |
| - msg.lastTrigger = t |
71 |
| - elseif msg.reqTS == nil or msg.reqTS + READOUT_DELAY <= t then |
72 |
| - protocol.mspRead(msgFormat.read) |
73 |
| - msg.reqTS = t |
74 |
| - else |
75 |
| - local cmd, rx_buf = mspPollReply() |
76 |
| - extractMspValues(cmd, rx_buf, msgFormat, msg) |
77 |
| - if msg.raw then |
78 |
| - for i=1,#(msg.readoutValues) do |
79 |
| - playNumber(msg.values[msg.readoutValues[i]], 0) |
80 |
| - end |
81 |
| - msg.raw = nil |
82 |
| - end |
83 |
| - end |
84 |
| - mspProcessTxQ() |
85 |
| -end |
| 1 | +MSP_PID_FORMAT = { |
| 2 | + read = 112, -- MSP_PID |
| 3 | + write = 202, -- MSP_SET_PID |
| 4 | + minBytes = 8, |
| 5 | + fields = { |
| 6 | + -- P |
| 7 | + { vals = { 1 } }, |
| 8 | + { vals = { 4 } }, |
| 9 | + { vals = { 7 } }, |
| 10 | + -- I |
| 11 | + { vals = { 2 } }, |
| 12 | + { vals = { 5 } }, |
| 13 | + { vals = { 8 } }, |
| 14 | + -- D |
| 15 | + { vals = { 3 } }, |
| 16 | + { vals = { 6 } }, |
| 17 | + }, |
| 18 | +} |
| 19 | + |
| 20 | +MSP_PID_ADVANCED_FORMAT = { |
| 21 | + read = 94, -- MSP_PID_ADVANCED |
| 22 | + write = 95, -- MSP_SET_PID_ADVANCED |
| 23 | + minBytes = 23, |
| 24 | + fields = { |
| 25 | + -- weight |
| 26 | + { vals = { 10 }, scale = 100 }, |
| 27 | + -- transition |
| 28 | + { vals = { 9 }, scale = 100 }, |
| 29 | + }, |
| 30 | +} |
| 31 | + |
| 32 | +local INTRO_DELAY = 1600 |
| 33 | +local READOUT_DELAY = 500 |
| 34 | + |
| 35 | +function extractMspValues(cmd, rx_buf, msgFormat, msgValues) |
| 36 | + if cmd == nil or rx_buf == nil then |
| 37 | + return |
| 38 | + end |
| 39 | + if cmd ~= msgFormat.read then |
| 40 | + return |
| 41 | + end |
| 42 | + if #(rx_buf) > 0 then |
| 43 | + msgValues.raw = {} |
| 44 | + for i=1,#(rx_buf) do |
| 45 | + msgValues.raw[i] = rx_buf[i] |
| 46 | + end |
| 47 | + |
| 48 | + msgValues.values = {} |
| 49 | + for i=1,#(msgFormat.fields) do |
| 50 | + if (#(msgValues.raw) or 0) >= msgFormat.minBytes then |
| 51 | + local f = msgFormat.fields[i] |
| 52 | + if f.vals then |
| 53 | + local value = 0; |
| 54 | + for idx=1, #(f.vals) do |
| 55 | + local raw_val = msgValues.raw[f.vals[idx]] |
| 56 | + raw_val = bit32.lshift(raw_val, (idx-1)*8) |
| 57 | + value = bit32.bor(value, raw_val) |
| 58 | + end |
| 59 | + msgValues.values[i] = value/(f.scale or 1) |
| 60 | + end |
| 61 | + end |
| 62 | + end |
| 63 | + end |
| 64 | +end |
| 65 | + |
| 66 | +function readoutMsp(msgFormat, msg) |
| 67 | + local t = getTime() |
| 68 | + if msg.lastTrigger == nil or msg.lastTrigger + INTRO_DELAY <= t then |
| 69 | + playFile(msg.intro) |
| 70 | + msg.lastTrigger = t |
| 71 | + elseif msg.reqTS == nil or msg.reqTS + READOUT_DELAY <= t then |
| 72 | + protocol.mspRead(msgFormat.read) |
| 73 | + msg.reqTS = t |
| 74 | + else |
| 75 | + local cmd, rx_buf = mspPollReply() |
| 76 | + extractMspValues(cmd, rx_buf, msgFormat, msg) |
| 77 | + if msg.raw then |
| 78 | + for i=1,#(msg.readoutValues) do |
| 79 | + playNumber(msg.values[msg.readoutValues[i]], 0) |
| 80 | + end |
| 81 | + msg.raw = nil |
| 82 | + end |
| 83 | + end |
| 84 | + mspProcessTxQ() |
| 85 | +end |
0 commit comments