PX4 Firmware
PX4 Autopilot Software http://px4.io
rc_update.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016-2019 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 rc_update.cpp
36  *
37  * @author Beat Kueng <beat-kueng@gmx.net>
38  */
39 
40 #include "rc_update.h"
41 
42 #include "parameters.h"
43 
44 namespace RCUpdate
45 {
46 
48  ModuleParams(nullptr),
49  WorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
50  _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)),
51  _filter_roll(50.0f, 10.f), /* get replaced by parameter */
52  _filter_pitch(50.0f, 10.f),
53  _filter_yaw(50.0f, 10.f),
54  _filter_throttle(50.0f, 10.f)
55 {
57  rc_parameter_map_poll(true /* forced */);
58 
59  parameters_updated();
60 }
61 
62 RCUpdate::~RCUpdate()
63 {
64  perf_free(_loop_perf);
65 }
66 
67 bool
69 {
70  if (!_input_rc_sub.registerCallback()) {
71  PX4_ERR("input_rc callback registration failed!");
72  return false;
73  }
74 
75  return true;
76 }
77 
78 void
79 RCUpdate::parameters_updated()
80 {
81  /* read the parameter values into _parameters */
83 
84  update_rc_functions();
85 }
86 
87 void
88 RCUpdate::update_rc_functions()
89 {
90  /* update RC function mappings */
91  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
92  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
93  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
94  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
95 
96  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
97  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
98  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw - 1;
99  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
100  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
101  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
102  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
103  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1;
104  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH] = _parameters.rc_map_arm_sw - 1;
105  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1;
106  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _parameters.rc_map_gear_sw - 1;
107  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_STAB] = _parameters.rc_map_stab_sw - 1;
108  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MAN] = _parameters.rc_map_man_sw - 1;
109 
110  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
111 
112  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
113  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
114  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
115  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
116  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
117  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6] = _parameters.rc_map_aux6 - 1;
118 
119  for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
120  _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
121  }
122 
123  /* update the RC low pass filter frequencies */
124  _filter_roll.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff);
125  _filter_pitch.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff);
126  _filter_yaw.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff);
127  _filter_throttle.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff);
128  _filter_roll.reset(0.f);
129  _filter_pitch.reset(0.f);
130  _filter_yaw.reset(0.f);
131  _filter_throttle.reset(0.f);
132 }
133 
134 void
135 RCUpdate::rc_parameter_map_poll(bool forced)
136 {
137  if (_rc_parameter_map_sub.updated() || forced) {
138  _rc_parameter_map_sub.copy(&_rc_parameter_map);
139 
140  /* update parameter handles to which the RC channels are mapped */
141  for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
142  if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
143  /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
144  * or no request to map this channel to a param has been sent via mavlink
145  */
146  continue;
147  }
148 
149  /* Set the handle by index if the index is set, otherwise use the id */
150  if (_rc_parameter_map.param_index[i] >= 0) {
151  _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
152 
153  } else {
154  _parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]);
155  }
156 
157  }
158 
159  PX4_DEBUG("rc to parameter map updated");
160 
161  for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
162  PX4_DEBUG("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
163  i,
164  &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)],
165  (double)_rc_parameter_map.scale[i],
166  (double)_rc_parameter_map.value0[i],
167  (double)_rc_parameter_map.value_min[i],
168  (double)_rc_parameter_map.value_max[i]
169  );
170  }
171  }
172 }
173 
174 float
175 RCUpdate::get_rc_value(uint8_t func, float min_value, float max_value)
176 {
177  if (_rc.function[func] >= 0) {
178  float value = _rc.channels[_rc.function[func]];
179  return math::constrain(value, min_value, max_value);
180 
181  } else {
182  return 0.0f;
183  }
184 }
185 
187 RCUpdate::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
188 {
189  if (_rc.function[func] >= 0) {
190  float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
191 
192  if (on_inv ? value < on_th : value > on_th) {
193  return manual_control_setpoint_s::SWITCH_POS_ON;
194 
195  } else if (mid_inv ? value < mid_th : value > mid_th) {
196  return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
197 
198  } else {
199  return manual_control_setpoint_s::SWITCH_POS_OFF;
200  }
201 
202  } else {
203  return manual_control_setpoint_s::SWITCH_POS_NONE;
204  }
205 }
206 
208 RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
209 {
210  if (_rc.function[func] >= 0) {
211  float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
212 
213  if (on_inv ? value < on_th : value > on_th) {
214  return manual_control_setpoint_s::SWITCH_POS_ON;
215 
216  } else {
217  return manual_control_setpoint_s::SWITCH_POS_OFF;
218  }
219 
220  } else {
221  return manual_control_setpoint_s::SWITCH_POS_NONE;
222  }
223 }
224 
225 void
226 RCUpdate::set_params_from_rc()
227 {
228  for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
229  if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
230  /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
231  * or no request to map this channel to a param has been sent via mavlink
232  */
233  continue;
234  }
235 
236  float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
237 
238  /* Check if the value has changed,
239  * maybe we need to introduce a more aggressive limit here */
240  if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) {
241  _param_rc_values[i] = rc_val;
242  float param_val = math::constrain(
243  _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
244  _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
245  param_set(_parameter_handles.rc_param[i], &param_val);
246  }
247  }
248 }
249 
250 void
251 RCUpdate::Run()
252 {
253  if (should_exit()) {
254  _input_rc_sub.unregisterCallback();
255  exit_and_cleanup();
256  return;
257  }
258 
259  perf_begin(_loop_perf);
260 
261  // check for parameter updates
262  if (_parameter_update_sub.updated()) {
263  // clear update
264  parameter_update_s pupdate;
265  _parameter_update_sub.copy(&pupdate);
266 
267  // update parameters from storage
268  updateParams();
269  parameters_updated();
270  }
271 
272  rc_parameter_map_poll();
273 
274  /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
276 
277  if (_input_rc_sub.copy(&rc_input)) {
278 
279  /* detect RC signal loss */
280  bool signal_lost = true;
281 
282  /* check flags and require at least four channels to consider the signal valid */
283  if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) {
284  /* signal is lost or no enough channels */
285  signal_lost = true;
286 
287  } else {
288  /* signal looks good */
289  signal_lost = false;
290 
291  /* check failsafe */
292  int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle
293 
294  if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping
295  fs_ch = _parameters.rc_map_failsafe - 1;
296  }
297 
298  if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) {
299  /* failsafe configured */
300  if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) ||
301  (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) {
302  /* failsafe triggered, signal is lost by receiver */
303  signal_lost = true;
304  }
305  }
306  }
307 
308  unsigned channel_limit = rc_input.channel_count;
309 
310  if (channel_limit > RC_MAX_CHAN_COUNT) {
311  channel_limit = RC_MAX_CHAN_COUNT;
312  }
313 
314  /* read out and scale values from raw message even if signal is invalid */
315  for (unsigned int i = 0; i < channel_limit; i++) {
316 
317  /*
318  * 1) Constrain to min/max values, as later processing depends on bounds.
319  */
320  if (rc_input.values[i] < _parameters.min[i]) {
321  rc_input.values[i] = _parameters.min[i];
322  }
323 
324  if (rc_input.values[i] > _parameters.max[i]) {
325  rc_input.values[i] = _parameters.max[i];
326  }
327 
328  /*
329  * 2) Scale around the mid point differently for lower and upper range.
330  *
331  * This is necessary as they don't share the same endpoints and slope.
332  *
333  * First normalize to 0..1 range with correct sign (below or above center),
334  * the total range is 2 (-1..1).
335  * If center (trim) == min, scale to 0..1, if center (trim) == max,
336  * scale to -1..0.
337  *
338  * As the min and max bounds were enforced in step 1), division by zero
339  * cannot occur, as for the case of center == min or center == max the if
340  * statement is mutually exclusive with the arithmetic NaN case.
341  *
342  * DO NOT REMOVE OR ALTER STEP 1!
343  */
344  if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
345  _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(
346  _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
347 
348  } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
349  _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(
350  _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
351 
352  } else {
353  /* in the configured dead zone, output zero */
354  _rc.channels[i] = 0.0f;
355  }
356 
357  _rc.channels[i] *= _parameters.rev[i];
358 
359  /* handle any parameter-induced blowups */
360  if (!PX4_ISFINITE(_rc.channels[i])) {
361  _rc.channels[i] = 0.0f;
362  }
363  }
364 
365  _rc.channel_count = rc_input.channel_count;
366  _rc.rssi = rc_input.rssi;
367  _rc.signal_lost = signal_lost;
368  _rc.timestamp = rc_input.timestamp_last_signal;
369  _rc.frame_drop_count = rc_input.rc_lost_frame_count;
370 
371  /* publish rc_channels topic even if signal is invalid, for debug */
372  _rc_pub.publish(_rc);
373 
374  /* only publish manual control if the signal is still present and was present once */
375  if (!signal_lost && rc_input.timestamp_last_signal > 0) {
376 
377  /* initialize manual setpoint */
378  manual_control_setpoint_s manual{};
379  /* set mode slot to unassigned */
380  manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;
381  /* set the timestamp to the last signal time */
382  manual.timestamp = rc_input.timestamp_last_signal;
383  manual.data_source = manual_control_setpoint_s::SOURCE_RC;
384 
385  /* limit controls */
386  manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
387  manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
388  manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
389  manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
390  manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
391  manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
392  manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
393  manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
394  manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
395  manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
396  manual.aux6 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6, -1.0, 1.0);
397 
398  /* filter controls */
399  manual.y = math::constrain(_filter_roll.apply(manual.y), -1.f, 1.f);
400  manual.x = math::constrain(_filter_pitch.apply(manual.x), -1.f, 1.f);
401  manual.r = math::constrain(_filter_yaw.apply(manual.r), -1.f, 1.f);
402  manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f);
403 
404  if (_parameters.rc_map_flightmode > 0) {
405  /* number of valid slots */
406  const int num_slots = manual_control_setpoint_s::MODE_SLOT_NUM;
407 
408  /* the half width of the range of a slot is the total range
409  * divided by the number of slots, again divided by two
410  */
411  const float slot_width_half = 2.0f / num_slots / 2.0f;
412 
413  /* min is -1, max is +1, range is 2. We offset below min and max */
414  const float slot_min = -1.0f - 0.05f;
415  const float slot_max = 1.0f + 0.05f;
416 
417  /* the slot gets mapped by first normalizing into a 0..1 interval using min
418  * and max. Then the right slot is obtained by multiplying with the number of
419  * slots. And finally we add half a slot width to ensure that integer rounding
420  * will take us to the correct final index.
421  */
422  manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) /
423  (slot_max - slot_min)) + (1.0f / num_slots)) + 1;
424 
425  if (manual.mode_slot > num_slots) {
426  manual.mode_slot = num_slots;
427  }
428  }
429 
430  /* mode switches */
431  manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
432  _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
433  manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
434  _parameters.rc_rattitude_th,
435  _parameters.rc_rattitude_inv);
436  manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
437  _parameters.rc_posctl_inv);
438  manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,
439  _parameters.rc_return_inv);
440  manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th,
441  _parameters.rc_loiter_inv);
442  manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th,
443  _parameters.rc_acro_inv);
444  manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
445  _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
446  manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
447  _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
448  manual.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH,
449  _parameters.rc_armswitch_th, _parameters.rc_armswitch_inv);
450  manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
451  _parameters.rc_trans_th, _parameters.rc_trans_inv);
452  manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
453  _parameters.rc_gear_th, _parameters.rc_gear_inv);
454  manual.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB,
455  _parameters.rc_stab_th, _parameters.rc_stab_inv);
456  manual.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN,
457  _parameters.rc_man_th, _parameters.rc_man_inv);
458 
459  /* publish manual_control_setpoint topic */
460  _manual_control_pub.publish(manual);
461 
462  /* copy from mapped manual control to control group 3 */
463  actuator_controls_s actuator_group_3{};
464 
465  actuator_group_3.timestamp = rc_input.timestamp_last_signal;
466 
467  actuator_group_3.control[0] = manual.y;
468  actuator_group_3.control[1] = manual.x;
469  actuator_group_3.control[2] = manual.r;
470  actuator_group_3.control[3] = manual.z;
471  actuator_group_3.control[4] = manual.flaps;
472  actuator_group_3.control[5] = manual.aux1;
473  actuator_group_3.control[6] = manual.aux2;
474  actuator_group_3.control[7] = manual.aux3;
475 
476  /* publish actuator_controls_3 topic */
477  _actuator_group_3_pub.publish(actuator_group_3);
478 
479  /* Update parameters from RC Channels (tuning with RC) if activated */
480  if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1e6) {
481  set_params_from_rc();
482  _last_rc_to_param_map_time = hrt_absolute_time();
483  }
484  }
485  }
486 
487  perf_end(_loop_perf);
488 }
489 
490 int
491 RCUpdate::task_spawn(int argc, char *argv[])
492 {
493  RCUpdate *instance = new RCUpdate();
494 
495  if (instance) {
496  _object.store(instance);
497  _task_id = task_id_is_work_queue;
498 
499  if (instance->init()) {
500  return PX4_OK;
501  }
502 
503  } else {
504  PX4_ERR("alloc failed");
505  }
506 
507  delete instance;
508  _object.store(nullptr);
509  _task_id = -1;
510 
511  return PX4_ERROR;
512 }
513 
514 int
516 {
517  PX4_INFO("Running");
518  perf_print_counter(_loop_perf);
519 
520  return 0;
521 }
522 
523 int
524 RCUpdate::custom_command(int argc, char *argv[])
525 {
526  return print_usage("unknown command");
527 }
528 
529 int
530 RCUpdate::print_usage(const char *reason)
531 {
532  if (reason) {
533  PX4_WARN("%s\n", reason);
534  }
535 
536  PRINT_MODULE_DESCRIPTION(
537  R"DESCR_STR(
538 ### Description
539 The rc_update module handles RC channel mapping: read the raw input channels (`input_rc`),
540 then apply the calibration, map the RC channels to the configured channels & mode switches,
541 low-pass filter, and then publish as `rc_channels` and `manual_control_setpoint`.
542 
543 ### Implementation
544 To reduce control latency, the module is scheduled on input_rc publications.
545 
546 )DESCR_STR");
547 
548  PRINT_MODULE_USAGE_NAME("rc_update", "system");
549  PRINT_MODULE_USAGE_COMMAND("start");
550  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
551 
552  return 0;
553 }
554 
555 } // namespace RCUpdate
556 
557 extern "C" __EXPORT int rc_update_main(int argc, char *argv[])
558 {
559  return RCUpdate::RCUpdate::main(argc, argv);
560 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
static constexpr unsigned RC_MAX_CHAN_COUNT
maximum number of r/c channels we handle
Definition: parameters.h:54
int32_t rssi
Definition: input_rc.h:72
measure the time elapsed performing an event
Definition: perf_counter.h:56
struct uart_esc::@19 _parameter_handles
bool rc_failsafe
Definition: input_rc.h:77
static RcInput * rc_input
Definition: linux_sbus.h:117
int main(int argc, char **argv)
Definition: main.cpp:3
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
Definition: parameters.cpp:814
Definition: I2C.hpp:51
void print_status()
Definition: Commander.cpp:517
static void print_usage()
LidarLite * instance
Definition: ll40ls.cpp:65
#define FLT_EPSILON
uint16_t rc_lost_frame_count
Definition: input_rc.h:73
__EXPORT int rc_update_main(int argc, char *argv[])
Definition: rc_update.cpp:557
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
uint16_t values[18]
Definition: input_rc.h:76
#define perf_alloc(a, b)
Definition: px4io.h:59
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
Read out the parameters using the handles into the parameters struct.
Definition: parameters.cpp:142
uint32_t channel_count
Definition: input_rc.h:71
uint8_t switch_pos_t
Definition: uORB.h:261
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
void perf_end(perf_counter_t handle)
End a performance event.
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
void initialize_parameter_handles(ParameterHandles &parameter_handles)
initialize ParameterHandles struct
Definition: parameters.cpp:45
bool rc_lost
Definition: input_rc.h:78
struct uart_esc::@18 _parameters
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
Definition: bst.cpp:62
__EXPORT param_t param_for_used_index(unsigned index)
Look up an used parameter by index.
Definition: parameters.cpp:420
void perf_begin(perf_counter_t handle)
Begin a performance event.
uint64_t timestamp_last_signal
Definition: input_rc.h:70
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
class RCUpdate
Definition: rc_update.h:72