From f9b3b5a799667e0ec4b1bf29692892533514c9cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 29 Jun 2016 13:37:51 +0200 Subject: [PATCH] sensors: break off parameters initialization into separate source file --- src/modules/sensors/CMakeLists.txt | 1 + src/modules/sensors/parameters.cpp | 465 +++++++++++++++++++++++++++++ src/modules/sensors/parameters.h | 12 + src/modules/sensors/sensors.cpp | 410 +------------------------ src/platforms/px4_defines.h | 5 +- 5 files changed, 487 insertions(+), 406 deletions(-) create mode 100644 src/modules/sensors/parameters.cpp diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index d8e64e1048..520d38831e 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( rc_update.cpp sensors.cpp sensors_init.cpp + parameters.cpp DEPENDS platforms__common diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp new file mode 100644 index 0000000000..c1669383ad --- /dev/null +++ b/src/modules/sensors/parameters.cpp @@ -0,0 +1,465 @@ +/**************************************************************************** + * + * 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 sensors +{ + +int 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_trans_sw = param_find("RC_MAP_TRANS_SW"); + parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_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"); + + /* 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_trans_th = param_find("RC_TRANS_TH"); + parameter_handles.rc_gear_th = param_find("RC_GEAR_TH"); + + /* Differential pressure offset */ + parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); + + parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT"); + parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR"); + parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR"); + parameter_handles.battery_v_div = param_find("BAT_V_DIV"); + parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V"); + parameter_handles.battery_source = param_find("BAT_SOURCE"); + + /* rotations */ + parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + + /* rotation offsets */ + parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + + /* Barometer QNH */ + parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + + parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); + + // 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"); + (void)param_find("RC_TH_USER"); + (void)param_find("CAL_MAG0_ID"); + (void)param_find("CAL_MAG1_ID"); + (void)param_find("CAL_MAG2_ID"); + (void)param_find("CAL_MAG0_ROT"); + (void)param_find("CAL_MAG1_ROT"); + (void)param_find("CAL_MAG2_ROT"); + (void)param_find("CAL_MAG_SIDES"); + + (void)param_find("CAL_MAG1_XOFF"); + (void)param_find("CAL_MAG1_XSCALE"); + (void)param_find("CAL_MAG1_YOFF"); + (void)param_find("CAL_MAG1_YSCALE"); + (void)param_find("CAL_MAG1_ZOFF"); + (void)param_find("CAL_MAG1_ZSCALE"); + + (void)param_find("CAL_MAG2_XOFF"); + (void)param_find("CAL_MAG2_XSCALE"); + (void)param_find("CAL_MAG2_YOFF"); + (void)param_find("CAL_MAG2_YSCALE"); + (void)param_find("CAL_MAG2_ZOFF"); + (void)param_find("CAL_MAG2_ZSCALE"); + + (void)param_find("CAL_GYRO1_XOFF"); + (void)param_find("CAL_GYRO1_XSCALE"); + (void)param_find("CAL_GYRO1_YOFF"); + (void)param_find("CAL_GYRO1_YSCALE"); + (void)param_find("CAL_GYRO1_ZOFF"); + (void)param_find("CAL_GYRO1_ZSCALE"); + + (void)param_find("CAL_GYRO2_XOFF"); + (void)param_find("CAL_GYRO2_XSCALE"); + (void)param_find("CAL_GYRO2_YOFF"); + (void)param_find("CAL_GYRO2_YSCALE"); + (void)param_find("CAL_GYRO2_ZOFF"); + (void)param_find("CAL_GYRO2_ZSCALE"); + + (void)param_find("CAL_ACC1_XOFF"); + (void)param_find("CAL_ACC1_XSCALE"); + (void)param_find("CAL_ACC1_YOFF"); + (void)param_find("CAL_ACC1_YSCALE"); + (void)param_find("CAL_ACC1_ZOFF"); + (void)param_find("CAL_ACC1_ZSCALE"); + + (void)param_find("CAL_ACC2_XOFF"); + (void)param_find("CAL_ACC2_XSCALE"); + (void)param_find("CAL_ACC2_YOFF"); + (void)param_find("CAL_ACC2_YSCALE"); + (void)param_find("CAL_ACC2_ZOFF"); + (void)param_find("CAL_ACC2_ZSCALE"); + + (void)param_find("SYS_PARAM_VER"); + (void)param_find("SYS_AUTOSTART"); + (void)param_find("SYS_AUTOCONFIG"); + (void)param_find("PWM_RATE"); + (void)param_find("PWM_MIN"); + (void)param_find("PWM_MAX"); + (void)param_find("PWM_DISARMED"); + (void)param_find("PWM_AUX_MIN"); + (void)param_find("PWM_AUX_MAX"); + (void)param_find("PWM_AUX_DISARMED"); + (void)param_find("TRIG_MODE"); + (void)param_find("UAVCAN_ENABLE"); + (void)param_find("SYS_MC_EST_GROUP"); + + + return 0; +} + +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_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); + } + + 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)); + + 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 = fabs(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 = fabs(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 = fabs(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 = fabs(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 = fabs(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 = fabs(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 = fabs(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 = fabs(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 = fabs(parameters.rc_killswitch_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 = fabs(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 = fabs(parameters.rc_gear_th); + + /* Airspeed offset */ + param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa)); + param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale)); + + /* scaling of ADC ticks to battery voltage */ + if (param_get(parameter_handles.battery_voltage_scaling, &(parameters.battery_voltage_scaling)) != OK) { + PX4_WARN("%s", paramerr); + + } else if (parameters.battery_voltage_scaling < 0.0f) { + /* apply scaling according to defaults if set to default */ + parameters.battery_voltage_scaling = (3.3f / 4096); + param_set(parameter_handles.battery_voltage_scaling, ¶meters.battery_voltage_scaling); + } + + /* scaling of ADC ticks to battery current */ + if (param_get(parameter_handles.battery_current_scaling, &(parameters.battery_current_scaling)) != OK) { + PX4_WARN("%s", paramerr); + + } else if (parameters.battery_current_scaling < 0.0f) { + /* apply scaling according to defaults if set to default */ + parameters.battery_current_scaling = (3.3f / 4096); + param_set(parameter_handles.battery_current_scaling, ¶meters.battery_current_scaling); + } + + if (param_get(parameter_handles.battery_current_offset, &(parameters.battery_current_offset)) != OK) { + PX4_WARN("%s", paramerr); + + } + + if (param_get(parameter_handles.battery_v_div, &(parameters.battery_v_div)) != OK) { + PX4_WARN("%s", paramerr); + parameters.battery_v_div = 0.0f; + + } else if (parameters.battery_v_div <= 0.0f) { + /* apply scaling according to defaults if set to default */ +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + parameters.battery_v_div = 13.653333333f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) + parameters.battery_v_div = 10.177939394f; +#elif defined (CONFIG_ARCH_BOARD_AEROCORE) + parameters.battery_v_div = 7.8196363636f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) + parameters.battery_v_div = 5.7013919372f; +#elif defined (CONFIG_ARCH_BOARD_SITL) + parameters.battery_v_div = 10.177939394f; +#elif defined (CONFIG_ARCH_BOARD_TAP_V1) + parameters.battery_v_div = 9.0f; +#else + /* ensure a missing default trips a low voltage lockdown */ + parameters.battery_v_div = 0.0f; +#endif + param_set(parameter_handles.battery_v_div, ¶meters.battery_v_div); + } + + if (param_get(parameter_handles.battery_a_per_v, &(parameters.battery_a_per_v)) != OK) { + PX4_WARN("%s", paramerr); + parameters.battery_a_per_v = 0.0f; + + } else if (parameters.battery_a_per_v <= 0.0f) { + /* apply scaling according to defaults if set to default */ +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + /* current scaling for ACSP4 */ + parameters.battery_a_per_v = 36.367515152f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_AEROCORE) || defined (CONFIG_ARCH_BOARD_PX4FMU_V1) + /* current scaling for 3DR power brick */ + parameters.battery_a_per_v = 15.391030303f; +#elif defined (CONFIG_ARCH_BOARD_SITL) + parameters.battery_a_per_v = 15.391030303f; +#else + /* ensure a missing default leads to an unrealistic current value */ + parameters.battery_a_per_v = 0.0f; +#endif + param_set(parameter_handles.battery_a_per_v, ¶meters.battery_a_per_v); + } + + param_get(parameter_handles.battery_source, &(parameters.battery_source)); + + param_get(parameter_handles.board_rotation, &(parameters.board_rotation)); + + param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0])); + param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1])); + param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2])); + + param_get(parameter_handles.baro_qnh, &(parameters.baro_qnh)); + + param_get(parameter_handles.vibe_thresh, ¶meters.vibration_warning_threshold); + + return ret; +} + +} /* namespace sensors */ diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index cc405fc558..d8b817c1d5 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -208,5 +208,17 @@ struct ParameterHandles { }; +/** + * initialize ParameterHandles struct + * @return 0 on succes, <0 on error + */ +int initialize_parameter_handles(ParameterHandles ¶meter_handles); + + +/** + * Read out the parameters using the handles into the parameters struct. + * @return 0 on succes, <0 on error + */ +int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); } /* namespace sensors */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b91be6a102..467a31fb7d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -437,177 +437,7 @@ Sensors::Sensors() : memset(&_accel_diff, 0, sizeof(_accel_diff)); memset(&_gyro_diff, 0, sizeof(_gyro_diff)); - /* 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_trans_sw = param_find("RC_MAP_TRANS_SW"); - _parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_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"); - - /* 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_trans_th = param_find("RC_TRANS_TH"); - _parameter_handles.rc_gear_th = param_find("RC_GEAR_TH"); - - - /* Differential pressure offset */ - _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); - _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); - - _parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT"); - _parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR"); - _parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR"); - _parameter_handles.battery_v_div = param_find("BAT_V_DIV"); - _parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V"); - _parameter_handles.battery_source = param_find("BAT_SOURCE"); - - /* rotations */ - _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); - - /* rotation offsets */ - _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); - _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); - _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); - - /* Barometer QNH */ - _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); - - _parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); - - // 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"); - (void)param_find("RC_TH_USER"); - (void)param_find("CAL_MAG0_ID"); - (void)param_find("CAL_MAG1_ID"); - (void)param_find("CAL_MAG2_ID"); - (void)param_find("CAL_MAG0_ROT"); - (void)param_find("CAL_MAG1_ROT"); - (void)param_find("CAL_MAG2_ROT"); - (void)param_find("CAL_MAG_SIDES"); - - (void)param_find("CAL_MAG1_XOFF"); - (void)param_find("CAL_MAG1_XSCALE"); - (void)param_find("CAL_MAG1_YOFF"); - (void)param_find("CAL_MAG1_YSCALE"); - (void)param_find("CAL_MAG1_ZOFF"); - (void)param_find("CAL_MAG1_ZSCALE"); - - (void)param_find("CAL_MAG2_XOFF"); - (void)param_find("CAL_MAG2_XSCALE"); - (void)param_find("CAL_MAG2_YOFF"); - (void)param_find("CAL_MAG2_YSCALE"); - (void)param_find("CAL_MAG2_ZOFF"); - (void)param_find("CAL_MAG2_ZSCALE"); - - (void)param_find("CAL_GYRO1_XOFF"); - (void)param_find("CAL_GYRO1_XSCALE"); - (void)param_find("CAL_GYRO1_YOFF"); - (void)param_find("CAL_GYRO1_YSCALE"); - (void)param_find("CAL_GYRO1_ZOFF"); - (void)param_find("CAL_GYRO1_ZSCALE"); - - (void)param_find("CAL_GYRO2_XOFF"); - (void)param_find("CAL_GYRO2_XSCALE"); - (void)param_find("CAL_GYRO2_YOFF"); - (void)param_find("CAL_GYRO2_YSCALE"); - (void)param_find("CAL_GYRO2_ZOFF"); - (void)param_find("CAL_GYRO2_ZSCALE"); - - (void)param_find("CAL_ACC1_XOFF"); - (void)param_find("CAL_ACC1_XSCALE"); - (void)param_find("CAL_ACC1_YOFF"); - (void)param_find("CAL_ACC1_YSCALE"); - (void)param_find("CAL_ACC1_ZOFF"); - (void)param_find("CAL_ACC1_ZSCALE"); - - (void)param_find("CAL_ACC2_XOFF"); - (void)param_find("CAL_ACC2_XSCALE"); - (void)param_find("CAL_ACC2_YOFF"); - (void)param_find("CAL_ACC2_YSCALE"); - (void)param_find("CAL_ACC2_ZOFF"); - (void)param_find("CAL_ACC2_ZSCALE"); - - (void)param_find("SYS_PARAM_VER"); - (void)param_find("SYS_AUTOSTART"); - (void)param_find("SYS_AUTOCONFIG"); - (void)param_find("PWM_RATE"); - (void)param_find("PWM_MIN"); - (void)param_find("PWM_MAX"); - (void)param_find("PWM_DISARMED"); - (void)param_find("PWM_AUX_MIN"); - (void)param_find("PWM_AUX_MAX"); - (void)param_find("PWM_AUX_DISARMED"); - (void)param_find("TRIG_MODE"); - (void)param_find("UAVCAN_ENABLE"); - (void)param_find("SYS_MC_EST_GROUP"); + initialize_parameter_handles(_parameter_handles); /* fetch initial parameter values */ parameters_update(); @@ -641,243 +471,16 @@ Sensors::~Sensors() int Sensors::parameters_update() { - bool rc_valid = true; - float tmpScaleFactor = 0.0f; - float tmpRevFactor = 0.0f; - int ret = PX4_OK; + int ret = update_parameters(_parameter_handles, _parameters); - /* 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; - } + if (ret) { + return ret; } - /* 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_trans_sw, &(_parameters.rc_map_trans_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_gear_sw, &(_parameters.rc_map_gear_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != 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)); - - 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 = fabs(_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 = fabs(_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 = fabs(_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 = fabs(_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 = fabs(_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 = fabs(_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 = fabs(_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 = fabs(_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 = fabs(_parameters.rc_killswitch_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 = fabs(_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 = fabs(_parameters.rc_gear_th); - _rc_update.update_rc_functions(); - /* Airspeed offset */ - param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); - param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); - - /* scaling of ADC ticks to battery voltage */ - if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - PX4_WARN("%s", paramerr); - - } else if (_parameters.battery_voltage_scaling < 0.0f) { - /* apply scaling according to defaults if set to default */ - _parameters.battery_voltage_scaling = (3.3f / 4096); - param_set(_parameter_handles.battery_voltage_scaling, &_parameters.battery_voltage_scaling); - } - - /* scaling of ADC ticks to battery current */ - if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - PX4_WARN("%s", paramerr); - - } else if (_parameters.battery_current_scaling < 0.0f) { - /* apply scaling according to defaults if set to default */ - _parameters.battery_current_scaling = (3.3f / 4096); - param_set(_parameter_handles.battery_current_scaling, &_parameters.battery_current_scaling); - } - - if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) { - PX4_WARN("%s", paramerr); - - } - - if (param_get(_parameter_handles.battery_v_div, &(_parameters.battery_v_div)) != OK) { - PX4_WARN("%s", paramerr); - _parameters.battery_v_div = 0.0f; - - } else if (_parameters.battery_v_div <= 0.0f) { - /* apply scaling according to defaults if set to default */ -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - _parameters.battery_v_div = 13.653333333f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) - _parameters.battery_v_div = 10.177939394f; -#elif defined (CONFIG_ARCH_BOARD_AEROCORE) - _parameters.battery_v_div = 7.8196363636f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) - _parameters.battery_v_div = 5.7013919372f; -#elif defined (CONFIG_ARCH_BOARD_SITL) - _parameters.battery_v_div = 10.177939394f; -#elif defined (CONFIG_ARCH_BOARD_TAP_V1) - _parameters.battery_v_div = 9.0f; -#else - /* ensure a missing default trips a low voltage lockdown */ - _parameters.battery_v_div = 0.0f; -#endif - param_set(_parameter_handles.battery_v_div, &_parameters.battery_v_div); - } - - if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) { - PX4_WARN("%s", paramerr); - _parameters.battery_a_per_v = 0.0f; - - } else if (_parameters.battery_a_per_v <= 0.0f) { - /* apply scaling according to defaults if set to default */ -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - /* current scaling for ACSP4 */ - _parameters.battery_a_per_v = 36.367515152f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_AEROCORE) || defined (CONFIG_ARCH_BOARD_PX4FMU_V1) - /* current scaling for 3DR power brick */ - _parameters.battery_a_per_v = 15.391030303f; -#elif defined (CONFIG_ARCH_BOARD_SITL) - _parameters.battery_a_per_v = 15.391030303f; -#else - /* ensure a missing default leads to an unrealistic current value */ - _parameters.battery_a_per_v = 0.0f; -#endif - param_set(_parameter_handles.battery_a_per_v, &_parameters.battery_a_per_v); - } - - param_get(_parameter_handles.battery_source, &(_parameters.battery_source)); - - param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); - - param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); - param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); - param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - - /** fine tune board offset on parameter update **/ + /* fine tune board offset */ math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], @@ -886,7 +489,6 @@ Sensors::parameters_update() _board_rotation = board_rotation_offset * _board_rotation; /* update barometer qnh setting */ - param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); DevHandle h_baro; DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); @@ -908,8 +510,6 @@ Sensors::parameters_update() #endif - param_get(_parameter_handles.vibe_thresh, &_parameters.vibration_warning_threshold); - return ret; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 4da7914403..7555e143b6 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -63,7 +63,10 @@ /* Define a usable PX4_ISFINITE. Note that PX4_ISFINITE is ONLY used in C++ files, * therefore, by default, we want to use std::isfinite. */ -# define PX4_ISFINITE(x) std::isfinite(x) +#ifdef __cplusplus +#include +#define PX4_ISFINITE(x) std::isfinite(x) +#endif #if defined(__PX4_ROS) /****************************************************************************