From d9767eb100ad6965012dfb945992843d157782d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 7 Nov 2013 22:23:57 +0400 Subject: [PATCH] Battery current reading implemented. --- src/modules/sensors/sensor_params.c | 2 ++ src/modules/sensors/sensors.cpp | 40 +++++++++++++++++++++-------- 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b815886..09ddbf5032 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -197,12 +197,14 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Star #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value #else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value #endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372c..789a4a30de 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -261,6 +261,7 @@ private: float rc_scale_flaps; float battery_voltage_scaling; + float battery_current_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -306,6 +307,7 @@ private: param_t rc_scale_flaps; param_t battery_voltage_scaling; + param_t battery_current_scaling; param_t board_rotation; param_t external_mag_rotation; @@ -547,6 +549,7 @@ Sensors::Sensors() : _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -724,6 +727,11 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + /* scaling of ADC ticks to battery current */ + if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { + warnx("Failed updating current scaling param"); + } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); @@ -1162,25 +1170,25 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - /* one-time initialization of low-pass value to avoid long init delays */ - if (_battery_status.voltage_v < 3.0f) { + if (_battery_status.voltage_v < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { _battery_status.voltage_v = voltage; } _battery_status.timestamp = hrt_absolute_time(); _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; - /* announce the battery voltage if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + } else { + /* mark status as invalid */ + _battery_status.timestamp = 0; + } - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); - } + } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { + /* handle current only if voltage is valid */ + if (_battery_status.timestamp != 0) { + _battery_status.current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling); + float dt = fminf(20.0f, (hrt_absolute_time() - _last_adc) * 0.001f); // in ms, limit to 20ms + _battery_status.discharged_mah += _battery_status.current_a * dt; } } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1214,6 +1222,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _last_adc = hrt_absolute_time(); } } + + if (_battery_status.timestamp != 0) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } } }