|
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