Line data Source code
1 : /*
2 : GCS MAVLink functions related to parameter handling
3 :
4 : This program is free software: you can redistribute it and/or modify
5 : it under the terms of the GNU General Public License as published by
6 : the Free Software Foundation, either version 3 of the License, or
7 : (at your option) any later version.
8 :
9 : This program is distributed in the hope that it will be useful,
10 : but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : GNU General Public License for more details.
13 :
14 : You should have received a copy of the GNU General Public License
15 : along with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 :
18 : #include "GCS_config.h"
19 :
20 : #if HAL_GCS_ENABLED
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 :
24 : #include "GCS.h"
25 : #include <AP_Logger/AP_Logger.h>
26 : #include <AP_BoardConfig/AP_BoardConfig.h>
27 :
28 : extern const AP_HAL::HAL& hal;
29 :
30 : // queue of pending parameter requests and replies
31 : ObjectBuffer<GCS_MAVLINK::pending_param_request> GCS_MAVLINK::param_requests(20);
32 : ObjectBuffer<GCS_MAVLINK::pending_param_reply> GCS_MAVLINK::param_replies(5);
33 :
34 : bool GCS_MAVLINK::param_timer_registered;
35 :
36 : /**
37 : * @brief Send the next pending parameter, called from deferred message
38 : * handling code
39 : */
40 : void
41 1625060 : GCS_MAVLINK::queued_param_send()
42 : {
43 : // send parameter async replies
44 1625060 : uint8_t async_replies_sent_count = send_parameter_async_replies();
45 :
46 : // now send the streaming parameters (from PARAM_REQUEST_LIST)
47 1625060 : if (_queued_parameter == nullptr) {
48 : // .... or not....
49 1617356 : return;
50 : }
51 :
52 7704 : const uint32_t tnow = AP_HAL::millis();
53 7704 : const uint32_t tstart = AP_HAL::micros();
54 :
55 : // use at most 30% of bandwidth on parameters
56 7704 : const uint32_t link_bw = _port->bw_in_bytes_per_second();
57 :
58 7704 : uint32_t bytes_allowed = link_bw * (tnow - _queued_parameter_send_time_ms) / 3333;
59 7704 : const uint16_t size_for_one_param_value_msg = MAVLINK_MSG_ID_PARAM_VALUE_LEN + packet_overhead();
60 7704 : if (bytes_allowed < size_for_one_param_value_msg) {
61 66 : bytes_allowed = size_for_one_param_value_msg;
62 : }
63 7704 : if (bytes_allowed > txspace()) {
64 7588 : bytes_allowed = txspace();
65 : }
66 7704 : uint32_t count = bytes_allowed / size_for_one_param_value_msg;
67 :
68 : // when we don't have flow control we really need to keep the
69 : // param download very slow, or it tends to stall
70 7704 : if (!have_flow_control() && count > 5) {
71 679 : count = 5;
72 : }
73 7704 : if (async_replies_sent_count >= count) {
74 6 : return;
75 : }
76 7698 : count -= async_replies_sent_count;
77 :
78 1436585 : while (count && _queued_parameter != nullptr && last_txbuf_is_greater(33)) {
79 : char param_name[AP_MAX_NAME_SIZE];
80 1428887 : _queued_parameter->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
81 :
82 1428887 : mavlink_msg_param_value_send(
83 : chan,
84 : param_name,
85 1428887 : _queued_parameter->cast_to_float(_queued_parameter_type),
86 1428887 : mav_param_type(_queued_parameter_type),
87 1428887 : _queued_parameter_count,
88 1428887 : _queued_parameter_index);
89 :
90 1428887 : _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
91 1428887 : _queued_parameter_index++;
92 :
93 1428887 : if (AP_HAL::micros() - tstart > 1000) {
94 : // don't use more than 1ms sending blocks of parameters
95 0 : break;
96 : }
97 1428887 : count--;
98 : }
99 7698 : _queued_parameter_send_time_ms = tnow;
100 : }
101 :
102 : /*
103 : return true if a channel has flow control
104 : */
105 7704 : bool GCS_MAVLINK::have_flow_control(void)
106 : {
107 7704 : if (_port == nullptr) {
108 0 : return false;
109 : }
110 :
111 7704 : if (_port->flow_control_enabled()) {
112 7012 : return true;
113 : }
114 :
115 692 : if (chan == MAVLINK_COMM_0) {
116 : // assume USB console has flow control
117 0 : return hal.gpio->usb_connected();
118 : }
119 :
120 692 : return false;
121 : }
122 :
123 :
124 : /*
125 : handle a request to change stream rate. Note that copter passes in
126 : save==false so we don't want the save to happen when the user connects the
127 : ground station.
128 : */
129 2124 : void GCS_MAVLINK::handle_request_data_stream(const mavlink_message_t &msg)
130 : {
131 : mavlink_request_data_stream_t packet;
132 2124 : mavlink_msg_request_data_stream_decode(&msg, &packet);
133 :
134 2124 : int16_t freq = 0; // packet frequency
135 :
136 2124 : if (packet.start_stop == 0)
137 0 : freq = 0; // stop sending
138 2124 : else if (packet.start_stop == 1)
139 2124 : freq = packet.req_message_rate; // start sending
140 : else
141 2122 : return;
142 :
143 : // if stream_id is still NUM_STREAMS at the end of this switch
144 : // block then either we set stream rates for all streams, or we
145 : // were asked to set the streamrate for an unrecognised stream
146 2124 : streams stream_id = NUM_STREAMS;
147 2124 : switch (packet.req_stream_id) {
148 2122 : case MAV_DATA_STREAM_ALL:
149 23342 : for (uint8_t i=0; i<NUM_STREAMS; i++) {
150 21220 : if (i == STREAM_PARAMS) {
151 : // don't touch parameter streaming rate; it is
152 : // considered "internal".
153 2122 : continue;
154 : }
155 19098 : if (persist_streamrates()) {
156 6354 : streamRates[i].set_and_save_ifchanged(freq);
157 : } else {
158 12744 : streamRates[i].set(freq);
159 : }
160 19098 : initialise_message_intervals_for_stream((streams)i);
161 : }
162 2122 : break;
163 0 : case MAV_DATA_STREAM_RAW_SENSORS:
164 0 : stream_id = STREAM_RAW_SENSORS;
165 0 : break;
166 0 : case MAV_DATA_STREAM_EXTENDED_STATUS:
167 0 : stream_id = STREAM_EXTENDED_STATUS;
168 0 : break;
169 1 : case MAV_DATA_STREAM_RC_CHANNELS:
170 1 : stream_id = STREAM_RC_CHANNELS;
171 1 : break;
172 0 : case MAV_DATA_STREAM_RAW_CONTROLLER:
173 0 : stream_id = STREAM_RAW_CONTROLLER;
174 0 : break;
175 0 : case MAV_DATA_STREAM_POSITION:
176 0 : stream_id = STREAM_POSITION;
177 0 : break;
178 0 : case MAV_DATA_STREAM_EXTRA1:
179 0 : stream_id = STREAM_EXTRA1;
180 0 : break;
181 0 : case MAV_DATA_STREAM_EXTRA2:
182 0 : stream_id = STREAM_EXTRA2;
183 0 : break;
184 1 : case MAV_DATA_STREAM_EXTRA3:
185 1 : stream_id = STREAM_EXTRA3;
186 1 : break;
187 : }
188 :
189 2124 : if (stream_id == NUM_STREAMS) {
190 : // asked to set rate on unknown stream (or all were set already)
191 2122 : return;
192 : }
193 :
194 2 : AP_Int16 *rate = &streamRates[stream_id];
195 :
196 2 : if (rate != nullptr) {
197 2 : if (persist_streamrates()) {
198 0 : rate->set_and_save_ifchanged(freq);
199 : } else {
200 2 : rate->set(freq);
201 : }
202 2 : initialise_message_intervals_for_stream(stream_id);
203 : }
204 : }
205 :
206 998 : void GCS_MAVLINK::handle_param_request_list(const mavlink_message_t &msg)
207 : {
208 998 : if (!params_ready()) {
209 0 : return;
210 : }
211 :
212 : mavlink_param_request_list_t packet;
213 998 : mavlink_msg_param_request_list_decode(&msg, &packet);
214 :
215 : // requesting parameters is a convenient way to get extra information
216 998 : send_banner();
217 :
218 : // Start sending parameters - next call to ::update will kick the first one out
219 998 : _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
220 998 : _queued_parameter_index = 0;
221 998 : _queued_parameter_count = AP_Param::count_parameters();
222 998 : _queued_parameter_send_time_ms = AP_HAL::millis(); // avoid initial flooding
223 : }
224 :
225 27497 : void GCS_MAVLINK::handle_param_request_read(const mavlink_message_t &msg)
226 : {
227 27497 : if (param_requests.space() == 0) {
228 : // we can't process this right now, drop it
229 2200 : return;
230 : }
231 :
232 : mavlink_param_request_read_t packet;
233 25297 : mavlink_msg_param_request_read_decode(&msg, &packet);
234 :
235 : /*
236 : we reserve some space for sending parameters if the client ever
237 : fails to get a parameter due to lack of space
238 : */
239 25297 : uint32_t saved_reserve_param_space_start_ms = reserve_param_space_start_ms;
240 25297 : reserve_param_space_start_ms = 0; // bypass packet_overhead_chan reservation checking
241 25297 : if (!HAVE_PAYLOAD_SPACE(chan, PARAM_VALUE)) {
242 0 : reserve_param_space_start_ms = AP_HAL::millis();
243 : } else {
244 25297 : reserve_param_space_start_ms = saved_reserve_param_space_start_ms;
245 : }
246 :
247 : struct pending_param_request req;
248 25297 : req.src_system_id = msg.sysid;
249 25297 : req.src_component_id = msg.compid;
250 25297 : req.chan = chan;
251 25297 : req.param_index = packet.param_index;
252 25297 : memcpy(req.param_name, packet.param_id, MIN(sizeof(packet.param_id), sizeof(req.param_name)));
253 25297 : req.param_name[AP_MAX_NAME_SIZE] = 0;
254 :
255 : // queue it for processing by io timer
256 25297 : param_requests.push(req);
257 :
258 : // speaking of which, we'd best make sure it is running:
259 25297 : if (!param_timer_registered) {
260 2082 : param_timer_registered = true;
261 2082 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&GCS_MAVLINK::param_io_timer, void));
262 : }
263 : }
264 :
265 10125 : void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg)
266 : {
267 : mavlink_param_set_t packet;
268 10125 : mavlink_msg_param_set_decode(&msg, &packet);
269 : enum ap_var_type var_type;
270 :
271 : // set parameter
272 : AP_Param *vp;
273 : char key[AP_MAX_NAME_SIZE+1];
274 10125 : strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
275 10125 : key[AP_MAX_NAME_SIZE] = 0;
276 :
277 : // find existing param so we can get the old value
278 10125 : uint16_t parameter_flags = 0;
279 10125 : vp = AP_Param::find(key, &var_type, ¶meter_flags);
280 10125 : if (vp == nullptr) {
281 1 : send_param_error(msg, packet, MAV_PARAM_ERROR_DOES_NOT_EXIST);
282 9 : return;
283 : }
284 10124 : if (isnan(packet.param_value) || isinf(packet.param_value)) {
285 1 : send_param_error(msg, packet, MAV_PARAM_ERROR_VALUE_OUT_OF_RANGE);
286 1 : return;
287 : }
288 :
289 10123 : float old_value = vp->cast_to_float(var_type);
290 :
291 10123 : if (!vp->allow_set_via_mavlink(parameter_flags)) {
292 : // don't warn the user about this failure if we are dropping
293 : // messages here. This is on the assumption that scripting is
294 : // currently responsible for setting parameters and may set
295 : // the value instead of us.
296 7 : if (gcs().get_allow_param_set()) {
297 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param write denied (%s)", key);
298 1 : send_param_error(msg, packet, MAV_PARAM_ERROR_PERMISSION_DENIED);
299 : }
300 : // send the readonly value
301 7 : send_parameter_value(key, var_type, old_value);
302 7 : return;
303 : }
304 :
305 : // set the value
306 10116 : vp->set_float(packet.param_value, var_type);
307 :
308 : /*
309 : we force the save if the value is not equal to the old
310 : value. This copes with the use of override values in
311 : constructors, such as PID elements. Otherwise a set to the
312 : default value which differs from the constructor value doesn't
313 : save the change
314 : */
315 10116 : bool force_save = !is_equal(packet.param_value, old_value);
316 :
317 : // save the change
318 10116 : vp->save(force_save);
319 :
320 10116 : if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) {
321 1301 : AP_Param::invalidate_count();
322 : }
323 :
324 : #if HAL_LOGGING_ENABLED
325 10116 : AP_Logger *logger = AP_Logger::get_singleton();
326 10116 : if (logger != nullptr) {
327 10116 : logger->Write_Parameter(key, vp->cast_to_float(var_type));
328 : }
329 : #endif
330 : }
331 :
332 7 : void GCS_MAVLINK::send_parameter_value(const char *param_name, ap_var_type param_type, float param_value)
333 : {
334 7 : if (!HAVE_PAYLOAD_SPACE(chan, PARAM_VALUE)) {
335 0 : return;
336 : }
337 7 : mavlink_msg_param_value_send(
338 : chan,
339 : param_name,
340 : param_value,
341 7 : mav_param_type(param_type),
342 7 : AP_Param::count_parameters(),
343 : -1);
344 : }
345 :
346 : /*
347 : send a parameter value message to all active MAVLink connections
348 : */
349 87691 : void GCS::send_parameter_value(const char *param_name, ap_var_type param_type, float param_value)
350 : {
351 87691 : mavlink_param_value_t packet{};
352 87691 : const uint8_t to_copy = MIN(ARRAY_SIZE(packet.param_id), strlen(param_name));
353 87691 : memcpy(packet.param_id, param_name, to_copy);
354 87691 : packet.param_value = param_value;
355 87691 : packet.param_type = GCS_MAVLINK::mav_param_type(param_type);
356 87691 : packet.param_count = AP_Param::count_parameters();
357 87691 : packet.param_index = -1;
358 :
359 87691 : gcs().send_to_active_channels(MAVLINK_MSG_ID_PARAM_VALUE,
360 : (const char *)&packet);
361 :
362 : #if HAL_LOGGING_ENABLED
363 : // also log to AP_Logger
364 87691 : AP_Logger *logger = AP_Logger::get_singleton();
365 87691 : if (logger != nullptr) {
366 87691 : logger->Write_Parameter(param_name, param_value);
367 : }
368 : #endif
369 87691 : }
370 :
371 :
372 : /*
373 : timer callback for async parameter requests
374 : */
375 233768128 : void GCS_MAVLINK::param_io_timer(void)
376 : {
377 : struct pending_param_request req;
378 :
379 : // this is mostly a no-op, but doing this here means we won't
380 : // block the main thread counting parameters (~30ms on PH)
381 233768128 : AP_Param::count_parameters();
382 :
383 233768128 : if (param_replies.space() == 0) {
384 : // no room
385 233742831 : return;
386 : }
387 :
388 233532682 : if (!param_requests.pop(req)) {
389 : // nothing to do
390 233507385 : return;
391 : }
392 :
393 : struct pending_param_reply reply;
394 : AP_Param *vp;
395 :
396 25297 : if (req.param_index != -1) {
397 1 : AP_Param::ParamToken token {};
398 1 : vp = AP_Param::find_by_index(req.param_index, &reply.p_type, &token);
399 1 : if (vp != nullptr) {
400 0 : vp->copy_name_token(token, reply.param_name, AP_MAX_NAME_SIZE, true);
401 : } else {
402 1 : memset(reply.param_name, '\0', sizeof(reply.param_name));
403 : }
404 : } else {
405 25296 : strncpy(reply.param_name, req.param_name, AP_MAX_NAME_SIZE+1);
406 25296 : vp = AP_Param::find(req.param_name, &reply.p_type);
407 : }
408 :
409 25297 : reply.chan = req.chan;
410 25297 : reply.src_system_id = req.src_system_id;
411 25297 : reply.src_component_id = req.src_component_id;
412 25297 : reply.param_name[AP_MAX_NAME_SIZE] = 0;
413 25297 : if (vp != nullptr) {
414 25291 : reply.value = vp->cast_to_float(reply.p_type);
415 25291 : reply.param_error = MAV_PARAM_ERROR_NO_ERROR;
416 : } else {
417 6 : reply.value = NaNf;
418 6 : reply.param_error = MAV_PARAM_ERROR_DOES_NOT_EXIST;
419 : }
420 25297 : reply.param_index = req.param_index;
421 25297 : reply.count = AP_Param::count_parameters();
422 :
423 : // queue for transmission
424 25297 : param_replies.push(reply);
425 : }
426 :
427 : /*
428 : A variant of send_param_error which sends based off the received mavlink message information
429 : */
430 3 : void GCS_MAVLINK::send_param_error(const mavlink_message_t &msg, const mavlink_param_set_t ¶m_set, MAV_PARAM_ERROR error)
431 : {
432 3 : if (!HAVE_PAYLOAD_SPACE(chan, PARAM_ERROR)) {
433 0 : return;
434 : }
435 3 : char param_id[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN] {};
436 3 : strncpy_noterm(param_id, param_set.param_id, ARRAY_SIZE(param_id));
437 3 : mavlink_msg_param_error_send(
438 : chan,
439 3 : msg.sysid,
440 3 : msg.compid,
441 : param_id,
442 : -1,
443 : error
444 : );
445 : }
446 :
447 : /*
448 : A variant of send_param_error which is sent based on what the
449 : parameter set thread returns
450 : */
451 6 : void GCS_MAVLINK::send_param_error(const GCS_MAVLINK::pending_param_reply &reply, MAV_PARAM_ERROR error)
452 : {
453 6 : if (!HAVE_PAYLOAD_SPACE(chan, PARAM_ERROR)) {
454 0 : return;
455 : }
456 6 : char param_id[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN] {};
457 6 : strncpy_noterm(param_id, reply.param_name, ARRAY_SIZE(param_id));
458 6 : mavlink_msg_param_error_send(
459 : chan,
460 6 : reply.src_system_id,
461 6 : reply.src_component_id,
462 : param_id,
463 6 : reply.param_index,
464 : error
465 : );
466 : }
467 :
468 : /*
469 : send replies to PARAM_REQUEST_READ
470 : */
471 1625060 : uint8_t GCS_MAVLINK::send_parameter_async_replies()
472 : {
473 1625060 : uint8_t async_replies_sent_count = 0;
474 :
475 1650357 : while (async_replies_sent_count < 5) {
476 : struct pending_param_reply reply;
477 1648500 : if (!param_replies.peek(reply)) {
478 1623203 : return async_replies_sent_count;
479 : }
480 :
481 : uint16_t required_space;
482 25297 : if (reply.param_error == MAV_PARAM_ERROR_NO_ERROR) {
483 25291 : required_space = PAYLOAD_SIZE(chan, PARAM_VALUE);
484 : } else {
485 6 : required_space = PAYLOAD_SIZE(chan, PARAM_ERROR);
486 : }
487 :
488 : /*
489 : we reserve some space for sending parameters if the client ever
490 : fails to get a parameter due to lack of space
491 : */
492 25297 : uint32_t saved_reserve_param_space_start_ms = reserve_param_space_start_ms;
493 25297 : reserve_param_space_start_ms = 0; // bypass packet_overhead_chan reservation checking
494 25297 : if (txspace() < required_space) {
495 0 : out_of_space_to_send();
496 0 : reserve_param_space_start_ms = AP_HAL::millis();
497 0 : return async_replies_sent_count;
498 : }
499 25297 : reserve_param_space_start_ms = saved_reserve_param_space_start_ms;
500 :
501 25297 : if (reply.param_error == MAV_PARAM_ERROR_NO_ERROR) {
502 25291 : mavlink_msg_param_value_send(
503 : reply.chan,
504 : reply.param_name,
505 : reply.value,
506 25291 : mav_param_type(reply.p_type),
507 25291 : reply.count,
508 25291 : reply.param_index);
509 : } else {
510 6 : send_param_error(reply, reply.param_error);
511 : }
512 :
513 25297 : _queued_parameter_send_time_ms = AP_HAL::millis();
514 25297 : async_replies_sent_count++;
515 :
516 25297 : if (!param_replies.pop()) {
517 : // internal error...
518 0 : return async_replies_sent_count;
519 : }
520 : }
521 1857 : return async_replies_sent_count;
522 : }
523 :
524 38620 : void GCS_MAVLINK::handle_common_param_message(const mavlink_message_t &msg)
525 : {
526 38620 : switch (msg.msgid) {
527 998 : case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
528 998 : handle_param_request_list(msg);
529 998 : break;
530 10125 : case MAVLINK_MSG_ID_PARAM_SET:
531 10125 : handle_param_set(msg);
532 10125 : break;
533 27497 : case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
534 27497 : handle_param_request_read(msg);
535 27497 : break;
536 : }
537 38620 : }
538 :
539 : #endif // HAL_GCS_ENABLED
|
