48 ModuleParams(nullptr),
49 WorkItem(MODULE_NAME,
px4::wq_configurations::hp_default),
51 _filter_roll(50.0
f, 10.
f),
52 _filter_pitch(50.0
f, 10.
f),
53 _filter_yaw(50.0
f, 10.
f),
54 _filter_throttle(50.0
f, 10.
f)
57 rc_parameter_map_poll(
true );
70 if (!_input_rc_sub.registerCallback()) {
71 PX4_ERR(
"input_rc callback registration failed!");
79 RCUpdate::parameters_updated()
84 update_rc_functions();
88 RCUpdate::update_rc_functions()
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;
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;
110 _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] =
_parameters.rc_map_flaps - 1;
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;
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;
128 _filter_roll.reset(0.
f);
129 _filter_pitch.reset(0.
f);
130 _filter_yaw.reset(0.
f);
131 _filter_throttle.reset(0.
f);
135 RCUpdate::rc_parameter_map_poll(
bool forced)
137 if (_rc_parameter_map_sub.updated() || forced) {
138 _rc_parameter_map_sub.copy(&_rc_parameter_map);
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]) {
150 if (_rc_parameter_map.param_index[i] >= 0) {
159 PX4_DEBUG(
"rc to parameter map updated");
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",
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]
175 RCUpdate::get_rc_value(uint8_t func,
float min_value,
float max_value)
177 if (_rc.function[func] >= 0) {
178 float value = _rc.channels[_rc.function[func]];
187 RCUpdate::get_rc_sw3pos_position(uint8_t func,
float on_th,
bool on_inv,
float mid_th,
bool mid_inv)
189 if (_rc.function[func] >= 0) {
190 float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
192 if (on_inv ? value < on_th : value > on_th) {
193 return manual_control_setpoint_s::SWITCH_POS_ON;
195 }
else if (mid_inv ? value < mid_th : value > mid_th) {
196 return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
199 return manual_control_setpoint_s::SWITCH_POS_OFF;
203 return manual_control_setpoint_s::SWITCH_POS_NONE;
208 RCUpdate::get_rc_sw2pos_position(uint8_t func,
float on_th,
bool on_inv)
210 if (_rc.function[func] >= 0) {
211 float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
213 if (on_inv ? value < on_th : value > on_th) {
214 return manual_control_setpoint_s::SWITCH_POS_ON;
217 return manual_control_setpoint_s::SWITCH_POS_OFF;
221 return manual_control_setpoint_s::SWITCH_POS_NONE;
226 RCUpdate::set_params_from_rc()
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]) {
236 float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
241 _param_rc_values[i] = rc_val;
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]);
254 _input_rc_sub.unregisterCallback();
262 if (_parameter_update_sub.updated()) {
265 _parameter_update_sub.copy(&pupdate);
269 parameters_updated();
272 rc_parameter_map_poll();
277 if (_input_rc_sub.copy(&rc_input)) {
280 bool signal_lost =
true;
292 int8_t fs_ch = _rc.function[
_parameters.rc_map_failsafe];
315 for (
unsigned int i = 0; i < channel_limit; i++) {
354 _rc.channels[i] = 0.0f;
360 if (!PX4_ISFINITE(_rc.channels[i])) {
361 _rc.channels[i] = 0.0f;
366 _rc.rssi = rc_input.
rssi;
367 _rc.signal_lost = signal_lost;
380 manual.
mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;
383 manual.data_source = manual_control_setpoint_s::SOURCE_RC;
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);
402 manual.z =
math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f);
406 const int num_slots = manual_control_setpoint_s::MODE_SLOT_NUM;
411 const float slot_width_half = 2.0f / num_slots / 2.0f;
414 const float slot_min = -1.0f - 0.05f;
415 const float slot_max = 1.0f + 0.05f;
422 manual.mode_slot = (((((_rc.channels[
_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) /
423 (slot_max - slot_min)) + (1.0
f / num_slots)) + 1;
425 if (manual.mode_slot > num_slots) {
426 manual.mode_slot = num_slots;
431 manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE,
_parameters.rc_auto_th,
433 manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
436 manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL,
_parameters.rc_posctl_th,
438 manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN,
_parameters.rc_return_th,
440 manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER,
_parameters.rc_loiter_th,
442 manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO,
_parameters.rc_acro_th,
444 manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
446 manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
448 manual.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH,
450 manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
452 manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
454 manual.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB,
456 manual.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN,
460 _manual_control_pub.publish(manual);
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;
477 _actuator_group_3_pub.publish(actuator_group_3);
481 set_params_from_rc();
491 RCUpdate::task_spawn(
int argc,
char *argv[])
496 _object.store(instance);
497 _task_id = task_id_is_work_queue;
499 if (instance->init()) {
504 PX4_ERR(
"alloc failed");
508 _object.store(
nullptr);
524 RCUpdate::custom_command(
int argc,
char *argv[])
533 PX4_WARN(
"%s\n", reason);
536 PRINT_MODULE_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`. 544 To reduce control latency, the module is scheduled on input_rc publications. 548 PRINT_MODULE_USAGE_NAME("rc_update",
"system");
549 PRINT_MODULE_USAGE_COMMAND(
"start");
550 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
static constexpr unsigned RC_MAX_CHAN_COUNT
maximum number of r/c channels we handle
measure the time elapsed performing an event
struct uart_esc::@19 _parameter_handles
static RcInput * rc_input
int main(int argc, char **argv)
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
static void print_usage()
__EXPORT int rc_update_main(int argc, char *argv[])
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters)
Read out the parameters using the handles into the parameters struct.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
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.
void initialize_parameter_handles(ParameterHandles ¶meter_handles)
initialize ParameterHandles struct
struct uart_esc::@18 _parameters
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
__EXPORT param_t param_for_used_index(unsigned index)
Look up an used parameter by index.
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).