Line data Source code
1 : #include "AP_Scripting_config.h"
2 :
3 : #if AP_SCRIPTING_ENABLED
4 :
5 : #include "AP_Scripting_helpers.h"
6 : #include <AP_Scripting/lua_generated_bindings.h>
7 :
8 : /// Fast param access via pointer helper class
9 :
10 : // Custom lua constructor with optional param name
11 1912 : int lua_new_Parameter(lua_State *L) {
12 :
13 1912 : const int args = lua_gettop(L);
14 1912 : if (args > 1) {
15 0 : return luaL_argerror(L, args, "too many arguments");
16 : }
17 1912 : const char * name = nullptr;
18 1912 : if (args == 1) {
19 1777 : name = luaL_checkstring(L, 1);
20 : }
21 :
22 1912 : auto *p = new_Parameter(L);
23 :
24 1912 : if (args == 0) {
25 : // no arguments, nothing to do
26 135 : return 1;
27 : }
28 :
29 1777 : if (!p->init(name)) {
30 7 : return luaL_error(L, "No parameter: %s", name);
31 : }
32 :
33 1770 : return 1;
34 : }
35 :
36 : // init by name
37 1912 : bool Parameter::init(const char *name)
38 : {
39 1912 : vp = AP_Param::find(name, &vtype);
40 1912 : if (vp == nullptr) {
41 7 : return false;
42 : }
43 1905 : return true;
44 : }
45 :
46 : // init by info, to get the value of old params
47 0 : bool Parameter::init_by_info(uint16_t key, uint32_t group_element, enum ap_var_type type)
48 : {
49 0 : switch (type) {
50 0 : case AP_PARAM_INT8:
51 0 : vp = NEW_NOTHROW AP_Int8;
52 0 : break;
53 0 : case AP_PARAM_INT16:
54 0 : vp = NEW_NOTHROW AP_Int16;
55 0 : break;
56 0 : case AP_PARAM_INT32:
57 0 : vp = NEW_NOTHROW AP_Int32;
58 0 : break;
59 0 : case AP_PARAM_FLOAT:
60 0 : vp = NEW_NOTHROW AP_Float;
61 0 : break;
62 0 : default:
63 0 : return false;
64 : }
65 0 : if (vp == nullptr) {
66 0 : return false;
67 : }
68 0 : vtype = type;
69 0 : AP_Param::ConversionInfo info = {
70 : key,
71 : group_element,
72 : type,
73 : nullptr
74 0 : };
75 0 : return AP_Param::find_old_parameter(&info, vp);
76 : }
77 :
78 : // set a value
79 3855 : bool Parameter::set(float value)
80 : {
81 3855 : if (vp == nullptr) {
82 0 : return false;
83 : }
84 3855 : switch (vtype) {
85 10 : case AP_PARAM_INT8:
86 10 : ((AP_Int8 *)vp)->set(value);
87 10 : return true;
88 0 : case AP_PARAM_INT16:
89 0 : ((AP_Int16 *)vp)->set(value);
90 0 : return true;
91 0 : case AP_PARAM_INT32:
92 0 : ((AP_Int32 *)vp)->set(value);
93 0 : return true;
94 3845 : case AP_PARAM_FLOAT:
95 3845 : ((AP_Float *)vp)->set(value);
96 3845 : return true;
97 0 : default:
98 0 : break;
99 : }
100 : // not a supported type
101 0 : return false;
102 : }
103 :
104 : // get value
105 354665 : bool Parameter::get(float &value)
106 : {
107 354665 : if (vp == nullptr) {
108 0 : return false;
109 : }
110 354665 : switch (vtype) {
111 4334 : case AP_PARAM_INT8:
112 4334 : value = ((AP_Int8 *)vp)->get();
113 4334 : break;
114 17480 : case AP_PARAM_INT16:
115 17480 : value = ((AP_Int16 *)vp)->get();
116 17480 : break;
117 :
118 14 : case AP_PARAM_INT32:
119 14 : value = ((AP_Int32 *)vp)->get();
120 14 : break;
121 :
122 332837 : case AP_PARAM_FLOAT:
123 332837 : value = ((AP_Float *)vp)->get();
124 332837 : break;
125 :
126 0 : default:
127 : // not a supported type
128 0 : return false;
129 : }
130 354665 : return true;
131 : }
132 :
133 : // set and save value
134 15 : bool Parameter::set_and_save(float value)
135 : {
136 15 : if (vp == nullptr) {
137 0 : return false;
138 : }
139 15 : switch (vtype) {
140 0 : case AP_PARAM_INT8:
141 0 : ((AP_Int8 *)vp)->set_and_save(value);
142 0 : return true;
143 0 : case AP_PARAM_INT16:
144 0 : ((AP_Int16 *)vp)->set_and_save(value);
145 0 : return true;
146 0 : case AP_PARAM_INT32:
147 0 : ((AP_Int32 *)vp)->set_and_save(value);
148 0 : return true;
149 15 : case AP_PARAM_FLOAT:
150 15 : ((AP_Float *)vp)->set_and_save(value);
151 15 : return true;
152 0 : default:
153 0 : break;
154 : }
155 : // not a supported type
156 0 : return false;
157 : }
158 :
159 : // Check if param had been configured
160 0 : bool Parameter::configured()
161 : {
162 0 : if (vp == nullptr) {
163 0 : return false;
164 : }
165 0 : return vp->configured();
166 : }
167 :
168 : // set default value
169 0 : bool Parameter::set_default(float value)
170 : {
171 0 : if (vp == nullptr) {
172 0 : return false;
173 : }
174 0 : switch (vtype) {
175 0 : case AP_PARAM_INT8:
176 0 : ((AP_Int8 *)vp)->set_default(value);
177 0 : return true;
178 0 : case AP_PARAM_INT16:
179 0 : ((AP_Int16 *)vp)->set_default(value);
180 0 : return true;
181 0 : case AP_PARAM_INT32:
182 0 : ((AP_Int32 *)vp)->set_default(value);
183 0 : return true;
184 0 : case AP_PARAM_FLOAT:
185 0 : ((AP_Float *)vp)->set_default(value);
186 0 : return true;
187 0 : default:
188 0 : break;
189 : }
190 : // not a supported type
191 0 : return false;
192 : }
193 :
194 : #if HAL_ENABLE_DRONECAN_DRIVERS
195 :
196 : #define IFACE_ALL uint8_t(((1U<<(HAL_NUM_CAN_IFACES))-1U))
197 :
198 : /************************************************
199 : DroneCAN broadcast and request message handling
200 : ************************************************/
201 :
202 : /*
203 : broadcast a message, takes a lua string as payload
204 : */
205 0 : int DroneCAN_Handle::broadcast(lua_State *L)
206 : {
207 0 : binding_argcheck(L, 2);
208 :
209 0 : DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1);
210 :
211 : size_t data_length;
212 0 : const void *data = luaL_checklstring(L, 2, &data_length);
213 :
214 0 : auto &iface = h->dc->get_canard_iface();
215 : Canard::Transfer transfer;
216 :
217 0 : transfer.transfer_type = CanardTransferTypeBroadcast;
218 0 : transfer.data_type_signature = h->signature;
219 0 : transfer.data_type_id = h->data_type;
220 0 : transfer.inout_transfer_id = &h->transfer_id;
221 0 : transfer.priority = 0;
222 0 : transfer.payload = data;
223 0 : transfer.payload_len = data_length;
224 0 : transfer.iface_mask = IFACE_ALL;
225 : #if CANARD_ENABLE_CANFD
226 0 : transfer.canfd = h->canfd;
227 : #endif
228 0 : transfer.timeout_ms = 10;
229 :
230 0 : bool ok = iface.broadcast(transfer);
231 0 : lua_pushboolean(L, ok);
232 :
233 0 : return 1;
234 : }
235 :
236 : /*
237 : send a request message that expects a reply
238 : */
239 0 : int DroneCAN_Handle::request(lua_State *L)
240 : {
241 0 : binding_argcheck(L, 3);
242 :
243 0 : DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1);
244 0 : uint8_t target_node = luaL_checknumber(L, 2);
245 :
246 : size_t data_length;
247 0 : const void *data = luaL_checklstring(L, 3, &data_length);
248 :
249 0 : auto &iface = h->dc->get_canard_iface();
250 :
251 : Canard::Transfer transfer;
252 :
253 0 : transfer.transfer_type = CanardTransferTypeRequest;
254 0 : transfer.data_type_signature = h->signature;
255 0 : transfer.data_type_id = h->data_type;
256 0 : transfer.inout_transfer_id = &h->transfer_id;
257 0 : transfer.priority = 0;
258 0 : transfer.payload = data;
259 0 : transfer.payload_len = data_length;
260 0 : transfer.iface_mask = IFACE_ALL;
261 : #if CANARD_ENABLE_CANFD
262 0 : transfer.canfd = h->canfd;
263 : #endif
264 0 : transfer.timeout_ms = 10;
265 :
266 0 : WITH_SEMAPHORE(iface.get_sem_rx());
267 :
268 0 : if (h->subscriber != nullptr) {
269 0 : delete h->subscriber;
270 : }
271 :
272 0 : h->subscriber = NEW_NOTHROW Subscriber(*h, CanardTransferTypeResponse);
273 0 : bool ok = h->subscriber != nullptr;
274 :
275 0 : if (ok) {
276 0 : h->subscriber->node_id = target_node;
277 0 : ok &= iface.request(target_node, transfer);
278 : }
279 :
280 0 : lua_pushboolean(L, ok);
281 :
282 0 : return 1;
283 0 : }
284 :
285 : /*
286 : subscribe to broadcast messages
287 : */
288 0 : bool DroneCAN_Handle::subscribe(void)
289 : {
290 0 : WITH_SEMAPHORE(dc->get_canard_iface().get_sem_rx());
291 0 : if (subscriber != nullptr) {
292 0 : delete subscriber;
293 : }
294 0 : subscriber = NEW_NOTHROW Subscriber(*this, CanardTransferTypeBroadcast);
295 0 : return subscriber != nullptr;
296 0 : }
297 :
298 0 : DroneCAN_Handle::Subscriber::Subscriber(DroneCAN_Handle &_handle, CanardTransferType _transfer_type) :
299 0 : Canard::HandlerList(_transfer_type, _handle.data_type, _handle.signature, _handle.dc->get_driver_index())
300 : {
301 0 : handle = &_handle;
302 0 : trans_type = _transfer_type;
303 0 : link();
304 0 : }
305 :
306 0 : DroneCAN_Handle::Subscriber::~Subscriber(void)
307 : {
308 0 : unlink();
309 : Payload payload;
310 0 : while (payloads.pop(payload)) {
311 0 : free(payload.data);
312 : }
313 0 : }
314 :
315 : /*
316 : handle an incoming subscribed message
317 : */
318 0 : bool DroneCAN_Handle::Subscriber::handle_message(const CanardRxTransfer& transfer)
319 : {
320 0 : if (transfer.transfer_type == CanardTransferTypeResponse &&
321 0 : (node_id != transfer.source_node_id ||
322 0 : ((transfer.transfer_id+1)&0xFF) != handle->transfer_id)) {
323 : // not from right node, or not right transfer ID
324 0 : return false;
325 : }
326 :
327 0 : if (payloads.space() == 0) {
328 : // already have the max number of messages pending, discard
329 : // this one, but return true to say it was for us
330 0 : return true;
331 : }
332 : Payload payload;
333 0 : payload.data = (uint8_t *)malloc(transfer.payload_len);
334 0 : if (payload.data == nullptr) {
335 : // discard messages if we can't allocate
336 : // but return true to say it was for us
337 0 : return true;
338 : }
339 :
340 0 : payload.length = transfer.payload_len;
341 0 : payload.node_id = transfer.source_node_id;
342 0 : payload.timestamp = transfer.timestamp_usec;
343 : #if CANARD_ENABLE_CANFD
344 0 : payload.canfd = transfer.canfd;
345 : #endif
346 :
347 0 : uint32_t bits_remaining = transfer.payload_len * 8;
348 0 : uint32_t ofs = 0;
349 :
350 : // decode in bytes, we don't have a canardDecodeBuffer function
351 0 : while (bits_remaining > 0) {
352 0 : uint8_t nbits = MIN(8U, bits_remaining);
353 0 : canardDecodeScalar(&transfer, ofs*8, nbits, false, (void*)&payload.data[ofs]);
354 0 : ofs++;
355 0 : bits_remaining -= nbits;
356 : }
357 :
358 : // push to queue
359 0 : payloads.push(payload);
360 :
361 0 : return true;
362 : }
363 :
364 : /*
365 : check for an incoming message
366 : */
367 0 : int DroneCAN_Handle::check_message(lua_State *L)
368 : {
369 : /*
370 : get the receive mutex for this driver
371 : */
372 0 : DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1);
373 :
374 0 : auto *s = h->subscriber;
375 :
376 0 : if (s == nullptr) {
377 0 : return 0;
378 : }
379 : Subscriber::Payload payload;
380 : /*
381 : peek first to avoid a memory leak if lua_pushlstring() faults
382 : */
383 0 : if (!s->payloads.peek(payload)) {
384 0 : return 0;
385 : }
386 :
387 0 : lua_pushlstring(L, (char *)payload.data, payload.length);
388 :
389 0 : s->payloads.pop();
390 0 : free(payload.data);
391 :
392 0 : lua_pushinteger(L, payload.node_id);
393 0 : *new_uint64_t(L) = payload.timestamp;
394 : #if CANARD_ENABLE_CANFD
395 0 : lua_pushboolean(L, payload.canfd);
396 : #else
397 : lua_pushboolean(L, false);
398 : #endif
399 :
400 0 : if (s->trans_type == CanardTransferTypeResponse) {
401 : // request reply removes the subscriber
402 0 : WITH_SEMAPHORE(h->dc->get_canard_iface().get_sem_rx());
403 0 : delete h->subscriber;
404 0 : h->subscriber = nullptr;
405 0 : }
406 :
407 0 : return 4;
408 : }
409 :
410 : /*
411 : garbage collect a handle
412 : */
413 0 : int DroneCAN_Handle::__gc(lua_State *L)
414 : {
415 0 : DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1);
416 0 : WITH_SEMAPHORE(h->dc->get_canard_iface().get_sem_rx());
417 :
418 0 : if (h->subscriber != nullptr) {
419 0 : delete h->subscriber;
420 0 : h->subscriber = nullptr;
421 : }
422 0 : return 0;
423 0 : }
424 :
425 : // lua constructor for DroneCAN_Handle
426 0 : int DroneCAN_Handle::new_handle(lua_State *L)
427 : {
428 0 : lua_Number driver_index = luaL_checknumber(L, 1);
429 0 : const uint64_t sig = *check_uint64_t(L, 2);
430 0 : const uint16_t dtype = luaL_checkinteger(L, 3);
431 : #if CANARD_ENABLE_CANFD
432 0 : const int args = lua_gettop(L);
433 0 : bool send_canfd = false;
434 0 : if (args > 3) {
435 0 : send_canfd = (bool)lua_toboolean(L, 4);
436 : }
437 : #endif
438 :
439 0 : auto *dc = AP_DroneCAN::get_dronecan(driver_index);
440 0 : if (dc == nullptr) {
441 0 : return 0;
442 : }
443 :
444 0 : auto *h = new_DroneCAN_Handle(L);
445 :
446 0 : h->dc = dc;
447 0 : h->signature = sig;
448 0 : h->data_type = dtype;
449 : #if CANARD_ENABLE_CANFD
450 0 : h->canfd = send_canfd;
451 : #endif
452 :
453 0 : return 1;
454 : }
455 :
456 : #endif // HAL_ENABLE_DRONECAN_DRIVERS
457 :
458 : #endif // AP_SCRIPTING_ENABLED
|
