diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 820e239827..19d101a01d 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1960,6 +1960,14 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f); */ PARAM_DEFINE_FLOAT(BAT_C_SCALING, -1.0); +/** + * Offset for battery current sensor. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_C_OFFSET, -1.0); + /** * RC channel count diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index eff6ae80e2..a5813022e4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -321,6 +321,7 @@ private: float battery_voltage_scaling; float battery_current_scaling; + float battery_current_offset; float baro_qnh; @@ -380,6 +381,7 @@ private: param_t battery_voltage_scaling; param_t battery_current_scaling; + param_t battery_current_offset; param_t board_rotation; @@ -652,6 +654,7 @@ Sensors::Sensors() : _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"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -927,6 +930,13 @@ Sensors::parameters_update() #endif } + if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) { + warnx("%s", paramerr); + + } else if (_parameters.battery_current_offset < 0.0f) { + _parameters.battery_current_offset = 0.0f; + } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); @@ -1669,7 +1679,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { /* handle current only if voltage is valid */ if (_battery_status.voltage_v > 0.0f) { - float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + float current = ((buf_adc[i].am_data - _parameters.battery_current_offset) * _parameters.battery_current_scaling); /* check measured current value */ if (current >= 0.0f) {