From e1805dfe0c94f082d96842e46d30dc76c3748314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 May 2016 14:09:29 +0200 Subject: [PATCH] Sensors app: Use two parameters to convert battery current and voltage to make configuring custom frames easier --- src/modules/sensors/sensor_params.c | 48 +++++++++++++++--- src/modules/sensors/sensors.cpp | 75 ++++++++++++++++++----------- 2 files changed, 89 insertions(+), 34 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 29439d61d4..2de4b4216f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1964,28 +1964,64 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); /** - * Scaling factor for battery voltage sensor on FMU v2. + * Scaling from ADC counts to volt on the ADC input (battery voltage) + * + * This is not the battery voltage, but the intermediate ADC voltage. + * A value of -1 signifies that the board defaults are used, which is + * highly recommended. * * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f); +PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f); /** - * Scaling factor for battery current sensor. + * Scaling from ADC counts to volt on the ADC input (battery current) + * + * This is not the battery current, but the intermediate ADC voltage. + * A value of -1 signifies that the board defaults are used, which is + * highly recommended. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0); + +/** + * Offset in volt as seen by the ADC input of the current sensor. + * + * This offset will be subtracted before calculating the battery + * current based on the voltage. * * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_C_SCALING, -1.0); +PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0); /** - * Offset for battery current sensor. + * Battery voltage divider (V divider) + * + * This is the divider from battery voltage to 3.3V ADC voltage. + * If using e.g. Mauch power modules the value from the datasheet + * can be applied straight here. A value of -1 means to use + * the board default. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); + +/** + * Battery current per volt (A/V) + * + * The voltage seen by the 3.3V ADC multiplied by this factor + * will determine the battery current. A value of -1 means to use + * the board default. * * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_C_OFFSET, -1.0); +PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); /** diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 116911bb3a..9eb02d46b9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -317,6 +317,8 @@ private: float battery_voltage_scaling; float battery_current_scaling; float battery_current_offset; + float battery_v_div; + float battery_a_per_v; float baro_qnh; @@ -377,6 +379,8 @@ private: param_t battery_voltage_scaling; param_t battery_current_scaling; param_t battery_current_offset; + param_t battery_v_div; + param_t battery_a_per_v; param_t board_rotation; @@ -644,9 +648,11 @@ Sensors::Sensors() : _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_V_SCALING"); - _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); - _parameter_handles.battery_current_offset = param_find("BAT_C_OFFSET"); + _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"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -887,18 +893,7 @@ Sensors::parameters_update() } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - _parameters.battery_voltage_scaling = 0.011f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) - _parameters.battery_voltage_scaling = 0.0082f; -#elif defined (CONFIG_ARCH_BOARD_AEROCORE) - _parameters.battery_voltage_scaling = 0.0063f; -#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) - _parameters.battery_voltage_scaling = 0.00459340659f; -#else - /* ensure a missing default trips a low voltage lockdown */ - _parameters.battery_voltage_scaling = 0.00001f; -#endif + _parameters.battery_voltage_scaling = (3.3f / 4096); } /* scaling of ADC ticks to battery current */ @@ -907,27 +902,50 @@ Sensors::parameters_update() } else if (_parameters.battery_current_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ + _parameters.battery_current_scaling = (3.3f / 4096); + } + + if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) { + warnx("%s", paramerr); + + } + + if (param_get(_parameter_handles.battery_v_div, &(_parameters.battery_v_div)) != OK) { + warnx("%s", paramerr); + _parameters.battery_v_div = 100.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) - /* current scaling for ACSP4 */ - _parameters.battery_current_scaling = 0.0293f; + _parameters.battery_v_div = 13.653333333f; #elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 ) - /* current scaling for 3DR power brick */ - _parameters.battery_current_scaling = 0.0124f; + _parameters.battery_v_div = 10.177939394f; #elif defined (CONFIG_ARCH_BOARD_AEROCORE) - _parameters.battery_current_scaling = 0.0124f; + _parameters.battery_v_div = 7.8196363636f; #elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) - _parameters.battery_current_scaling = 0.0124f; + _parameters.battery_v_div = 5.7013919372f; #else - /* ensure a missing default leads to an unrealistic current value */ - _parameters.battery_current_scaling = 0.00001f; + /* ensure a missing default trips a low voltage lockdown */ + _parameters.battery_v_div = 100.0f; #endif } - if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) { + if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) { warnx("%s", paramerr); + _parameters.battery_a_per_v = 100.0f; - } else if (_parameters.battery_current_offset < 0.0f) { - _parameters.battery_current_offset = 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; +#else + /* ensure a missing default leads to an unrealistic current value */ + _parameters.battery_a_per_v = 100.0f; +#endif } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); @@ -1655,14 +1673,15 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* look for specific channels and process the raw voltage to measurement data */ if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* Voltage in volts */ - bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); + bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div; if (bat_voltage_v > 0.5f) { updated_battery = true; } } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { - bat_current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling); + bat_current_a = ((buf_adc[i].am_data * _parameters.battery_current_scaling) + - _parameters.battery_current_offset) * _parameters.battery_a_per_v; #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL