fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Battery charge estimation: Factor in voltage drop for idle props

Browse Source
This commit is contained in:
Lorenz Meier 2016-05-06 18:14:34 +02:00
parent 48d7295be6
commit ebaca071f6
4 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/drivers/px4io/px4io.cpp
View File

@ -301,6 +301,7 @@ private:
bool _primary_pwm_device; ///< true if we are the default PWM output
bool _lockdown_override; ///< allow to override the safety lockdown
bool _armed; ///< wether the system is armed
float _battery_amp_per_volt; ///< current sensor amps/volt
float _battery_amp_bias; ///< current sensor bias
@ -537,6 +538,7 @@ PX4IO::PX4IO(device::Device *interface) :
_servorail_status{},
_primary_pwm_device(false),
_lockdown_override(false),
_armed(false),
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
@ -1348,6 +1350,8 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
_armed = armed.armed;
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
_lockdown_override = true;
@ -1683,7 +1687,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
current_a += _battery_amp_bias;
_battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, &battery_status);
_battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, _armed, &battery_status);
/* the announced battery status would conflict with the simulated battery status in HIL */
if (!(_pub_blocked)) {

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

@ -1705,7 +1705,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
actuator_controls_s ctrl;
orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
_battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, ctrl.control[actuator_controls_s::INDEX_THROTTLE],
&_battery_status);
_armed, &_battery_status);
/* announce the battery status if needed, just publish else */
if (_battery_pub != nullptr) {

13
src/modules/systemlib/battery.cpp
View File

@ -71,7 +71,7 @@ Battery::reset(battery_status_s *battery_status)
{
memset(battery_status, 0, sizeof(*battery_status));
battery_status->current_a = -1.0f;
battery_status->remaining = 0.0f;
battery_status->remaining = 1.0f;
battery_status->cell_count = _param_n_cells.get();
// TODO: check if it is sane to reset warning to NONE
battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
@ -79,13 +79,13 @@ Battery::reset(battery_status_s *battery_status)
void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
battery_status_s *battery_status)
bool armed, battery_status_s *battery_status)
{
reset(battery_status);
battery_status->timestamp = timestamp;
filterVoltage(voltage_v);
sumDischarged(timestamp, current_a);
estimateRemaining(voltage_v, throttle_normalized);
estimateRemaining(voltage_v, throttle_normalized, armed);
determineWarning();
if (_voltage_filtered_v > 2.1f) {
@ -134,10 +134,13 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
}
void
Battery::estimateRemaining(float voltage_v, float throttle_normalized)
Battery::estimateRemaining(float voltage_v, float throttle_normalized, bool armed)
{
// assume 10% voltage drop of the full drop range with motors idle
const float thr = (armed) ? ((fabsf(throttle_normalized) + 0.1f) / 1.1f) : 0.0f;
// remaining charge estimate based on voltage and internal resistance (drop under load)
const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * fabsf(throttle_normalized));
const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * thr);
// the range from full to empty is the same for batteries under load and without load,
// since the voltage drop applies to both the full and empty state

4
src/modules/systemlib/battery.h
View File

@ -88,12 +88,12 @@ public:
* @param throttle_normalized: throttle from 0 to 1
*/
void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
battery_status_s *status);
bool armed, battery_status_s *status);
private:
void filterVoltage(float voltage_v);
void sumDischarged(hrt_abstime timestamp, float current_a);
void estimateRemaining(float voltage_v, float throttle_normalized);
void estimateRemaining(float voltage_v, float throttle_normalized, bool armed);
void determineWarning();
control::BlockParamFloat _param_v_empty;