mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 21:03:34 +08:00
commander: move battery calculations to systemlib
The commander used to consume the battery_status topic and write the contents after some calculations into the system state. Instead, the calculations now happen in library calls in systemlib/battery. This moves the battery fields out of the vehicle_status message into the battery_status topic. This brought quite some changes in all modules that need battery information. The current state is compiling but untested.
This commit is contained in:
Julian Oes
@@ -48,6 +48,7 @@ set(SRCS
|
||||
mcu_version.c
|
||||
bson/tinybson.c
|
||||
circuit_breaker.cpp
|
||||
battery.cpp
|
||||
)
|
||||
|
||||
if(${OS} STREQUAL "nuttx")
|
||||
@@ -55,7 +56,7 @@ if(${OS} STREQUAL "nuttx")
|
||||
err.c
|
||||
printload.c
|
||||
param/param.c
|
||||
up_cxxinitialize.c
|
||||
up_cxxinitialize.c
|
||||
)
|
||||
elseif ("${CONFIG_SHMEM}" STREQUAL "1")
|
||||
list(APPEND SRCS
|
||||
@@ -66,7 +67,7 @@ else()
|
||||
list(APPEND SRCS
|
||||
param/param.c
|
||||
print_load_posix.c
|
||||
)
|
||||
)
|
||||
endif()
|
||||
|
||||
if(NOT ${OS} STREQUAL "qurt")
|
||||
@@ -85,4 +86,4 @@ px4_add_module(
|
||||
platforms__common
|
||||
modules__param
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
||||
@@ -0,0 +1,162 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file battery.cpp
|
||||
*
|
||||
* Library calls for battery functionality.
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include "battery.h"
|
||||
|
||||
Battery::Battery() :
|
||||
SuperBlock(NULL, NULL),
|
||||
_param_v_empty(this, "BAT_V_EMPTY", false),
|
||||
_param_v_full(this, "BAT_V_CHARGED", false),
|
||||
_param_n_cells(this, "BAT_N_CELLS", false),
|
||||
_param_capacity(this, "BAT_CAPACITY", false),
|
||||
_param_v_load_drop(this, "BAT_V_LOAD_DROP", false),
|
||||
_voltage_filtered_v(0.0f),
|
||||
_throttle_filtered(0.0f),
|
||||
_discharged_mah(0.0f),
|
||||
_remaining(1.0f),
|
||||
_warning(battery_status_s::BATTERY_WARNING_NONE),
|
||||
_last_timestamp(0)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
}
|
||||
|
||||
Battery::~Battery()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
Battery::reset(battery_status_s *battery_status)
|
||||
{
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
void
|
||||
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
|
||||
battery_status_s *battery_status)
|
||||
{
|
||||
filterVoltage(voltage_v);
|
||||
sumDischarged(timestamp, current_a);
|
||||
estimateRemaining(voltage_v, throttle_normalized);
|
||||
determineWarning();
|
||||
|
||||
if (_voltage_filtered_v > 2.1f) {
|
||||
battery_status->voltage_v = voltage_v;
|
||||
battery_status->voltage_filtered_v = _voltage_filtered_v;
|
||||
battery_status->current_a = current_a;
|
||||
battery_status->discharged_mah = _discharged_mah;
|
||||
battery_status->cell_count = _param_n_cells.get();
|
||||
battery_status->warning = _warning;
|
||||
} else {
|
||||
reset(battery_status);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Battery::filterVoltage(float voltage_v)
|
||||
{
|
||||
// TODO: inspect that filter performance
|
||||
const float filtered_next = _voltage_filtered_v * 0.999f + voltage_v * 0.001f;
|
||||
if (PX4_ISFINITE(filtered_next)) {
|
||||
_voltage_filtered_v = filtered_next;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
|
||||
{
|
||||
// Ignore first update because we don't know dT.
|
||||
if (_last_timestamp != 0) {
|
||||
_discharged_mah = current_a * (timestamp - _last_timestamp) * 1.0e-3f / 3600.0f;
|
||||
}
|
||||
_last_timestamp = timestamp;
|
||||
}
|
||||
|
||||
void
|
||||
Battery::estimateRemaining(float voltage_v, float throttle_normalized)
|
||||
{
|
||||
// XXX this time constant needs to become tunable but really, the right fix are smart batteries.
|
||||
const float filtered_next = _throttle_filtered * 0.97f + throttle_normalized * 0.03f;
|
||||
|
||||
if (PX4_ISFINITE(filtered_next)) {
|
||||
_throttle_filtered = filtered_next;
|
||||
}
|
||||
|
||||
/* 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() * _throttle_filtered);
|
||||
/* 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
|
||||
*/
|
||||
const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
|
||||
const float remaining_voltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic))
|
||||
/ (_param_n_cells.get() * voltage_range);
|
||||
|
||||
if (_param_capacity.get() > 0.0f) {
|
||||
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
|
||||
_remaining = fminf(remaining_voltage, 1.0f - _discharged_mah / _param_capacity.get());
|
||||
|
||||
} else {
|
||||
/* else use voltage */
|
||||
_remaining = remaining_voltage;
|
||||
}
|
||||
|
||||
/* limit to sane values */
|
||||
_remaining = (_remaining < 0.0f) ? 0.0f : _remaining;
|
||||
_remaining = (_remaining > 1.0f) ? 1.0f : _remaining;
|
||||
}
|
||||
|
||||
void
|
||||
Battery::determineWarning()
|
||||
{
|
||||
// TODO: Determine threshold or make params.
|
||||
if (_remaining < 0.18f) {
|
||||
_warning = battery_status_s::BATTERY_WARNING_LOW;
|
||||
|
||||
} else if (_remaining < 0.09f) {
|
||||
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file battery.h
|
||||
*
|
||||
* Library calls for battery functionality.
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
||||
class Battery : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
Battery();
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
~Battery();
|
||||
|
||||
/**
|
||||
* Reset all battery stats and report invalid/nothing.
|
||||
*/
|
||||
void reset(battery_status_s *battery_status);
|
||||
|
||||
/**
|
||||
* Update current battery status message.
|
||||
*
|
||||
* @param voltage_v: current voltage in V
|
||||
* @param current_a: current current in A
|
||||
* @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);
|
||||
|
||||
private:
|
||||
void filterVoltage(float voltage_v);
|
||||
void sumDischarged(hrt_abstime timestamp, float current_a);
|
||||
void estimateRemaining(float voltage_v, float throttle_normalized);
|
||||
void determineWarning();
|
||||
|
||||
control::BlockParamFloat _param_v_empty;
|
||||
control::BlockParamFloat _param_v_full;
|
||||
control::BlockParamFloat _param_n_cells;
|
||||
control::BlockParamFloat _param_capacity;
|
||||
control::BlockParamFloat _param_v_load_drop;
|
||||
|
||||
float _voltage_filtered_v;
|
||||
float _throttle_filtered;
|
||||
float _discharged_mah;
|
||||
float _remaining;
|
||||
uint8_t _warning;
|
||||
hrt_abstime _last_timestamp;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,123 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file battery_params.c
|
||||
*
|
||||
* Parameters defined by the battery lib.
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Empty cell voltage.
|
||||
*
|
||||
* Defines the voltage where a single cell of the battery is considered empty.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit V
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
|
||||
|
||||
/**
|
||||
* Full cell voltage.
|
||||
*
|
||||
* Defines the voltage where a single cell of the battery is considered full.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit V
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
|
||||
|
||||
/**
|
||||
* Voltage drop per cell on 100% load
|
||||
*
|
||||
* This implicitely defines the internal resistance
|
||||
* to maximum current ratio and assumes linearity.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit V
|
||||
* @min 0.0
|
||||
* @max 1.5
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
|
||||
|
||||
/**
|
||||
* Number of cells.
|
||||
*
|
||||
* Defines the number of cells the attached battery consists of.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit S
|
||||
* @min 2
|
||||
* @max 10
|
||||
* @value 2 2S Battery
|
||||
* @value 3 3S Battery
|
||||
* @value 4 4S Battery
|
||||
* @value 5 5S Battery
|
||||
* @value 6 6S Battery
|
||||
* @value 7 7S Battery
|
||||
* @value 8 8S Battery
|
||||
* @value 9 9S Battery
|
||||
* @value 10 10S Battery
|
||||
* @value 11 11S Battery
|
||||
* @value 12 12S Battery
|
||||
* @value 13 13S Battery
|
||||
* @value 14 14S Battery
|
||||
* @value 15 15S Battery
|
||||
* @value 16 16S Battery
|
||||
*/
|
||||
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
|
||||
|
||||
/**
|
||||
* Battery capacity.
|
||||
*
|
||||
* Defines the capacity of the attached battery.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit mA
|
||||
* @decimal 0
|
||||
* @min -1.0
|
||||
* @max 100000
|
||||
* @increment 50
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
|
||||
Reference in New Issue
Block a user