yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Track raw battery voltage and filtered battery voltage separately. Estimate remaining battery as min(voltage_estimate, discharged_estimate). Battery voltage LPF time increased.

Browse Source
This commit is contained in:
Anton Babushkin
2013-11-11 22:02:55 +04:00
parent e8487b7498
commit 714f5ea634
6 changed files with 57 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
src/modules/sensors/sensors.cpp
View File

@@ -125,9 +125,8 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
@@ -1173,32 +1172,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_v < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
_battery_status.voltage_v = voltage;
if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
}
_battery_status.timestamp = t;
_battery_status.voltage_v += (voltage - _battery_status.voltage_v) * BAT_VOL_LOWPASS;
_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
} else {
/* mark status as invalid */
_battery_status.timestamp = 0;
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
}
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.timestamp != 0) {
if (_battery_status.voltage_v > 0.0f) {
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f)
_battery_status.discharged_mah = 0.0f;
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
_battery_current_timestamp = t;
}
_battery_current_timestamp = t;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1229,7 +1236,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
_last_adc = t;
if (_battery_status.timestamp != 0) {
if (_battery_status.voltage_v > 0.0f) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
@@ -1512,7 +1519,10 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = BAT_VOL_INITIAL;
_battery_status.voltage_v = 0.0f;
_battery_status.voltage_filtered_v = 0.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);