PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_parameters.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015-2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mavlink_parameters.cpp
36  * Mavlink parameters manager implementation.
37  *
38  * @author Anton Babushkin <anton.babushkin@me.com>
39  * @author Lorenz Meier <lorenz@px4.io>
40  * @author Beat Kueng <beat@px4.io>
41  */
42 
43 #include <stdio.h>
44 
45 #include "mavlink_parameters.h"
46 #include "mavlink_main.h"
47 
49  _mavlink(mavlink)
50 {
51 }
52 
53 unsigned
55 {
56  return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
57 }
58 
59 void
61 {
62  switch (msg->msgid) {
63  case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
64  /* request all parameters */
65  mavlink_param_request_list_t req_list;
66  mavlink_msg_param_request_list_decode(msg, &req_list);
67 
68  if (req_list.target_system == mavlink_system.sysid &&
69  (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
70  if (_send_all_index < 0) {
72 
73  } else {
74  /* a restart should skip the hash check on the ground */
75  _send_all_index = 0;
76  }
77  }
78 
79  if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
80  (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
81  // publish list request to UAVCAN driver via uORB.
83  req.message_type = msg->msgid;
84  req.node_id = req_list.target_component;
85  req.param_index = 0;
86 
88  }
89 
90  break;
91  }
92 
93  case MAVLINK_MSG_ID_PARAM_SET: {
94  /* set parameter */
95  mavlink_param_set_t set;
96  mavlink_msg_param_set_decode(msg, &set);
97 
98  if (set.target_system == mavlink_system.sysid &&
99  (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
100 
101  /* local name buffer to enforce null-terminated string */
102  char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
103  strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
104  /* enforce null termination */
105  name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
106 
107  /* Whatever the value is, we're being told to stop sending */
108  if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) {
109 
110  if (_mavlink->hash_check_enabled()) {
111  _send_all_index = -1;
112  }
113 
114  /* No other action taken, return */
115  return;
116  }
117 
118  /* attempt to find parameter, set and send it */
119  param_t param = param_find_no_notification(name);
120 
121  if (param == PARAM_INVALID) {
122  char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
123  sprintf(buf, "[pm] unknown param: %s", name);
125 
126  } else if (!((param_type(param) == PARAM_TYPE_INT32 && set.param_type == MAV_PARAM_TYPE_INT32) ||
127  (param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32))) {
128  char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
129  sprintf(buf, "[pm] param types mismatch param: %s", name);
131 
132  } else {
133  // According to the mavlink spec we should always acknowledge a write operation.
134  param_set(param, &(set.param_value));
135  send_param(param);
136  }
137  }
138 
139  if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
140  (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
141  // publish set request to UAVCAN driver via uORB.
143  req.message_type = msg->msgid;
144  req.node_id = set.target_component;
145  req.param_index = -1;
146  strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
147  req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
148 
149  if (set.param_type == MAV_PARAM_TYPE_REAL32) {
150  req.param_type = MAV_PARAM_TYPE_REAL32;
151  req.real_value = set.param_value;
152 
153  } else {
154  int32_t val;
155  memcpy(&val, &set.param_value, sizeof(int32_t));
156  req.param_type = MAV_PARAM_TYPE_INT64;
157  req.int_value = val;
158  }
159 
161  }
162 
163  break;
164  }
165 
166  case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
167  /* request one parameter */
168  mavlink_param_request_read_t req_read;
169  mavlink_msg_param_request_read_decode(msg, &req_read);
170 
171  if (req_read.target_system == mavlink_system.sysid &&
172  (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
173 
174  /* when no index is given, loop through string ids and compare them */
175  if (req_read.param_index < 0) {
176  /* XXX: I left this in so older versions of QGC wouldn't break */
177  if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
178  /* return hash check for cached params */
179  uint32_t hash = param_hash_check();
180 
181  /* build the one-off response message */
182  mavlink_param_value_t param_value;
183  param_value.param_count = param_count_used();
184  param_value.param_index = -1;
185  strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
186  param_value.param_type = MAV_PARAM_TYPE_UINT32;
187  memcpy(&param_value.param_value, &hash, sizeof(hash));
188  mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
189 
190  } else {
191  /* local name buffer to enforce null-terminated string */
192  char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
193  strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
194  /* enforce null termination */
195  name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
196  /* attempt to find parameter and send it */
198  }
199 
200  } else {
201  /* when index is >= 0, send this parameter again */
202  int ret = send_param(param_for_used_index(req_read.param_index));
203 
204  if (ret == 1) {
205  char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
206  sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
208 
209  } else if (ret == 2) {
210  char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
211  sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index);
213  }
214  }
215  }
216 
217  if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
218  (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
219  // publish set request to UAVCAN driver via uORB.
222  req.message_type = msg->msgid;
223  req.node_id = req_read.target_component;
224  req.param_index = req_read.param_index;
225  strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
226  req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
227 
228  // Enque the request and forward the first to the uavcan node
229  enque_uavcan_request(&req);
231  }
232 
233  break;
234  }
235 
236  case MAVLINK_MSG_ID_PARAM_MAP_RC: {
237  /* map a rc channel to a parameter */
238  mavlink_param_map_rc_t map_rc;
239  mavlink_msg_param_map_rc_decode(msg, &map_rc);
240 
241  if (map_rc.target_system == mavlink_system.sysid &&
242  (map_rc.target_component == mavlink_system.compid ||
243  map_rc.target_component == MAV_COMP_ID_ALL)) {
244 
245  /* Copy values from msg to uorb using the parameter_rc_channel_index as index */
246  size_t i = map_rc.parameter_rc_channel_index;
247  _rc_param_map.param_index[i] = map_rc.param_index;
248  strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id,
249  MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
250  /* enforce null termination */
251  _rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
252  _rc_param_map.scale[i] = map_rc.scale;
253  _rc_param_map.value0[i] = map_rc.param_value0;
254  _rc_param_map.value_min[i] = map_rc.param_value_min;
255  _rc_param_map.value_max[i] = map_rc.param_value_max;
256 
257  if (map_rc.param_index == -2) { // -2 means unset map
258  _rc_param_map.valid[i] = false;
259 
260  } else {
261  _rc_param_map.valid[i] = true;
262  }
263 
266  }
267 
268  break;
269  }
270 
271  default:
272  break;
273  }
274 }
275 
276 void
278 {
279  int max_num_to_send;
280 
282  max_num_to_send = 3;
283 
284  } else {
285  // speed up parameter loading via UDP or USB: try to send 20 at once
286  max_num_to_send = 20;
287  }
288 
289  int i = 0;
290 
291  // Send while burst is not exceeded, we still have buffer space and still something to send
292  while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && send_params()) {}
293 }
294 
295 bool
297 {
298  if (send_uavcan()) {
299  return true;
300 
301  } else if (send_one()) {
302  return true;
303 
304  } else if (send_untransmitted()) {
305  return true;
306 
307  } else {
308  return false;
309  }
310 }
311 
312 bool
314 {
315  bool sent_one = false;
316 
318  // Clear the ready flag
319  parameter_update_s value;
321 
322  // Schedule an update if not already the case
323  if (_param_update_time == 0) {
326  }
327  }
328 
329  if ((_param_update_time != 0) && ((_param_update_time + 5 * 1000) < hrt_absolute_time())) {
330 
331  param_t param = 0;
332 
333  // send out all changed values
334  do {
335  // skip over all parameters which are not invalid and not used
336  do {
339  } while (param != PARAM_INVALID && !param_used(param));
340 
341  // send parameters which are untransmitted while there is
342  // space in the TX buffer
343  if ((param != PARAM_INVALID) && param_value_unsaved(param)) {
344  int ret = send_param(param);
345  char buf[100];
346  strncpy(&buf[0], param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
347  sent_one = true;
348 
349  if (ret != PX4_OK) {
350  break;
351  }
352  }
353  } while ((_mavlink->get_free_tx_buf() >= get_size()) && (_param_update_index < (int) param_count()));
354 
355  // Flag work as done once all params have been sent
356  if (_param_update_index >= (int) param_count()) {
357  _param_update_time = 0;
358  }
359  }
360 
361  return sent_one;
362 }
363 
364 bool
366 {
367  /* Send parameter values received from the UAVCAN topic */
368  uavcan_parameter_value_s value{};
369 
370  if (_uavcan_parameter_value_sub.update(&value)) {
371 
372  // Check if we received a matching parameter, drop it from the list and request the next
373  if ((_uavcan_open_request_list != nullptr)
374  && (value.param_index == _uavcan_open_request_list->req.param_index)
375  && (value.node_id == _uavcan_open_request_list->req.node_id)) {
376 
379  }
380 
381  mavlink_param_value_t msg{};
382  msg.param_count = value.param_count;
383  msg.param_index = value.param_index;
384 #if defined(__GNUC__) && __GNUC__ >= 8
385 #pragma GCC diagnostic ignored "-Wstringop-truncation"
386 #endif
387  /*
388  * coverity[buffer_size_warning : FALSE]
389  *
390  * The MAVLink spec does not require the string to be NUL-terminated if it
391  * has length 16. In this case the receiving end needs to terminate it
392  * when copying it.
393  */
394  strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
395 #if defined(__GNUC__) && __GNUC__ >= 8
396 #pragma GCC diagnostic pop
397 #endif
398 
399  if (value.param_type == MAV_PARAM_TYPE_REAL32) {
400  msg.param_type = MAVLINK_TYPE_FLOAT;
401  msg.param_value = value.real_value;
402 
403  } else {
404  int32_t val = (int32_t)value.int_value;
405  memcpy(&msg.param_value, &val, sizeof(int32_t));
406  msg.param_type = MAVLINK_TYPE_INT32_T;
407  }
408 
409  // Re-pack the message with the UAVCAN node ID
410  mavlink_message_t mavlink_packet{};
411  mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
412  &msg);
413  _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
414 
415  return true;
416  }
417 
418  return false;
419 }
420 
421 bool
423 {
424  if (_send_all_index >= 0) {
425  /* send all parameters if requested, but only after the system has booted */
426 
427  /* The first thing we send is a hash of all values for the ground
428  * station to try and quickly load a cached copy of our params
429  */
430  if (_send_all_index == PARAM_HASH) {
431  /* return hash check for cached params */
432  uint32_t hash = param_hash_check();
433 
434  /* build the one-off response message */
435  mavlink_param_value_t msg;
436  msg.param_count = param_count_used();
437  msg.param_index = -1;
438  strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
439  msg.param_type = MAV_PARAM_TYPE_UINT32;
440  memcpy(&msg.param_value, &hash, sizeof(hash));
441  mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
442 
443  /* after this we should start sending all params */
444  _send_all_index = 0;
445 
446  /* No further action, return now */
447  return true;
448  }
449 
450  /* look for the first parameter which is used */
451  param_t p;
452 
453  do {
454  /* walk through all parameters, including unused ones */
456  _send_all_index++;
457  } while (p != PARAM_INVALID && !param_used(p));
458 
459  if (p != PARAM_INVALID) {
460  send_param(p);
461  }
462 
463  if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
464  _send_all_index = -1;
465  return false;
466 
467  } else {
468  return true;
469  }
470  }
471 
472  return false;
473 }
474 
475 int
477 {
478  if (param == PARAM_INVALID) {
479  return 1;
480  }
481 
482  /* no free TX buf to send this param */
483  if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
484  return 1;
485  }
486 
487  mavlink_param_value_t msg;
488 
489  /*
490  * get param value, since MAVLink encodes float and int params in the same
491  * space during transmission, copy param onto float val_buf
492  */
493  float param_value{};
494 
495  if (param_get(param, &param_value) != OK) {
496  return 2;
497  }
498 
499  msg.param_value = param_value;
500 
501  msg.param_count = param_count_used();
502  msg.param_index = param_get_used_index(param);
503 
504 #if defined(__GNUC__) && __GNUC__ >= 8
505 #pragma GCC diagnostic push
506 #pragma GCC diagnostic ignored "-Wstringop-truncation"
507 #endif
508  /*
509  * coverity[buffer_size_warning : FALSE]
510  *
511  * The MAVLink spec does not require the string to be NUL-terminated if it
512  * has length 16. In this case the receiving end needs to terminate it
513  * when copying it.
514  */
515  strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
516 #if defined(__GNUC__) && __GNUC__ >= 8
517 #pragma GCC diagnostic pop
518 #endif
519 
520  /* query parameter type */
521  param_type_t type = param_type(param);
522 
523  /*
524  * Map onboard parameter type to MAVLink type,
525  * endianess matches (both little endian)
526  */
527  if (type == PARAM_TYPE_INT32) {
528  msg.param_type = MAVLINK_TYPE_INT32_T;
529 
530  } else if (type == PARAM_TYPE_FLOAT) {
531  msg.param_type = MAVLINK_TYPE_FLOAT;
532 
533  } else {
534  msg.param_type = MAVLINK_TYPE_FLOAT;
535  }
536 
537  /* default component ID */
538  if (component_id < 0) {
539  mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
540 
541  } else {
542  // Re-pack the message with a different component ID
543  mavlink_message_t mavlink_packet;
544  mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink->get_channel(), &mavlink_packet, &msg);
545  _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
546  }
547 
548  return 0;
549 }
550 
552 {
553  // Request a parameter if we are not already waiting on a response and if the list is not empty
556 
558 
560  }
561 }
562 
564 {
565  // We store at max 10 requests to keep memory consumption low.
566  // Dropped requests will be repeated by the ground station
567  if (_uavcan_queued_request_items >= 10) {
568  return;
569  }
570 
572  new_reqest->req = *req;
573  new_reqest->next = nullptr;
574 
577 
578  if (item == nullptr) {
579  // Add the first item to the list
580  _uavcan_open_request_list = new_reqest;
581 
582  } else {
583  // Find the last item and add the new request at the end
584  while (item->next != nullptr) {
585  item = item->next;
586  }
587 
588  item->next = new_reqest;
589  }
590 }
591 
593 {
594  if (_uavcan_open_request_list != nullptr) {
595  // Drop the first item in the list and free the used memory
599  delete first;
601  }
602 }
__EXPORT param_t param_find_no_notification(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:376
int32_t param_value
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
Definition: param.h:103
__EXPORT bool param_value_unsaved(param_t param)
Test whether a parameter&#39;s value has been changed but not saved.
Definition: parameters.cpp:508
#define PARAM_TYPE_INT32
Parameter types.
Definition: param.h:60
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
uint16_t param_type_t
Definition: param.h:66
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
Definition: parameters.cpp:814
bool publish(const T &data)
Publish the struct.
__EXPORT param_t param_for_index(unsigned index)
Look up a parameter by index.
Definition: parameters.cpp:408
__EXPORT int param_get_used_index(param_t param)
Look up the index of an used parameter.
Definition: parameters.cpp:459
__EXPORT unsigned param_count(void)
Return the total number of parameters.
Definition: parameters.cpp:382
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
__EXPORT const char * param_name(param_t param)
Obtain the name of a parameter.
Definition: parameters.cpp:486
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
__EXPORT unsigned param_count_used(void)
Return the actually used number of parameters.
Definition: parameters.cpp:388
__EXPORT param_type_t param_type(param_t param)
Obtain the type of a parameter.
Definition: parameters.cpp:519
bool updated()
Check if there is a new update.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__EXPORT uint32_t param_hash_check(void)
Generate the hash of all parameters and their values.
const char * name
Definition: tests_main.c:58
#define PARAM_TYPE_FLOAT
Definition: param.h:61
#define PARAM_HASH
Magic handle for hash check param.
Definition: param.h:108
#define OK
Definition: uavcan_main.cpp:71
bool update(void *dst)
Update the struct.
__EXPORT param_t param_for_used_index(unsigned index)
Look up an used parameter by index.
Definition: parameters.cpp:420
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Definition: param.h:98
__EXPORT bool param_used(param_t param)
Wether a parameter is in use in the system.
Definition: parameters.cpp:826