Skip to content

Commit 2882056

Browse files
tridgetridge
committed
AP_Scripting: example of using DroneCAN messaging
1 parent 7541604 commit 2882056

File tree

1 file changed

+256
-0
lines changed

1 file changed

+256
-0
lines changed

‎libraries/AP_Scripting/examples/DroneCAN_test.lua

+256
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,256 @@
1+
--[[
2+
example of creating and sending DroneCAN messages
3+
--]]
4+
5+
6+
local MAGNETICFIELDSTRENGTHHIRES_ID = 1043
7+
local MAGNETICFIELDSTRENGTHHIRES_SIGNATURE = uint64_t(0x3053EBE3, 0xD750286F)
8+
9+
local RAWAIRDATA_ID = 1027
10+
local RAWAIRDATA_SIGNATURE = uint64_t(0xC77DF38B, 0xA122F5DA)
11+
12+
local PARAM_GETSET_ID = 11
13+
local PARAM_GETSET_SIGNATURE = uint64_t(0xA7B622F9, 0x39D1A4D5)
14+
15+
local NODESTATUS_ID = 341
16+
local NODESTATUS_SIGNATURE = uint64_t(0x0F0868D0, 0xC1A7C6F1)
17+
18+
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
19+
20+
local TARGET_NODE = Parameter('SCR_USER1')
21+
22+
-- a handle we will use for broadcasting
23+
local dc_handle = DroneCAN_Handle(0, MAGNETICFIELDSTRENGTHHIRES_SIGNATURE, MAGNETICFIELDSTRENGTHHIRES_ID)
24+
25+
-- a handle for fetching parameters
26+
local param_handle = DroneCAN_Handle(0, PARAM_GETSET_SIGNATURE, PARAM_GETSET_ID)
27+
28+
--[[
29+
setup subscription to NodeStatus
30+
--]]
31+
local nodestatus_handle = DroneCAN_Handle(0, NODESTATUS_SIGNATURE, NODESTATUS_ID)
32+
nodestatus_handle:subscribe()
33+
34+
--[[
35+
setup subscription to raw air data
36+
--]]
37+
local airspeed_handle = DroneCAN_Handle(0, RAWAIRDATA_SIGNATURE, RAWAIRDATA_ID)
38+
airspeed_handle:subscribe()
39+
40+
-- table of all nodes
41+
local node_status = {}
42+
43+
44+
--[[
45+
send highres mag using a global handle
46+
--]]
47+
local function send_mag_highres()
48+
local payload = string.pack("Bfff", 7, 1, 2, 3)
49+
dc_handle:broadcast(payload)
50+
gcs:send_text(MAV_SEVERITY.INFO, "mag highres broadcast done")
51+
end
52+
53+
--[[
54+
send highres mag using a handle that will be closed after being used
55+
--]]
56+
local function send_mag_highres2()
57+
local h = DroneCAN_Handle(0, MAGNETICFIELDSTRENGTHHIRES_SIGNATURE, MAGNETICFIELDSTRENGTHHIRES_ID)
58+
local payload = string.pack("Bfff", 8, 10, 11, 12)
59+
h:broadcast(payload)
60+
gcs:send_text(MAV_SEVERITY.INFO, "mag highres broadcast2 done")
61+
end
62+
63+
--[[
64+
unpack a float16 into a floating point number
65+
--]]
66+
local function unpackFloat16(v16)
67+
-- Extract the sign (bit 15), exponent (bits 10–14) and fraction (bits 0–9)
68+
local sign = (v16 >> 15) & 0x1
69+
local exponent = (v16 >> 10) & 0x1F
70+
local fraction = v16 & 0x3FF
71+
72+
local value
73+
if exponent == 0 then
74+
if fraction == 0 then
75+
-- Zero (positive or negative)
76+
value = 0.0
77+
else
78+
-- Subnormal numbers (exponent = -14, no implicit leading 1)
79+
value = (fraction / 1024.0) * 2.0^-14
80+
end
81+
elseif exponent == 0x1F then
82+
if fraction == 0 then
83+
-- Infinity (positive or negative)
84+
value = math.huge
85+
else
86+
-- NaN (Not a Number)
87+
value = 0/0
88+
end
89+
else
90+
-- Normalized numbers: implicit 1 before the fraction and exponent bias of 15.
91+
value = (1 + fraction / 1024.0) * 2.0^(exponent - 15)
92+
end
93+
94+
-- Apply the sign bit
95+
if sign == 1 then
96+
value = -value
97+
end
98+
99+
return value
100+
end
101+
102+
--[[
103+
check for incoming airspeed broadcast messages
104+
--]]
105+
local function check_airspeed()
106+
local payload, nodeid = airspeed_handle:check_message()
107+
if not payload then
108+
return
109+
end
110+
local flags, static_pressure, differential_pressure, static_pressure_sensor_temperature,
111+
differential_pressure_sensor_temperature, static_air_temperature, pitot_temperature = string.unpack("BffHHHH", payload)
112+
if flags then
113+
local temp_C = unpackFloat16(static_air_temperature) - 273.15;
114+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Rawairdata(%u): %f %.2fC",
115+
nodeid, differential_pressure, temp_C))
116+
logger:write("ASPL", 'SP,DP,ST,DT,SAT,PIT', 'ffffff',
117+
static_pressure, differential_pressure, static_pressure_sensor_temperature, differential_pressure_sensor_temperature,
118+
static_air_temperature, pitot_temperature)
119+
end
120+
end
121+
122+
--[[
123+
parse a parameter GetSet NumericValue
124+
--]]
125+
local function parse_param_NumericValue(payload, byte_offset)
126+
local vtype = string.unpack("B", payload, byte_offset[1])
127+
byte_offset[1] = byte_offset[1] + 1
128+
if vtype == 0 then
129+
return nil
130+
elseif vtype == 1 then
131+
-- integer (treat as 32 bit for now, actually 64 bit)
132+
local ret = string.unpack("i", payload, byte_offset[1])
133+
byte_offset[1] = byte_offset[1] + 8
134+
return ret
135+
elseif vtype == 2 then
136+
-- float32
137+
local ret = string.unpack("f", payload, byte_offset[1])
138+
byte_offset[1] = byte_offset[1] + 4
139+
return ret
140+
else
141+
return nil
142+
end
143+
end
144+
145+
--[[
146+
parse a parameter GetSet Value
147+
--]]
148+
local function parse_param_Value(payload, byte_offset)
149+
local vtype = string.unpack("B", payload, byte_offset[1])
150+
byte_offset[1] = byte_offset[1] + 1
151+
if vtype == 0 then
152+
return nil
153+
elseif vtype == 1 then
154+
-- signed integer (64 bit), return as signed 32 bit number
155+
local ret_lo, ret_hi = string.unpack("II", payload, byte_offset[1])
156+
byte_offset[1] = byte_offset[1] + 8
157+
if ret_hi & 0x80000000 then
158+
return -ret_lo
159+
end
160+
return ret_lo
161+
elseif vtype == 2 then
162+
-- float32
163+
local ret = string.unpack("f", payload, byte_offset[1])
164+
byte_offset[1] = byte_offset[1] + 4
165+
return ret
166+
elseif vtype == 3 then
167+
-- bool
168+
local v = string.unpack("B", payload, byte_offset[1])
169+
byte_offset[1] = byte_offset[1] + 1
170+
return v == 1
171+
elseif vtype == 4 then
172+
-- string
173+
local slen = string.unpack("B", payload, byte_offset[1])
174+
local ret = string.sub(payload, byte_offset[1]+1, slen+2)
175+
byte_offset[1] = byte_offset[1] + 1 + slen
176+
return ret
177+
else
178+
return nil
179+
end
180+
end
181+
182+
183+
--[[
184+
parse a parameter GetSet reply
185+
--]]
186+
local function parse_param_reply(payload)
187+
local byte_offset = {1}
188+
local value = parse_param_Value(payload, byte_offset)
189+
local default_value = parse_param_Value(payload, byte_offset)
190+
local max_value = parse_param_NumericValue(payload, byte_offset)
191+
local min_value = parse_param_NumericValue(payload, byte_offset)
192+
local name = string.sub(payload, byte_offset[1], #payload)
193+
return name, value, default_value, min_value, max_value
194+
end
195+
196+
local next_param_index = 0
197+
198+
--[[
199+
encode a 16 bit number as a DroneCAN int13
200+
--]]
201+
local function encode_int13(v)
202+
return (v & 0xFF) | (v&0xFF00)<<3
203+
end
204+
205+
local function fetch_param()
206+
local payload, nodeid = param_handle:check_message()
207+
if payload then
208+
local pname, pvalue = parse_param_reply(payload)
209+
if not pname or not pvalue then
210+
gcs:send_text(MAV_SEVERITY.INFO, string.format("param restart loop %u", next_param_index))
211+
next_param_index = -1
212+
elseif type(pvalue) == "string" then
213+
gcs:send_text(MAV_SEVERITY.INFO, string.format("got param reply from %u idx=%u '%s' : %s", nodeid, next_param_index, pname, pvalue))
214+
else
215+
gcs:send_text(MAV_SEVERITY.INFO, string.format("got param reply from %u idx=%u '%s' : %f", nodeid, next_param_index, pname, pvalue))
216+
end
217+
next_param_index = next_param_index + 1
218+
end
219+
param_handle:request(TARGET_NODE:get(), string.pack("H",encode_int13(next_param_index)))
220+
end
221+
222+
--[[
223+
check for new NodeStatus messages
224+
--]]
225+
local function check_node_status()
226+
local payload, nodeid = nodestatus_handle:check_message()
227+
if not payload then
228+
return
229+
end
230+
local uptime_sec, bits, _ = string.unpack("IBH", payload)
231+
local health = bits&3
232+
local mode = (bits>>2)&7
233+
local sub_mode = (bits>>5)&7
234+
if not node_status[nodeid] then
235+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Found node %u", nodeid))
236+
end
237+
node_status[nodeid] = { uptime_sec=uptime_sec, health=health, mode=mode, sub_mode=sub_mode }
238+
end
239+
240+
local last_low_rate_ms = uint32_t(0)
241+
242+
local function update()
243+
local now = millis()
244+
if now - last_low_rate_ms >= 1000 then
245+
last_low_rate_ms = now
246+
send_mag_highres()
247+
send_mag_highres2()
248+
check_airspeed()
249+
end
250+
check_node_status()
251+
fetch_param()
252+
253+
return update, 10
254+
end
255+
256+
return update, 1000

0 commit comments

Comments
 (0)