From e0ddb79f0db663b16c640c05279b155b541bc9a1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 3 Dec 2019 11:32:27 -0500 Subject: [PATCH 1/4] rc_update: cleanup parameter usage --- src/modules/rc_update/CMakeLists.txt | 3 +- src/modules/rc_update/parameters.cpp | 325 ------------------ src/modules/rc_update/parameters.h | 207 ----------- .../rc_update/{rc_params.c => params.c} | 0 src/modules/rc_update/rc_update.cpp | 217 ++++++++---- src/modules/rc_update/rc_update.h | 96 +++++- 6 files changed, 230 insertions(+), 618 deletions(-) delete mode 100644 src/modules/rc_update/parameters.cpp delete mode 100644 src/modules/rc_update/parameters.h rename src/modules/rc_update/{rc_params.c => params.c} (100%) diff --git a/src/modules/rc_update/CMakeLists.txt b/src/modules/rc_update/CMakeLists.txt index db99402d108f..c6ba78dcce26 100644 --- a/src/modules/rc_update/CMakeLists.txt +++ b/src/modules/rc_update/CMakeLists.txt @@ -36,7 +36,8 @@ px4_add_module( MAIN rc_update SRCS rc_update.cpp - parameters.cpp + rc_update.h DEPENDS mathlib + px4_work_queue ) diff --git a/src/modules/rc_update/parameters.cpp b/src/modules/rc_update/parameters.cpp deleted file mode 100644 index 9750204e7fc3..000000000000 --- a/src/modules/rc_update/parameters.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file parameters.cpp - * - * @author Beat Kueng - */ - -#include "parameters.h" - -namespace RCUpdate -{ - -void initialize_parameter_handles(ParameterHandles ¶meter_handles) -{ - /* basic r/c parameters */ - for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) { - char nbuf[16]; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - parameter_handles.min[i] = param_find(nbuf); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - parameter_handles.trim[i] = param_find(nbuf); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - parameter_handles.max[i] = param_find(nbuf); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - parameter_handles.rev[i] = param_find(nbuf); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - parameter_handles.dz[i] = param_find(nbuf); - - } - - /* mandatory input switched, mapped to channels 1-4 per default */ - parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); - parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); - parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); - parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); - parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); - - /* mandatory mode switches, mapped to channel 5 and 6 per default */ - parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); - parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); - - parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); - - /* optional mode switches, not mapped per default */ - parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW"); - parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); - parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); - parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); - parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); - parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW"); - parameter_handles.rc_map_arm_sw = param_find("RC_MAP_ARM_SW"); - parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW"); - parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW"); - parameter_handles.rc_map_stab_sw = param_find("RC_MAP_STAB_SW"); - parameter_handles.rc_map_man_sw = param_find("RC_MAP_MAN_SW"); - - parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); - parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); - parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); - parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); - parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - parameter_handles.rc_map_aux6 = param_find("RC_MAP_AUX6"); - - /* RC to parameter mapping for changing parameters with RC */ - for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { - char name[rc_parameter_map_s::PARAM_ID_LEN]; - snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", - i + 1); // shifted by 1 because param name starts at 1 - parameter_handles.rc_map_param[i] = param_find(name); - } - - parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE"); - - /* RC thresholds */ - parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); - parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); - parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); - parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH"); - parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); - parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); - parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); - parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); - parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); - parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH"); - parameter_handles.rc_armswitch_th = param_find("RC_ARMSWITCH_TH"); - parameter_handles.rc_trans_th = param_find("RC_TRANS_TH"); - parameter_handles.rc_gear_th = param_find("RC_GEAR_TH"); - parameter_handles.rc_stab_th = param_find("RC_STAB_TH"); - parameter_handles.rc_man_th = param_find("RC_MAN_TH"); - - /* RC low pass filter configuration */ - parameter_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE"); - parameter_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF"); - - // These are parameters for which QGroundControl always expects to be returned in a list request. - // We do a param_find here to force them into the list. - (void)param_find("RC_CHAN_CNT"); -} - -int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters) -{ - bool rc_valid = true; - float tmpScaleFactor = 0.0f; - float tmpRevFactor = 0.0f; - int ret = PX4_OK; - - /* rc values */ - for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) { - - param_get(parameter_handles.min[i], &(parameters.min[i])); - param_get(parameter_handles.trim[i], &(parameters.trim[i])); - param_get(parameter_handles.max[i], &(parameters.max[i])); - param_get(parameter_handles.rev[i], &(parameters.rev[i])); - param_get(parameter_handles.dz[i], &(parameters.dz[i])); - - tmpScaleFactor = (1.0f / ((parameters.max[i] - parameters.min[i]) / 2.0f) * parameters.rev[i]); - tmpRevFactor = tmpScaleFactor * parameters.rev[i]; - - /* handle blowup in the scaling factor calculation */ - if (!PX4_ISFINITE(tmpScaleFactor) || - (tmpRevFactor < 0.000001f) || - (tmpRevFactor > 0.2f)) { - PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(parameters.rev[i])); - /* scaling factors do not make sense, lock them down */ - parameters.scaling_factor[i] = 0.0f; - rc_valid = false; - - } else { - parameters.scaling_factor[i] = tmpScaleFactor; - } - } - - /* handle wrong values */ - if (!rc_valid) { - PX4_ERR("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - } - - const char *paramerr = "FAIL PARM LOAD"; - - /* channel mapping */ - if (param_get(parameter_handles.rc_map_roll, &(parameters.rc_map_roll)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_pitch, &(parameters.rc_map_pitch)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_yaw, &(parameters.rc_map_yaw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_throttle, &(parameters.rc_map_throttle)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_failsafe, &(parameters.rc_map_failsafe)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_mode_sw, &(parameters.rc_map_mode_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_return_sw, &(parameters.rc_map_return_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_rattitude_sw, &(parameters.rc_map_rattitude_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_posctl_sw, &(parameters.rc_map_posctl_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_loiter_sw, &(parameters.rc_map_loiter_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_acro_sw, &(parameters.rc_map_acro_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_offboard_sw, &(parameters.rc_map_offboard_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_kill_sw, &(parameters.rc_map_kill_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_arm_sw, &(parameters.rc_map_arm_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_trans_sw, &(parameters.rc_map_trans_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_flaps, &(parameters.rc_map_flaps)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_gear_sw, &(parameters.rc_map_gear_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_stab_sw, &(parameters.rc_map_stab_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_man_sw, &(parameters.rc_map_man_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - param_get(parameter_handles.rc_map_aux1, &(parameters.rc_map_aux1)); - param_get(parameter_handles.rc_map_aux2, &(parameters.rc_map_aux2)); - param_get(parameter_handles.rc_map_aux3, &(parameters.rc_map_aux3)); - param_get(parameter_handles.rc_map_aux4, &(parameters.rc_map_aux4)); - param_get(parameter_handles.rc_map_aux5, &(parameters.rc_map_aux5)); - param_get(parameter_handles.rc_map_aux6, &(parameters.rc_map_aux6)); - - for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { - param_get(parameter_handles.rc_map_param[i], &(parameters.rc_map_param[i])); - } - - param_get(parameter_handles.rc_map_flightmode, &(parameters.rc_map_flightmode)); - - param_get(parameter_handles.rc_fails_thr, &(parameters.rc_fails_thr)); - param_get(parameter_handles.rc_assist_th, &(parameters.rc_assist_th)); - parameters.rc_assist_inv = (parameters.rc_assist_th < 0); - parameters.rc_assist_th = fabsf(parameters.rc_assist_th); - param_get(parameter_handles.rc_auto_th, &(parameters.rc_auto_th)); - parameters.rc_auto_inv = (parameters.rc_auto_th < 0); - parameters.rc_auto_th = fabsf(parameters.rc_auto_th); - param_get(parameter_handles.rc_rattitude_th, &(parameters.rc_rattitude_th)); - parameters.rc_rattitude_inv = (parameters.rc_rattitude_th < 0); - parameters.rc_rattitude_th = fabsf(parameters.rc_rattitude_th); - param_get(parameter_handles.rc_posctl_th, &(parameters.rc_posctl_th)); - parameters.rc_posctl_inv = (parameters.rc_posctl_th < 0); - parameters.rc_posctl_th = fabsf(parameters.rc_posctl_th); - param_get(parameter_handles.rc_return_th, &(parameters.rc_return_th)); - parameters.rc_return_inv = (parameters.rc_return_th < 0); - parameters.rc_return_th = fabsf(parameters.rc_return_th); - param_get(parameter_handles.rc_loiter_th, &(parameters.rc_loiter_th)); - parameters.rc_loiter_inv = (parameters.rc_loiter_th < 0); - parameters.rc_loiter_th = fabsf(parameters.rc_loiter_th); - param_get(parameter_handles.rc_acro_th, &(parameters.rc_acro_th)); - parameters.rc_acro_inv = (parameters.rc_acro_th < 0); - parameters.rc_acro_th = fabsf(parameters.rc_acro_th); - param_get(parameter_handles.rc_offboard_th, &(parameters.rc_offboard_th)); - parameters.rc_offboard_inv = (parameters.rc_offboard_th < 0); - parameters.rc_offboard_th = fabsf(parameters.rc_offboard_th); - param_get(parameter_handles.rc_killswitch_th, &(parameters.rc_killswitch_th)); - parameters.rc_killswitch_inv = (parameters.rc_killswitch_th < 0); - parameters.rc_killswitch_th = fabsf(parameters.rc_killswitch_th); - param_get(parameter_handles.rc_armswitch_th, &(parameters.rc_armswitch_th)); - parameters.rc_armswitch_inv = (parameters.rc_armswitch_th < 0); - parameters.rc_armswitch_th = fabsf(parameters.rc_armswitch_th); - param_get(parameter_handles.rc_trans_th, &(parameters.rc_trans_th)); - parameters.rc_trans_inv = (parameters.rc_trans_th < 0); - parameters.rc_trans_th = fabsf(parameters.rc_trans_th); - param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th)); - parameters.rc_gear_inv = (parameters.rc_gear_th < 0); - parameters.rc_gear_th = fabsf(parameters.rc_gear_th); - param_get(parameter_handles.rc_stab_th, &(parameters.rc_stab_th)); - parameters.rc_stab_inv = (parameters.rc_stab_th < 0); - parameters.rc_stab_th = fabsf(parameters.rc_stab_th); - param_get(parameter_handles.rc_man_th, &(parameters.rc_man_th)); - parameters.rc_man_inv = (parameters.rc_man_th < 0); - parameters.rc_man_th = fabsf(parameters.rc_man_th); - - param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate)); - parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate); - param_get(parameter_handles.rc_flt_cutoff, &(parameters.rc_flt_cutoff)); - /* make sure the filter is in its stable region -> fc < fs/2 */ - parameters.rc_flt_cutoff = math::min(parameters.rc_flt_cutoff, (parameters.rc_flt_smp_rate / 2) - 1.f); - - return ret; -} - -} /* namespace RCUpdate */ diff --git a/src/modules/rc_update/parameters.h b/src/modules/rc_update/parameters.h deleted file mode 100644 index ff1718206d97..000000000000 --- a/src/modules/rc_update/parameters.h +++ /dev/null @@ -1,207 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -/** - * @file parameters.h - * - * defines the list of parameters that are used within the RCUpdate module - * - * @author Beat Kueng - */ -#include -#include - -#include -#include - -#include -#include -namespace RCUpdate -{ - -static constexpr unsigned RC_MAX_CHAN_COUNT = - input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ - -struct Parameters { - float min[RC_MAX_CHAN_COUNT]; - float trim[RC_MAX_CHAN_COUNT]; - float max[RC_MAX_CHAN_COUNT]; - float rev[RC_MAX_CHAN_COUNT]; - float dz[RC_MAX_CHAN_COUNT]; - float scaling_factor[RC_MAX_CHAN_COUNT]; - - int32_t rc_map_roll; - int32_t rc_map_pitch; - int32_t rc_map_yaw; - int32_t rc_map_throttle; - int32_t rc_map_failsafe; - - int32_t rc_map_mode_sw; - int32_t rc_map_return_sw; - int32_t rc_map_rattitude_sw; - int32_t rc_map_posctl_sw; - int32_t rc_map_loiter_sw; - int32_t rc_map_acro_sw; - int32_t rc_map_offboard_sw; - int32_t rc_map_kill_sw; - int32_t rc_map_arm_sw; - int32_t rc_map_trans_sw; - int32_t rc_map_gear_sw; - int32_t rc_map_stab_sw; - int32_t rc_map_man_sw; - int32_t rc_map_flaps; - - int32_t rc_map_aux1; - int32_t rc_map_aux2; - int32_t rc_map_aux3; - int32_t rc_map_aux4; - int32_t rc_map_aux5; - int32_t rc_map_aux6; - - int32_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; - - int32_t rc_map_flightmode; - - int32_t rc_fails_thr; - float rc_assist_th; - float rc_auto_th; - float rc_rattitude_th; - float rc_posctl_th; - float rc_return_th; - float rc_loiter_th; - float rc_acro_th; - float rc_offboard_th; - float rc_killswitch_th; - float rc_armswitch_th; - float rc_trans_th; - float rc_gear_th; - float rc_stab_th; - float rc_man_th; - - bool rc_assist_inv; - bool rc_auto_inv; - bool rc_rattitude_inv; - bool rc_posctl_inv; - bool rc_return_inv; - bool rc_loiter_inv; - bool rc_acro_inv; - bool rc_offboard_inv; - bool rc_killswitch_inv; - bool rc_armswitch_inv; - bool rc_trans_inv; - bool rc_gear_inv; - bool rc_stab_inv; - bool rc_man_inv; - - float rc_flt_smp_rate; - float rc_flt_cutoff; -}; - -struct ParameterHandles { - param_t min[RC_MAX_CHAN_COUNT]; - param_t trim[RC_MAX_CHAN_COUNT]; - param_t max[RC_MAX_CHAN_COUNT]; - param_t rev[RC_MAX_CHAN_COUNT]; - param_t dz[RC_MAX_CHAN_COUNT]; - - param_t rc_map_roll; - param_t rc_map_pitch; - param_t rc_map_yaw; - param_t rc_map_throttle; - param_t rc_map_failsafe; - - param_t rc_map_mode_sw; - param_t rc_map_return_sw; - param_t rc_map_rattitude_sw; - param_t rc_map_posctl_sw; - param_t rc_map_loiter_sw; - param_t rc_map_acro_sw; - param_t rc_map_offboard_sw; - param_t rc_map_kill_sw; - param_t rc_map_arm_sw; - param_t rc_map_trans_sw; - param_t rc_map_gear_sw; - param_t rc_map_flaps; - param_t rc_map_stab_sw; - param_t rc_map_man_sw; - - param_t rc_map_aux1; - param_t rc_map_aux2; - param_t rc_map_aux3; - param_t rc_map_aux4; - param_t rc_map_aux5; - param_t rc_map_aux6; - - param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; - param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the parameters which are bound - to a RC channel, equivalent float values in the - _parameters struct are not existing - because these parameters are never read. */ - - param_t rc_map_flightmode; - - param_t rc_fails_thr; - param_t rc_assist_th; - param_t rc_auto_th; - param_t rc_rattitude_th; - param_t rc_posctl_th; - param_t rc_return_th; - param_t rc_loiter_th; - param_t rc_acro_th; - param_t rc_offboard_th; - param_t rc_killswitch_th; - param_t rc_armswitch_th; - param_t rc_trans_th; - param_t rc_gear_th; - param_t rc_stab_th; - param_t rc_man_th; - - param_t rc_flt_smp_rate; - param_t rc_flt_cutoff; -}; - -/** - * initialize ParameterHandles struct - */ -void initialize_parameter_handles(ParameterHandles ¶meter_handles); - - -/** - * Read out the parameters using the handles into the parameters struct. - * @return 0 on success, <0 on error - */ -int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); - -} /* namespace RCUpdate */ diff --git a/src/modules/rc_update/rc_params.c b/src/modules/rc_update/params.c similarity index 100% rename from src/modules/rc_update/rc_params.c rename to src/modules/rc_update/params.c diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index ca828f6d129f..4f9e413b0f54 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -39,7 +39,7 @@ #include "rc_update.h" -#include "parameters.h" +using namespace time_literals; namespace RCUpdate { @@ -47,13 +47,41 @@ namespace RCUpdate RCUpdate::RCUpdate() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)), - _filter_roll(50.0f, 10.f), /* get replaced by parameter */ - _filter_pitch(50.0f, 10.f), - _filter_yaw(50.0f, 10.f), - _filter_throttle(50.0f, 10.f) + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)) { - initialize_parameter_handles(_parameter_handles); + // initialize parameter handles + for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) { + char nbuf[16]; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles.min[i] = param_find(nbuf); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles.trim[i] = param_find(nbuf); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles.max[i] = param_find(nbuf); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles.rev[i] = param_find(nbuf); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles.dz[i] = param_find(nbuf); + } + + // RC to parameter mapping for changing parameters with RC + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + // shifted by 1 because param name starts at 1 + char name[rc_parameter_map_s::PARAM_ID_LEN]; + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); + _parameter_handles.rc_map_param[i] = param_find(name); + } + rc_parameter_map_poll(true /* forced */); parameters_updated(); @@ -78,8 +106,46 @@ RCUpdate::init() void RCUpdate::parameters_updated() { - /* read the parameter values into _parameters */ - update_parameters(_parameter_handles, _parameters); + if (_param_rc_flt_smp_rate.get() < 1.0f) { + _param_rc_flt_smp_rate.set(1.0f); + _param_rc_flt_smp_rate.commit_no_notification(); + } + + // make sure the filter is in its stable region -> fc < fs/2 + const float flt_cutoff_min = _param_rc_flt_smp_rate.get() / 2.0f - 1.0f; + + if (_param_rc_flt_cutoff.get() < flt_cutoff_min) { + _param_rc_flt_cutoff.set(flt_cutoff_min); + _param_rc_flt_cutoff.commit_no_notification(); + } + + // rc values + for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) { + + float min = 0.0f; + param_get(_parameter_handles.min[i], &min); + _parameters.min[i] = min; + + float trim = 0.0f; + param_get(_parameter_handles.trim[i], &trim); + _parameters.trim[i] = trim; + + float max = 0.0f; + param_get(_parameter_handles.max[i], &max); + _parameters.max[i] = max; + + float rev = 0.0f; + param_get(_parameter_handles.rev[i], &rev); + _parameters.rev[i] = rev < 0.0f; + + float dz = 0.0f; + param_get(_parameter_handles.dz[i], &dz); + _parameters.dz[i] = dz; + } + + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); + } update_rc_functions(); } @@ -88,43 +154,43 @@ void RCUpdate::update_rc_functions() { /* update RC function mappings */ - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; - - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH] = _parameters.rc_map_arm_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _parameters.rc_map_gear_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_STAB] = _parameters.rc_map_stab_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MAN] = _parameters.rc_map_man_sw - 1; - - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; - - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6] = _parameters.rc_map_aux6 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _param_rc_map_throttle.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _param_rc_map_roll.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _param_rc_map_pitch.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _param_rc_map_yaw.get() - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _param_rc_map_mode_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _param_rc_map_return_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _param_rc_map_ratt_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _param_rc_map_posctl_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _param_rc_map_loiter_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _param_rc_map_acro_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _param_rc_map_offb_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _param_rc_map_kill_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH] = _param_rc_map_arm_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _param_rc_map_trans_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _param_rc_map_gear_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_STAB] = _param_rc_map_stab_sw.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MAN] = _param_rc_map_man_sw.get() - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _param_rc_map_flaps.get() - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _param_rc_map_aux1.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _param_rc_map_aux2.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _param_rc_map_aux3.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _param_rc_map_aux4.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _param_rc_map_aux5.get() - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6] = _param_rc_map_aux6.get() - 1; for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } /* update the RC low pass filter frequencies */ - _filter_roll.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff); - _filter_pitch.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff); - _filter_yaw.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff); - _filter_throttle.set_cutoff_frequency(_parameters.rc_flt_smp_rate, _parameters.rc_flt_cutoff); + _filter_roll.set_cutoff_frequency(_param_rc_flt_smp_rate.get(), _param_rc_flt_cutoff.get()); + _filter_pitch.set_cutoff_frequency(_param_rc_flt_smp_rate.get(), _param_rc_flt_cutoff.get()); + _filter_yaw.set_cutoff_frequency(_param_rc_flt_smp_rate.get(), _param_rc_flt_cutoff.get()); + _filter_throttle.set_cutoff_frequency(_param_rc_flt_smp_rate.get(), _param_rc_flt_cutoff.get()); _filter_roll.reset(0.f); _filter_pitch.reset(0.f); _filter_yaw.reset(0.f); @@ -242,6 +308,7 @@ RCUpdate::set_params_from_rc() float param_val = math::constrain( _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + param_set(_parameter_handles.rc_param[i], ¶m_val); } } @@ -289,16 +356,16 @@ RCUpdate::Run() signal_lost = false; /* check failsafe */ - int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle + int8_t fs_ch = _rc.function[_param_rc_map_failsafe.get()]; // get channel mapped to throttle - if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping - fs_ch = _parameters.rc_map_failsafe - 1; + if (_param_rc_map_failsafe.get() > 0) { // if not 0, use channel number instead of rc.function mapping + fs_ch = _param_rc_map_failsafe.get() - 1; } - if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { + if (_param_rc_fails_thr.get() > 0 && fs_ch >= 0) { /* failsafe configured */ - if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || - (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { + if ((_param_rc_fails_thr.get() < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _param_rc_fails_thr.get()) || + (_param_rc_fails_thr.get() > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _param_rc_fails_thr.get())) { /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } @@ -317,13 +384,7 @@ RCUpdate::Run() /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (rc_input.values[i] < _parameters.min[i]) { - rc_input.values[i] = _parameters.min[i]; - } - - if (rc_input.values[i] > _parameters.max[i]) { - rc_input.values[i] = _parameters.max[i]; - } + rc_input.values[i] = math::constrain(rc_input.values[i], _parameters.min[i], _parameters.max[i]); /* * 2) Scale around the mid point differently for lower and upper range. @@ -354,7 +415,10 @@ RCUpdate::Run() _rc.channels[i] = 0.0f; } - _rc.channels[i] *= _parameters.rev[i]; + if (_parameters.rev[i]) { + _rc.channels[i] = -1.0f * _rc.channels[i]; + } + /* handle any parameter-induced blowups */ if (!PX4_ISFINITE(_rc.channels[i])) { @@ -401,7 +465,7 @@ RCUpdate::Run() manual.r = math::constrain(_filter_yaw.apply(manual.r), -1.f, 1.f); manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f); - if (_parameters.rc_map_flightmode > 0) { + if (_param_rc_map_fltmode.get() > 0) { /* number of valid slots */ const int num_slots = manual_control_setpoint_s::MODE_SLOT_NUM; @@ -419,7 +483,7 @@ RCUpdate::Run() * slots. And finally we add half a slot width to ensure that integer rounding * will take us to the correct final index. */ - manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) / + manual.mode_slot = (((((_rc.channels[_param_rc_map_fltmode.get() - 1] - slot_min) * num_slots) + slot_width_half) / (slot_max - slot_min)) + (1.0f / num_slots)) + 1; if (manual.mode_slot > num_slots) { @@ -428,33 +492,34 @@ RCUpdate::Run() } /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, - _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, + _param_rc_auto_th.get(), _param_rc_auto_th.get() < 0.f, + _param_rc_assist_th.get(), _param_rc_assist_th.get() < 0.f); + manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, - _parameters.rc_rattitude_th, - _parameters.rc_rattitude_inv); - manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, - _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, - _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, - _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, - _parameters.rc_acro_inv); + _param_rc_ratt_th.get(), _param_rc_ratt_th.get() < 0.f); + manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, + _param_rc_posctl_th.get(), _param_rc_posctl_th.get() < 0.f); + manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, + _param_rc_return_th.get(), _param_rc_return_th.get() < 0.f); + manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, + _param_rc_loiter_th.get(), _param_rc_loiter_th.get() < 0.f); + manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, + _param_rc_acro_th.get(), _param_rc_acro_th.get() < 0.f); manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, - _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + _param_rc_offb_th.get(), _param_rc_offb_th.get() < 0.f); manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH, - _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv); + _param_rc_killswitch_th.get(), _param_rc_killswitch_th.get() < 0.f); manual.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH, - _parameters.rc_armswitch_th, _parameters.rc_armswitch_inv); + _param_rc_armswitch_th.get(), _param_rc_armswitch_th.get() < 0.f); manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION, - _parameters.rc_trans_th, _parameters.rc_trans_inv); + _param_rc_trans_th.get(), _param_rc_trans_th.get() < 0.f); manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR, - _parameters.rc_gear_th, _parameters.rc_gear_inv); + _param_rc_gear_th.get(), _param_rc_gear_th.get() < 0.f); manual.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB, - _parameters.rc_stab_th, _parameters.rc_stab_inv); + _param_rc_stab_th.get(), _param_rc_stab_th.get() < 0.f); manual.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN, - _parameters.rc_man_th, _parameters.rc_man_inv); + _param_rc_man_th.get(), _param_rc_man_th.get() < 0.f); /* publish manual_control_setpoint topic */ _manual_control_pub.publish(manual); @@ -477,7 +542,7 @@ RCUpdate::Run() _actuator_group_3_pub.publish(actuator_group_3); /* Update parameters from RC Channels (tuning with RC) if activated */ - if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1e6) { + if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) { set_params_from_rc(); _last_rc_to_param_map_time = hrt_absolute_time(); } diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index cae531fd1561..73106e570f7b 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -39,8 +39,6 @@ * @author Beat Kueng */ -#include "parameters.h" - #include #include #include @@ -57,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -123,10 +122,31 @@ class RCUpdate : public ModuleBase, public ModuleParams, public px4::W */ void set_params_from_rc(); - perf_counter_t _loop_perf; /**< loop performance counter */ + static constexpr unsigned RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */ + + struct Parameters { + uint16_t min[RC_MAX_CHAN_COUNT]; + uint16_t trim[RC_MAX_CHAN_COUNT]; + uint16_t max[RC_MAX_CHAN_COUNT]; + uint16_t dz[RC_MAX_CHAN_COUNT]; + bool rev[RC_MAX_CHAN_COUNT]; + + int32_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + } _parameters{}; - Parameters _parameters{}; /**< local copies of interesting parameters */ - ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ + struct ParameterHandles { + param_t min[RC_MAX_CHAN_COUNT]; + param_t trim[RC_MAX_CHAN_COUNT]; + param_t max[RC_MAX_CHAN_COUNT]; + param_t rev[RC_MAX_CHAN_COUNT]; + param_t dz[RC_MAX_CHAN_COUNT]; + + param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the parameters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + } _parameter_handles{}; uORB::SubscriptionCallbackWorkItem _input_rc_sub{this, ORB_ID(input_rc)}; @@ -145,10 +165,68 @@ class RCUpdate : public ModuleBase, public ModuleParams, public px4::W hrt_abstime _last_rc_to_param_map_time = 0; - math::LowPassFilter2p _filter_roll; /**< filters for the main 4 stick inputs */ - math::LowPassFilter2p _filter_pitch; /** we want smooth setpoints as inputs to the controllers */ - math::LowPassFilter2p _filter_yaw; - math::LowPassFilter2p _filter_throttle; + math::LowPassFilter2p _filter_roll{50.0f, 10.f}; /**< filters for the main 4 stick inputs */ + math::LowPassFilter2p _filter_pitch{50.0f, 10.f}; /** we want smooth setpoints as inputs to the controllers */ + math::LowPassFilter2p _filter_yaw{50.0f, 10.f}; + math::LowPassFilter2p _filter_throttle{50.0f, 10.f}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + DEFINE_PARAMETERS( + + (ParamInt) _param_rc_map_roll, + (ParamInt) _param_rc_map_pitch, + (ParamInt) _param_rc_map_yaw, + (ParamInt) _param_rc_map_throttle, + (ParamInt) _param_rc_map_failsafe, + + (ParamInt) _param_rc_map_fltmode, + (ParamInt) _param_rc_map_mode_sw, + + (ParamInt) _param_rc_map_flaps, + + (ParamInt) _param_rc_map_return_sw, + (ParamInt) _param_rc_map_ratt_sw, + (ParamInt) _param_rc_map_posctl_sw, + (ParamInt) _param_rc_map_loiter_sw, + (ParamInt) _param_rc_map_acro_sw, + (ParamInt) _param_rc_map_offb_sw, + (ParamInt) _param_rc_map_kill_sw, + (ParamInt) _param_rc_map_arm_sw, + (ParamInt) _param_rc_map_trans_sw, + (ParamInt) _param_rc_map_gear_sw, + (ParamInt) _param_rc_map_stab_sw, + (ParamInt) _param_rc_map_man_sw, + + (ParamInt) _param_rc_map_aux1, + (ParamInt) _param_rc_map_aux2, + (ParamInt) _param_rc_map_aux3, + (ParamInt) _param_rc_map_aux4, + (ParamInt) _param_rc_map_aux5, + (ParamInt) _param_rc_map_aux6, + + (ParamInt) _param_rc_fails_thr, + + (ParamFloat) _param_rc_assist_th, + (ParamFloat) _param_rc_auto_th, + (ParamFloat) _param_rc_ratt_th, + (ParamFloat) _param_rc_posctl_th, + (ParamFloat) _param_rc_loiter_th, + (ParamFloat) _param_rc_acro_th, + (ParamFloat) _param_rc_offb_th, + (ParamFloat) _param_rc_killswitch_th, + (ParamFloat) _param_rc_armswitch_th, + (ParamFloat) _param_rc_trans_th, + (ParamFloat) _param_rc_gear_th, + (ParamFloat) _param_rc_stab_th, + (ParamFloat) _param_rc_man_th, + (ParamFloat) _param_rc_return_th, + + (ParamFloat) _param_rc_flt_smp_rate, + (ParamFloat) _param_rc_flt_cutoff, + + (ParamInt) _param_rc_chan_cnt + ) }; From eb3791e6633e5e8f2d1c11a5a4719a9a7d32dd43 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 4 Dec 2019 10:21:32 -0500 Subject: [PATCH 2/4] Update src/modules/rc_update/rc_update.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Beat Küng --- src/modules/rc_update/rc_update.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 4f9e413b0f54..e67ac35adb82 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -114,7 +114,7 @@ RCUpdate::parameters_updated() // make sure the filter is in its stable region -> fc < fs/2 const float flt_cutoff_min = _param_rc_flt_smp_rate.get() / 2.0f - 1.0f; - if (_param_rc_flt_cutoff.get() < flt_cutoff_min) { + if (_param_rc_flt_cutoff.get() > flt_cutoff_max) { _param_rc_flt_cutoff.set(flt_cutoff_min); _param_rc_flt_cutoff.commit_no_notification(); } From f0147f03a7c566449a0de87f445d08eeb7150f1e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 4 Dec 2019 10:21:50 -0500 Subject: [PATCH 3/4] Update src/modules/rc_update/rc_update.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Beat Küng --- src/modules/rc_update/rc_update.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index e67ac35adb82..2cf859419e4d 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -416,7 +416,7 @@ RCUpdate::Run() } if (_parameters.rev[i]) { - _rc.channels[i] = -1.0f * _rc.channels[i]; + _rc.channels[i] = -_rc.channels[i]; } From 6abd35e0f7b0ecf3e99993b8be0f47640d6bef41 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 9 Dec 2019 12:48:07 -0500 Subject: [PATCH 4/4] fix flt_cutoff_min -> flt_cutoff_max --- src/modules/rc_update/rc_update.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 2cf859419e4d..a0107700e68e 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -112,10 +112,10 @@ RCUpdate::parameters_updated() } // make sure the filter is in its stable region -> fc < fs/2 - const float flt_cutoff_min = _param_rc_flt_smp_rate.get() / 2.0f - 1.0f; + const float flt_cutoff_max = _param_rc_flt_smp_rate.get() / 2.0f - 1.0f; if (_param_rc_flt_cutoff.get() > flt_cutoff_max) { - _param_rc_flt_cutoff.set(flt_cutoff_min); + _param_rc_flt_cutoff.set(flt_cutoff_max); _param_rc_flt_cutoff.commit_no_notification(); }