dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Refactored to work with new battery_status module

This commit is contained in:
Timothy Scott 2019-11-07 15:59:12 +01:00 committed by Julian Oes
parent d7bb5d46bb
commit 993fa5bd37
35 changed files with 1101 additions and 1837 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
View File

@ -420,7 +420,7 @@ Syslink::handle_message(syslink_message_t *msg)
}
/* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */
else if (powered && !charging && _battery_status.voltage_filtered_v > 3.7f) {
else if (powered && !charging && vbat > 3.7f) {
_bstate = BAT_CHARGED;
} else {

2
boards/bitcraze/crazyflie/syslink/syslink_main.h
View File

@ -137,7 +137,7 @@ private:
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
Battery1 _battery;
Battery _battery;
int32_t _rssi;
battery_state _bstate;

4
boards/intel/aerofc-v1/default.cmake
View File

@ -32,7 +32,7 @@ px4_add_board(
#uavcan
MODULES
#airspeed_selector
attitude_estimator_q
#attitude_estimator_q
battery_status
#camera_feedback
commander
@ -44,7 +44,7 @@ px4_add_board(
land_detector
landing_target_estimator
load_mon
local_position_estimator
#local_position_estimator
logger
mavlink
mc_att_control

1
msg/battery_status.msg
View File

@ -17,6 +17,7 @@ uint16 cycle_count # number of discharge cycles the battery has experien
uint16 run_time_to_empty # predicted remaining battery capacity based on the present rate of discharge in min
uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min
uint16 serial_number # serial number of the battery pack
uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed.
float32[4] voltage_cell_v # Battery individual cell voltages
float32 max_cell_voltage_delta # Max difference between individual cell voltages

10
src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
View File

@ -101,7 +101,7 @@ private:
orb_advert_t _battery_topic;
orb_advert_t _esc_topic;
Battery1 _battery;
Battery _battery;
bool _armed;
float _last_throttle;
@ -203,6 +203,10 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
const hrt_abstime timestamp = hrt_absolute_time();
// TODO Check if this is the right way for the Bebop
// We don't have current measurements
_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed, false);
esc_status_s esc_status = {};
uint16_t esc_speed_setpoint_rpm[4] = {};
@ -218,9 +222,7 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
// TODO Check if this is the right way for the Bebop
// We don't have current measurements
_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed, true);
_battery.publish();
if (_esc_topic == nullptr) {
_esc_topic = orb_advertise(ORB_ID(esc_status), &esc_status);

2
src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp
View File

@ -69,7 +69,7 @@ public:
private:
int _publish(const struct ltc2946_sensor_data &data);
Battery1 _battery{};
Battery _battery{};
int _actuator_ctrl_0_sub{-1};
int _vcontrol_mode_sub{-1};

10
src/drivers/power_monitor/voxlpm/voxlpm.cpp
View File

@ -78,7 +78,7 @@ VOXLPM::init()
write_reg(DEFAULT_CTRLA_REG_VAL, VOXLPM_LTC2946_CTRLA_REG);
write_reg(DEFAULT_CTRLB_REG_VAL, VOXLPM_LTC2946_CTRLB_REG);
_battery.reset(&_bat_status);
_battery.reset();
start();
@ -153,9 +153,7 @@ VOXLPM::measure()
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT: {
_battery.updateBatteryStatus(tnow, _voltage, _amperage, true, true, 0, 0, false, &_bat_status);
_bat_pub_topic.publish(_bat_status);
_battery.updateBatteryStatus(tnow, _voltage, _amperage, true, true, 0, 0, false, true);
}
// fallthrough
@ -176,9 +174,7 @@ VOXLPM::measure()
} else {
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT: {
_battery.updateBatteryStatus(tnow, 0.0, 0.0, true, true, 0, 0, false, &_bat_status);
_bat_pub_topic.publish(_bat_status);
_battery.updateBatteryStatus(tnow, 0.0, 0.0, true, true, 0, 0, false, true);
}
break;

2
src/drivers/power_monitor/voxlpm/voxlpm.hpp
View File

@ -160,10 +160,8 @@ private:
perf_counter_t _sample_perf;
uORB::PublicationMulti<battery_status_s> _bat_pub_topic{ORB_ID(battery_status)};
uORB::PublicationMulti<power_monitor_s> _pm_pub_topic{ORB_ID(power_monitor)};
battery_status_s _bat_status{};
power_monitor_s _pm_status{};
VOXLPM_CH_TYPE _ch_type;

7
src/lib/battery/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
# Copyright (c) 2019 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
@ -31,4 +31,7 @@
#
############################################################################
px4_add_library(battery battery_base.cpp battery.cpp)
px4_add_library(battery battery.cpp)
# TODO: Add an option in px4_add_library function to add module config file
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/module.yaml)

351
src/lib/battery/battery.cpp
View File

@ -1,14 +1,349 @@
/****************************************************************************
*
* Copyright (c) 2019 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>
* @author Timothy Scott <timothy@auterion.com>
*/
#include "battery.h"
#include <mathlib/mathlib.h>
#include <cstring>
#include <px4_platform_common/defines.h>
Battery1::Battery1()
Battery::Battery(int index, ModuleParams *parent) :
ModuleParams(parent),
_index(index < 1 || index > 9 ? 1 : index),
_warning(battery_status_s::BATTERY_WARNING_NONE),
_last_timestamp(0)
{
updateParams();
if (index > 9 || index < 1) {
PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
}
migrateParam(_param_old_bat_v_empty, _param_bat_v_empty, "V_EMPTY", 3.5f);
migrateParam(_param_old_bat_v_charged, _param_bat_v_charged, "V_CHARGED", 4.05f);
migrateParam(_param_old_bat_v_load_drop, _param_bat_v_load_drop, "V_LOAD_DROP", 0.3f);
migrateParam(_param_old_bat_r_internal, _param_bat_r_internal, "R_INTERNAL", -1.0f);
migrateParam(_param_old_bat_n_cells, _param_bat_n_cells, "N_CELLS", 0);
migrateParam(_param_old_bat_capacity, _param_bat_capacity, "CAPACITY", -1.0f);
// 16 chars for parameter name + null terminator
char param_name[17];
snprintf(param_name, sizeof(param_name), "BAT%d_V_EMPTY", _index);
_param_handles.v_empty = param_find(param_name);
if (_param_handles.v_empty == PARAM_INVALID) {
PX4_ERR("Could not find parameter with name %s", param_name);
}
snprintf(param_name, sizeof(param_name), "BAT%d_V_CHARGED", _index);
_param_handles.v_charged = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_N_CELLS", _index);
_param_handles.n_cells = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index);
_param_handles.capacity = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index);
_param_handles.v_load_drop = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index);
_param_handles.r_internal = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_SOURCE", _index);
_param_handles.source = param_find(param_name);
_param_handles.low_thr = param_find("BAT_LOW_THR");
_param_handles.crit_thr = param_find("BAT_CRIT_THR");
_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");
_param_handles.v_empty_old = param_find("BAT_V_EMPTY");
_param_handles.v_charged_old = param_find("BAT_V_CHARGED");
_param_handles.n_cells_old = param_find("BAT_N_CELLS");
_param_handles.capacity_old = param_find("BAT_CAPACITY");
_param_handles.v_load_drop_old = param_find("BAT_V_LOAD_DROP");
_param_handles.r_internal_old = param_find("BAT_R_INTERNAL");
_param_handles.source_old = param_find("BAT_SOURCE");
updateParams();
}
void
Battery::reset()
{
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.current_a = -1.f;
_battery_status.remaining = 1.f;
_battery_status.scale = 1.f;
_battery_status.cell_count = _params.n_cells;
// TODO: check if it is sane to reset warning to NONE
_battery_status.warning = battery_status_s::BATTERY_WARNING_NONE;
_battery_status.connected = false;
_battery_status.capacity = _params.capacity;
_battery_status.temperature = NAN;
_battery_status.id = (uint8_t) _index;
}
void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
bool connected, bool selected_source, int priority,
float throttle_normalized,
bool armed, bool should_publish)
{
reset();
_battery_status.timestamp = timestamp;
filterVoltage(voltage_v);
filterThrottle(throttle_normalized);
filterCurrent(current_a);
sumDischarged(timestamp, current_a);
estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed);
computeScale();
if (_battery_initialized) {
determineWarning(connected);
}
if (_voltage_filtered_v > 2.1f) {
_battery_initialized = true;
_battery_status.voltage_v = voltage_v;
_battery_status.voltage_filtered_v = _voltage_filtered_v;
_battery_status.scale = _scale;
_battery_status.current_a = current_a;
_battery_status.current_filtered_a = _current_filtered_a;
_battery_status.discharged_mah = _discharged_mah;
_battery_status.warning = _warning;
_battery_status.remaining = _remaining;
_battery_status.connected = connected;
_battery_status.system_source = selected_source;
_battery_status.priority = priority;
}
_battery_status.timestamp = timestamp;
if (should_publish) {
publish();
}
}
void
Battery::publish()
{
orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT);
}
void
Battery::swapUorbAdvert(Battery &other)
{
orb_advert_t tmp = _orb_advert;
_orb_advert = other._orb_advert;
other._orb_advert = tmp;
}
void
Battery::filterVoltage(float voltage_v)
{
if (!_battery_initialized) {
_voltage_filtered_v = voltage_v;
}
// TODO: inspect that filter performance
const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;
if (PX4_ISFINITE(filtered_next)) {
_voltage_filtered_v = filtered_next;
}
}
void
Battery::filterCurrent(float current_a)
{
if (!_battery_initialized) {
_current_filtered_a = current_a;
}
// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;
if (PX4_ISFINITE(filtered_next)) {
_current_filtered_a = filtered_next;
}
}
void Battery::filterThrottle(float throttle)
{
if (!_battery_initialized) {
_throttle_filtered = throttle;
}
const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f;
if (PX4_ISFINITE(filtered_next)) {
_throttle_filtered = filtered_next;
}
}
void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{
// Not a valid measurement
if (current_a < 0.f) {
// Because the measurement was invalid we need to stop integration
// and re-initialize with the next valid measurement
_last_timestamp = 0;
return;
}
// Ignore first update because we don't know dt.
if (_last_timestamp != 0) {
const float dt = (timestamp - _last_timestamp) / 1e6;
// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
_discharged_mah += _discharged_mah_loop;
}
_last_timestamp = timestamp;
}
void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
{
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _params.n_cells;
// correct battery voltage locally for load drop to avoid estimation fluctuations
if (_params.r_internal >= 0.f) {
cell_voltage += _params.r_internal * current_a;
} else {
// assume linear relation between throttle and voltage drop
cell_voltage += throttle * _params.v_load_drop;
}
_remaining_voltage = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
// choose which quantity we're using for final reporting
if (_params.capacity > 0.f) {
// if battery capacity is known, fuse voltage measurement with used capacity
if (!_battery_initialized) {
// initialization of the estimation state
_remaining = _remaining_voltage;
} else {
// The lower the voltage the more adjust the estimate with it to avoid deep discharge
const float weight_v = 3e-4f * (1 - _remaining_voltage);
_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
// directly apply current capacity slope calculated using current
_remaining -= _discharged_mah_loop / _params.capacity;
_remaining = math::max(_remaining, 0.f);
}
} else {
// else use voltage
_remaining = _remaining_voltage;
}
}
void
Battery::determineWarning(bool connected)
{
if (connected) {
// propagate warning state only if the state is higher, otherwise remain in current warning state
if (_remaining < _params.emergen_thr) {
_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
} else if (_remaining < _params.crit_thr) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (_remaining < _params.low_thr) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
} else {
_warning = battery_status_s::BATTERY_WARNING_NONE;
}
}
}
void
Battery::computeScale()
{
const float voltage_range = (_params.v_charged - _params.v_empty);
// reusing capacity calculation to get single cell voltage before drop
const float bat_v = _params.v_empty + (voltage_range * _remaining_voltage);
_scale = _params.v_charged / bat_v;
if (_scale > 1.3f) { // Allow at most 30% compensation
_scale = 1.3f;
} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
_scale = 1.f;
}
}
void Battery::updateParams()
{
if (_index == 1) {
migrateParam<float>(_param_handles.v_empty_old, _param_handles.v_empty, &_params.v_empty_old, &_params.v_empty,
_first_parameter_update);
migrateParam<float>(_param_handles.v_charged_old, _param_handles.v_charged, &_params.v_charged_old, &_params.v_charged,
_first_parameter_update);
migrateParam<int>(_param_handles.n_cells_old, _param_handles.n_cells, &_params.n_cells_old, &_params.n_cells,
_first_parameter_update);
migrateParam<float>(_param_handles.capacity_old, _param_handles.capacity, &_params.capacity_old, &_params.capacity,
_first_parameter_update);
migrateParam<float>(_param_handles.v_load_drop_old, _param_handles.v_load_drop, &_params.v_load_drop_old,
&_params.v_load_drop, _first_parameter_update);
migrateParam<float>(_param_handles.r_internal_old, _param_handles.r_internal, &_params.r_internal_old,
&_params.r_internal, _first_parameter_update);
migrateParam<int>(_param_handles.source_old, _param_handles.source, &_params.source_old, &_params.source,
_first_parameter_update);
} else {
param_get(_param_handles.v_empty, &_params.v_empty);
param_get(_param_handles.v_charged, &_params.v_charged);
param_get(_param_handles.n_cells, &_params.n_cells);
param_get(_param_handles.capacity, &_params.capacity);
param_get(_param_handles.v_load_drop, &_params.v_load_drop);
param_get(_param_handles.r_internal, &_params.r_internal);
param_get(_param_handles.source, &_params.source);
}
param_get(_param_handles.low_thr, &_params.low_thr);
param_get(_param_handles.crit_thr, &_params.crit_thr);
param_get(_param_handles.emergen_thr, &_params.emergen_thr);
ModuleParams::updateParams();
_first_parameter_update = false;
}

337
src/lib/battery/battery.h
View File

@ -31,157 +31,228 @@
*
****************************************************************************/
#pragma once
#include "battery_base.h"
#include <px4_log.h>
#include <math.h>
/**
* @file battery.h
* Basic implementation of BatteryBase. Battery1 is calibrated by BAT1_* parameters. Battery2 is calibrated
* by BAT2_* parameters.
*
* The multiple batteries all share the same logic for calibration. The only difference is which parameters are used
* (Battery 1 uses `BAT1_*`, while Battery 2 uses `BAT2_*`). To avoid code duplication, inheritance is being used.
* The problem is that the `ModuleParams` class depends on a macro which defines member variables. You can't override
* member variables in C++, so we have to declare virtual getter functions in BatteryBase, and implement them here.
* Library calls for battery functionality.
*
* The alternative would be to avoid ModuleParams entirely, and build parameter names dynamically, like so:
* ```
* char param_name[17]; //16 max length of parameter name, + null terminator
* int battery_index = 1; // Or 2 or 3 or whatever
* snprintf(param_name, 17, "BAT%d_N_CELLS", battery_index);
* param_find(param_name); // etc
* ```
*
* This was decided against because the newer ModuleParams API provides more type safety and avoids code duplication.
*
* To add a third battery, follow these steps:
* - Copy/Paste all of Battery2 to make Battery3
* - Change all "BAT2_*" parameters to "BAT3_*" in Battery3
* - Copy the file "battery_params_2.c" to "battery_params_3.c"
* - Change all of the "BAT2_*" params in "battery_params_3.c" to "BAT3_*"
* This is not done now because there is not yet any demand for a third battery, and adding parameters uses up space.
* @author Julian Oes <julian@oes.ch>
* @author Timothy Scott <timothy@auterion.com>
*/
#pragma once
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
#include <parameters/param.h>
#include <drivers/drv_adc.h>
#include <board_config.h>
#include <px4_platform_common/board_common.h>
#include <math.h>
#include <float.h>
/**
* Battery1 represents a battery calibrated by BAT1_* parameters.
* BatteryBase is a base class for any type of battery.
*
* You can use this class on its own. Or, if you need to implement a custom battery type,
* you can inherit from this class. See, for example, src/modules/battery_status/AnalogBattery.h
*/
class Battery1 : public BatteryBase
class Battery : public ModuleParams
{
public:
Battery1();
Battery(int index = 1, ModuleParams *parent = nullptr);
/**
* This function migrates the old deprecated parameters like BAT_N_CELLS to the new parameters like BAT1_N_CELLS.
* It checks if the old parameter is non-defaulT AND the new parameter IS default, and if so, it:
* - Issues a warning using PX4_WARN(...)
* - Copies the value of the old parameter over to the new parameter
* - Resets the old parameter to its default
*
* The 'name' parameter should be only the part of the parameter name that comes after "BAT1_" or "BAT_". It is
* used only for the warning message. For example, for parameter BAT1_N_CELLS, name should be "N_CELLS".
* (See the implementation of this function for why I have taken this strange choice)
*
* In an ideal world, this function would be protected so that only child classes of Battery1 could access it.
* However, the way ModuleParams works makes it very difficult to inherit from a ModuleParams class.
* For example, the AnalogBattery class in the Sensors module does not inherit this class; it just contains
* a Battery1 member variable.
*
* The templating is complicated because every parameter is technically a different type. However, in normal
* use, the template can just be ignored. See the implementation of Battery1::Battery1() for example usage.
*
* @tparam P1 Type of the first parameter
* @tparam P2 Type of the second parameter
* @tparam T Data type for the default value
* @param oldParam Reference to the old parameter, as a ParamFloat<...>, ParamInt<...>, or ParamBool<...>
* @param newParam Reference to the new paramater, as a ParamFloat<...>, ParamInt<...>, or ParamBool<...>
* @param name The name of the parameter, WITHOUT the "BAT_" or "BAT1_" prefix. This is used only for logging.
* @param defaultValue Default value of the parameter, as specified in PARAM_DEFINE_*(...)
* Reset all battery stats and report invalid/nothing.
*/
template<class P1, class P2, typename T> static void
migrateParam(P1 &oldParam, P2 &newParam, const char *name, T defaultValue)
{
float diffOld = fabs((float) oldParam.get() - defaultValue);
float diffNew = fabs((float) newParam.get() - defaultValue);
void reset();
if (diffOld > 0.0001f && diffNew < 0.0001f) {
PX4_WARN("Parameter BAT_%s is deprecated. Copying value to BAT1_%s.", name, name);
newParam.set(oldParam.get());
oldParam.set(defaultValue);
newParam.commit();
oldParam.commit();
/**
* Get the battery cell count
*/
int cell_count() { return _params.n_cells; }
/**
* Get the empty voltage per cell
*/
float empty_cell_voltage() { return _params.v_empty; }
/**
* Get the full voltage per cell
*/
float full_cell_voltage() { return _params.v_charged; }
int source() { return _params.source; }
/**
* Update current battery status message.
*
* @param voltage_raw: Battery voltage, in Volts
* @param current_raw: Battery current, in Amps
* @param timestamp: Time at which the ADC was read (use hrt_absolute_time())
* @param selected_source: This battery is on the brick that the selected source for selected_source
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
* @param throttle_normalized: Throttle of the vehicle, between 0 and 1
* @param armed: Arming state of the vehicle
* @param should_publish If True, this function published a battery_status uORB message.
*/
void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected,
bool selected_source, int priority, float throttle_normalized, bool armed, bool should_publish);
/**
* Publishes the uORB battery_status message with the most recently-updated data.
*/
void publish();
/**
* Some old functionality expects the primary battery to be published on instance 0. To maintain backwards
* compatibility, this function allows the advertisements (and therefore instances) of 2 batteries to be swapped.
* However, this should not be relied upon anywhere, and should be considered for all intents deprecated.
*
* The proper way to uniquely identify batteries is by the `id` field in the `battery_status` message.
*/
void swapUorbAdvert(Battery &other);
protected:
struct {
param_t v_empty;
param_t v_charged;
param_t n_cells;
param_t capacity;
param_t v_load_drop;
param_t r_internal;
param_t low_thr;
param_t crit_thr;
param_t emergen_thr;
param_t source;
// TODO: These parameters are depracated. They can be removed entirely once the
// new version of Firmware has been around for long enough.
param_t v_empty_old;
param_t v_charged_old;
param_t n_cells_old;
param_t capacity_old;
param_t v_load_drop_old;
param_t r_internal_old;
param_t source_old;
} _param_handles;
struct {
float v_empty;
float v_charged;
int n_cells;
float capacity;
float v_load_drop;
float r_internal;
float low_thr;
float crit_thr;
float emergen_thr;
int source;
// TODO: These parameters are depracated. They can be removed entirely once the
// new version of Firmware has been around for long enough.
float v_empty_old;
float v_charged_old;
int n_cells_old;
float capacity_old;
float v_load_drop_old;
float r_internal_old;
int source_old;
} _params;
battery_status_s _battery_status;
const int _index;
bool _first_parameter_update{false};
virtual void updateParams() override;
/**
* This function helps with migrating to new parameters. It performs several tasks:
* - Update both the old and new parameter values using `param_get(...)`
* - Check if either parameter changed just now
* - If so, display a warning if the deprecated parameter was used
* - Copy the new value over to the other parameter
* - If this is the first time the parameters are fetched, check if they are equal
* - If not, display a warning and copy the value of the deprecated parameter over to the new one
* @tparam T Type of the parameter (int or float)
* @param old_param Handle to the old deprecated parameter (for example, param_find("BAT_N_CELLS")
* @param new_param Handle to the new replacement parameter (for example, param_find("BAT1_N_CELLS")
* @param old_val Pointer to the value of the old deprecated parameter
* @param new_val Pointer to the value of the new replacement parameter
* @param firstcall If true, then this function will not check to see if the values have changed
* (Since the old values are uninitialized)
* @return True iff either of these parameters changed just now and the migration was done.
*/
template<typename T>
bool migrateParam(param_t old_param, param_t new_param, T *old_val, T *new_val, bool firstcall)
{
T previous_old_val = *old_val;
T previous_new_val = *new_val;
param_get(old_param, old_val);
param_get(new_param, new_val);
if (!firstcall) {
if ((float) fabs((float) *old_val - (float) previous_old_val) > FLT_EPSILON
&& (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) {
// TODO fix this error message. I was getting hardfaults while using this message, but they stopped
// after removing this message. I was unable to determine why I was getting them.
PX4_WARN("Detected change of deprecated parameter %s. Please use the new parameter %s instead. "
"The new value of the deprecated parameter will be copied to the new parameter.",
param_name(old_param), param_name(new_param));
param_set_no_notification(new_param, old_val);
param_get(new_param, new_val);
return true;
} else if ((float) fabs((float) *new_val - (float) previous_new_val) > FLT_EPSILON
&& (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) {
PX4_INFO("Copying new value for %s to deprecated parameter %s.",
param_name(new_param), param_name(old_param));
param_set_no_notification(old_param, new_val);
param_get(old_param, old_val);
return true;
}
} else {
if ((float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) {
PX4_WARN("At boot, deprecated parameter %s is different from its replacement %s."
" The new value will be overwritten by the old one. This should happen only once.",
param_name(old_param), param_name(new_param));
param_set_no_notification(new_param, old_val);
param_get(new_param, new_val);
return true;
}
}
return false;
}
private:
void filterVoltage(float voltage_v);
void filterThrottle(float throttle);
void filterCurrent(float current_a);
void sumDischarged(hrt_abstime timestamp, float current_a);
void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed);
void determineWarning(bool connected);
void computeScale();
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_V_EMPTY>) _param_old_bat_v_empty,
(ParamFloat<px4::params::BAT_V_CHARGED>) _param_old_bat_v_charged,
(ParamInt<px4::params::BAT_N_CELLS>) _param_old_bat_n_cells,
(ParamFloat<px4::params::BAT_CAPACITY>) _param_old_bat_capacity,
(ParamFloat<px4::params::BAT_V_LOAD_DROP>) _param_old_bat_v_load_drop,
(ParamFloat<px4::params::BAT_R_INTERNAL>) _param_old_bat_r_internal,
bool _battery_initialized = false;
float _voltage_filtered_v = -1.f;
float _throttle_filtered = -1.f;
float _current_filtered_a = -1.f;
float _discharged_mah = 0.f;
float _discharged_mah_loop = 0.f;
float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage
float _remaining = -1.f; ///< normalized battery charge level, selected based on config param
float _scale = 1.f;
uint8_t _warning;
hrt_abstime _last_timestamp;
(ParamFloat<px4::params::BAT1_V_EMPTY>) _param_bat_v_empty,
(ParamFloat<px4::params::BAT1_V_CHARGED>) _param_bat_v_charged,
(ParamInt<px4::params::BAT1_N_CELLS>) _param_bat_n_cells,
(ParamFloat<px4::params::BAT1_CAPACITY>) _param_bat_capacity,
(ParamFloat<px4::params::BAT1_V_LOAD_DROP>) _param_bat_v_load_drop,
(ParamFloat<px4::params::BAT1_R_INTERNAL>) _param_bat_r_internal,
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
(ParamInt<px4::params::BAT_SOURCE>) _param_source
)
float _get_bat_v_empty() override {return _param_bat_v_empty.get(); }
float _get_bat_v_charged() override {return _param_bat_v_charged.get(); }
int _get_bat_n_cells() override {return _param_bat_n_cells.get(); }
float _get_bat_capacity() override {return _param_bat_capacity.get(); }
float _get_bat_v_load_drop() override {return _param_bat_v_load_drop.get(); }
float _get_bat_r_internal() override {return _param_bat_r_internal.get(); }
float _get_bat_low_thr() override {return _param_bat_low_thr.get(); }
float _get_bat_crit_thr() override {return _param_bat_crit_thr.get(); }
float _get_bat_emergen_thr() override {return _param_bat_emergen_thr.get(); }
int _get_source() override {return _param_source.get(); }
orb_advert_t _orb_advert{nullptr};
int _orb_instance;
};
/**
* Battery2 represents a battery calibrated by BAT2_* parameters.
*/
class Battery2 : public BatteryBase
{
public:
Battery2() {}
private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT2_V_EMPTY>) _param_bat_v_empty,
(ParamFloat<px4::params::BAT2_V_CHARGED>) _param_bat_v_charged,
(ParamInt<px4::params::BAT2_N_CELLS>) _param_bat_n_cells,
(ParamFloat<px4::params::BAT2_CAPACITY>) _param_bat_capacity,
(ParamFloat<px4::params::BAT2_V_LOAD_DROP>) _param_bat_v_load_drop,
(ParamFloat<px4::params::BAT2_R_INTERNAL>) _param_bat_r_internal,
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
(ParamInt<px4::params::BAT_SOURCE>) _param_source
)
float _get_bat_v_empty() override {return _param_bat_v_empty.get(); }
float _get_bat_v_charged() override {return _param_bat_v_charged.get(); }
int _get_bat_n_cells() override {return _param_bat_n_cells.get(); }
float _get_bat_capacity() override {return _param_bat_capacity.get(); }
float _get_bat_v_load_drop() override {return _param_bat_v_load_drop.get(); }
float _get_bat_r_internal() override {return _param_bat_r_internal.get(); }
float _get_bat_low_thr() override {return _param_bat_low_thr.get(); }
float _get_bat_crit_thr() override {return _param_bat_crit_thr.get(); }
float _get_bat_emergen_thr() override {return _param_bat_emergen_thr.get(); }
int _get_source() override {return _param_source.get(); }
};

256
src/lib/battery/battery_base.cpp
View File

@ -1,256 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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_base.cpp
*
* Library calls for battery functionality.
*
* @author Julian Oes <julian@oes.ch>
* @author Timothy Scott <timothy@auterion.com>
*/
#include "battery_base.h"
#include <mathlib/mathlib.h>
#include <cstring>
#include <px4_platform_common/defines.h>
BatteryBase::BatteryBase() :
ModuleParams(nullptr),
_warning(battery_status_s::BATTERY_WARNING_NONE),
_last_timestamp(0)
{
}
void
BatteryBase::reset()
{
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.current_a = -1.f;
_battery_status.remaining = 1.f;
_battery_status.scale = 1.f;
_battery_status.cell_count = _get_bat_n_cells();
// TODO: check if it is sane to reset warning to NONE
_battery_status.warning = battery_status_s::BATTERY_WARNING_NONE;
_battery_status.connected = false;
_battery_status.capacity = _get_bat_capacity();
}
void
BatteryBase::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected,
bool selected_source, int priority, float throttle_normalized, bool armed, bool should_publish)
{
reset();
_battery_status.timestamp = timestamp;
filterVoltage(voltage_v);
filterThrottle(throttle_normalized);
filterCurrent(current_a);
sumDischarged(timestamp, current_a);
estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed);
computeScale();
if (_battery_initialized) {
determineWarning(connected);
}
if (_voltage_filtered_v > 2.1f) {
_battery_initialized = true;
_battery_status.voltage_v = voltage_v;
_battery_status.voltage_filtered_v = _voltage_filtered_v;
_battery_status.scale = _scale;
_battery_status.current_a = current_a;
_battery_status.current_filtered_a = _current_filtered_a;
_battery_status.discharged_mah = _discharged_mah;
_battery_status.warning = _warning;
_battery_status.remaining = _remaining;
_battery_status.connected = connected;
_battery_status.system_source = selected_source;
_battery_status.priority = priority;
}
_battery_status.timestamp = timestamp;
if (should_publish) {
publish();
}
_battery_status.temperature = NAN;
}
void
BatteryBase::publish()
{
orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT);
}
void
BatteryBase::filterVoltage(float voltage_v)
{
if (!_battery_initialized) {
_voltage_filtered_v = voltage_v;
}
// TODO: inspect that filter performance
const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;
if (PX4_ISFINITE(filtered_next)) {
_voltage_filtered_v = filtered_next;
}
}
void
BatteryBase::filterCurrent(float current_a)
{
if (!_battery_initialized) {
_current_filtered_a = current_a;
}
// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;
if (PX4_ISFINITE(filtered_next)) {
_current_filtered_a = filtered_next;
}
}
void BatteryBase::filterThrottle(float throttle)
{
if (!_battery_initialized) {
_throttle_filtered = throttle;
}
const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f;
if (PX4_ISFINITE(filtered_next)) {
_throttle_filtered = filtered_next;
}
}
void
BatteryBase::sumDischarged(hrt_abstime timestamp, float current_a)
{
// Not a valid measurement
if (current_a < 0.f) {
// Because the measurement was invalid we need to stop integration
// and re-initialize with the next valid measurement
_last_timestamp = 0;
return;
}
// Ignore first update because we don't know dt.
if (_last_timestamp != 0) {
const float dt = (timestamp - _last_timestamp) / 1e6;
// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
_discharged_mah += _discharged_mah_loop;
}
_last_timestamp = timestamp;
}
void
BatteryBase::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
{
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _get_bat_n_cells();
// correct battery voltage locally for load drop to avoid estimation fluctuations
if (_get_bat_r_internal() >= 0.f) {
cell_voltage += _get_bat_r_internal() * current_a;
} else {
// assume linear relation between throttle and voltage drop
cell_voltage += throttle * _get_bat_v_load_drop();
}
_remaining_voltage = math::gradual(cell_voltage, _get_bat_v_empty(), _get_bat_v_charged(), 0.f, 1.f);
// choose which quantity we're using for final reporting
if (_get_bat_capacity() > 0.f) {
// if battery capacity is known, fuse voltage measurement with used capacity
if (!_battery_initialized) {
// initialization of the estimation state
_remaining = _remaining_voltage;
} else {
// The lower the voltage the more adjust the estimate with it to avoid deep discharge
const float weight_v = 3e-4f * (1 - _remaining_voltage);
_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
// directly apply current capacity slope calculated using current
_remaining -= _discharged_mah_loop / _get_bat_capacity();
_remaining = math::max(_remaining, 0.f);
}
} else {
// else use voltage
_remaining = _remaining_voltage;
}
}
void
BatteryBase::determineWarning(bool connected)
{
if (connected) {
// propagate warning state only if the state is higher, otherwise remain in current warning state
if (_remaining < _get_bat_emergen_thr()) {
_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
} else if (_remaining < _get_bat_crit_thr()) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (_remaining < _get_bat_low_thr()) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
} else {
_warning = battery_status_s::BATTERY_WARNING_NONE;
}
}
}
void
BatteryBase::computeScale()
{
const float voltage_range = (_get_bat_v_charged() - _get_bat_v_empty());
// reusing capacity calculation to get single cell voltage before drop
const float bat_v = _get_bat_v_empty() + (voltage_range * _remaining_voltage);
_scale = _get_bat_v_charged() / bat_v;
if (_scale > 1.3f) { // Allow at most 30% compensation
_scale = 1.3f;
} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
_scale = 1.f;
}
}

183
src/lib/battery/battery_base.h
View File

@ -1,183 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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_base.h
*
* Library calls for battery functionality.
*
* @author Julian Oes <julian@oes.ch>
* @author Timothy Scott <timothy@auterion.com>
*/
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/battery_status.h>
#include <drivers/drv_hrt.h>
#include <px4_module_params.h>
#include <drivers/drv_adc.h>
#include <board_config.h>
#include <drivers/boards/common/board_common.h>
/**
* BatteryBase is a base class for any type of battery.
*
* Each of the virtual _get_*() functions corresponds to a parameter used for calibration. However, depending
* on the type of battery, these values may not come from parameters.
*
* Most implementations of BatteryBase will also inherit ModuleParams, and most of the virtual function
* implementations will look something like:
* float _get_bat_v_empty() override {return _param_bat_v_empty().get();}
*
* For a full implementation, see src/modules/sensors/analog_battery.{cpp, h}
* For a minimal implementation, see src/modules/simulator/simulator.h
*/
class BatteryBase : ModuleParams
{
public:
BatteryBase();
/**
* Reset all battery stats and report invalid/nothing.
*/
void reset();
/**
* Get the battery cell count
*/
int cell_count() { return _get_bat_n_cells(); }
/**
* Get the empty voltage per cell
*/
float empty_cell_voltage() { return _get_bat_v_empty(); }
/**
* Get the full voltage per cell
*/
float full_cell_voltage() { return _get_bat_v_charged(); }
/**
* Update current battery status message.
*
* @param voltage_raw Battery voltage read from ADC, in Volts
* @param current_raw Voltage of current sense resistor, in Amps
* @param timestamp Time at which the ADC was read (use hrt_absolute_time())
* @param selected_source This battery is on the brick that the selected source for selected_source
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
* @param throttle_normalized Throttle of the vehicle, between 0 and 1
* @param armed Arming state of the vehicle
* @param should_publish If True, this function published a battery_status uORB message.
*/
void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected,
bool selected_source, int priority, float throttle_normalized, bool armed, bool should_publish);
/**
* Publishes the uORB battery_status message with the most recently-updated data.
*/
void publish();
// TODO: Check that there was actually a parameter update
void updateParams() {ModuleParams::updateParams();}
protected:
/**
* @return Value, in volts, at which a single cell of the battery would be considered empty.
*/
virtual float _get_bat_v_empty() = 0;
/**
* @return Value, in volts, at which a single cell of the battery would be considered full.
*/
virtual float _get_bat_v_charged() = 0;
/**
* @return Number of cells in series in the battery.
*/
virtual int _get_bat_n_cells() = 0;
/**
* @return Total capacity of the battery, in mAh
*/
virtual float _get_bat_capacity() = 0;
/**
* @return Voltage drop per cell at full throttle. This implicitly defines the internal resistance of the
* battery. If _get_bat_r_internal() >= to 0, then _get_bat_v_load_drop() is ignored.
*/
virtual float _get_bat_v_load_drop() = 0;
/**
* @return Internal resistance of the battery, in Ohms. If non-negative, then this is used instead of
* _get_bat_v_load_drop().
*/
virtual float _get_bat_r_internal() = 0;
/**
* @return Low battery threshold, as fraction between 0 and 1.
*/
virtual float _get_bat_low_thr() = 0;
/**
* @return Critical battery threshold, as fraction between 0 and 1.
*/
virtual float _get_bat_crit_thr() = 0;
/**
* @return Emergency battery threshold, as fraction between 0 and 1.
*/
virtual float _get_bat_emergen_thr() = 0;
/**
* @return Source of battery data. 0 = internal power module. 1 = external, via Mavlink
*/
virtual int _get_source() = 0;
battery_status_s _battery_status;
private:
void filterVoltage(float voltage_v);
void filterThrottle(float throttle);
void filterCurrent(float current_a);
void sumDischarged(hrt_abstime timestamp, float current_a);
void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed);
void determineWarning(bool connected);
void computeScale();
bool _battery_initialized = false;
float _voltage_filtered_v = -1.f;
float _throttle_filtered = -1.f;
float _current_filtered_a = -1.f;
float _discharged_mah = 0.f;
float _discharged_mah_loop = 0.f;
float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage
float _remaining = -1.f; ///< normalized battery charge level, selected based on config param
float _scale = 1.f;
uint8_t _warning;
hrt_abstime _last_timestamp;
orb_advert_t _orbAdvert{nullptr};
int _orbInstance;
};

184
src/lib/battery/battery_params_1.c
View File

@ -1,184 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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_1.c
* @author Timothy Scott <timothy@auterion.com>
*
* Defines parameters for Battery 1. For backwards compatibility, the
* parameter names do not have a "1" in them.
*/
#include <px4_platform_common/px4_config.h>
#include <parameters/param.h>
/**
* Empty cell voltage (5C load)
*
* Defines the voltage where a single cell of battery 1 is considered empty.
* The voltage should be chosen before the steep dropoff to 2.8V. A typical
* lithium battery can only be discharged down to 10% before it drops off
* to a voltage level damaging the cells.
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT1_V_EMPTY, 3.5f);
/**
* Full cell voltage (5C load)
*
* Defines the voltage where a single cell of battery 1 is considered full
* under a mild load. This will never be the nominal voltage of 4.2V
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT1_V_CHARGED, 4.05f);
/**
* Voltage drop per cell on full throttle
*
* This implicitely defines the internal resistance
* to maximum current ratio for battery 1 and assumes linearity.
* A good value to use is the difference between the
* 5C and 20-25C load. Not used if BAT1_R_INTERNAL is
* set.
*
* @group Battery Calibration
* @unit V
* @min 0.07
* @max 0.5
* @decimal 2
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT1_V_LOAD_DROP, 0.3f);
/**
* Explicitly defines the per cell internal resistance for battery 1
*
* If non-negative, then this will be used in place of
* BAT1_V_LOAD_DROP for all calculations.
*
* @group Battery Calibration
* @unit Ohms
* @min -1.0
* @max 0.2
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT1_R_INTERNAL, -1.0f);
/**
* Number of cells for battery 1.
*
* Defines the number of cells the attached battery consists of.
*
* @group Battery Calibration
* @unit S
* @value 0 Unconfigured
* @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
* @reboot_required true
*/
PARAM_DEFINE_INT32(BAT1_N_CELLS, 0);
/**
* Battery 1 capacity.
*
* Defines the capacity of battery 1.
*
* @group Battery Calibration
* @unit mAh
* @decimal 0
* @min -1.0
* @max 100000
* @increment 50
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT1_CAPACITY, -1.0f);
/**
* Battery 1 voltage divider (V divider)
*
* This is the divider from battery 1 voltage to 3.3V ADC voltage.
* If using e.g. Mauch power modules the value from the datasheet
* can be applied straight here. A value of -1 means to use
* the board default.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT1_V_DIV, -1.0);
/**
* Battery 1 current per volt (A/V)
*
* The voltage seen by the 3.3V ADC multiplied by this factor
* will determine the battery current. A value of -1 means to use
* the board default.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT1_A_PER_V, -1.0);
/**
* Battery 1 ADC Channel
*
* This parameter specifies the ADC channel used to monitor voltage of main power battery.
* A value of -1 means to use the board default.
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT1_ADC_CHANNEL, -1);

180
src/lib/battery/battery_params_2.c
View File

@ -1,180 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 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_2.c
* @author Timothy Scott <timothy@auterion.com>
*
* Defines parameters for Battery 2.
*/
/**
* Empty cell voltage (5C load)
*
* Defines the voltage where a single cell of battery 2 is considered empty.
* The voltage should be chosen before the steep dropoff to 2.8V. A typical
* lithium battery can only be discharged down to 10% before it drops off
* to a voltage level damaging the cells.
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT2_V_EMPTY, 3.5f);
/**
* Full cell voltage (5C load)
*
* Defines the voltage where a single cell of battery 2 is considered full
* under a mild load. This will never be the nominal voltage of 4.2V
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT2_V_CHARGED, 4.05f);
/**
* Voltage drop per cell on full throttle
*
* This implicitely defines the internal resistance
* to maximum current ratio for battery 2 and assumes linearity.
* A good value to use is the difference between the
* 5C and 20-25C load. Not used if BAT2_R_INTERNAL is
* set.
*
* @group Battery Calibration
* @unit V
* @min 0.07
* @max 0.5
* @decimal 2
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT2_V_LOAD_DROP, 0.3f);
/**
* Explicitly defines the per cell internal resistance for battery 2
*
* If non-negative, then this will be used in place of
* BAT2_V_LOAD_DROP for all calculations.
*
* @group Battery Calibration
* @unit Ohms
* @min -1.0
* @max 0.2
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT2_R_INTERNAL, -1.0f);
/**
* Number of cells for battery 2.
*
* Defines the number of cells the attached battery consists of.
*
* @group Battery Calibration
* @unit S
* @value 0 Unconfigured
* @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
* @reboot_required true
*/
PARAM_DEFINE_INT32(BAT2_N_CELLS, 0);
/**
* Battery 2 capacity.
*
* Defines the capacity of battery 2.
*
* @group Battery Calibration
* @unit mAh
* @decimal 0
* @min -1.0
* @max 100000
* @increment 50
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT2_CAPACITY, -1.0f);
/**
* Battery 2 voltage divider (V divider)
*
* This is the divider from battery 2 voltage to 3.3V ADC voltage.
* If using e.g. Mauch power modules the value from the datasheet
* can be applied straight here. A value of -1 means to use
* the board default.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT2_V_DIV, -1.0);
/**
* Battery 2 current per volt (A/V)
*
* The voltage seen by the 3.3V ADC multiplied by this factor
* will determine the battery current. A value of -1 means to use
* the board default.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT2_A_PER_V, -1.0);
/**
* Battery 2 ADC Channel
*
* This parameter specifies the ADC channel used to monitor voltage of main power battery.
* A value of -1 means to use the board default.
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT2_ADC_CHANNEL, -1);

54
src/lib/battery/battery_params_common.c
View File

@ -39,10 +39,6 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_config.h>
#include <parameters/param.h>
/**
* Low threshold
*
@ -92,53 +88,3 @@ PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f);
/**
* Scaling from ADC counts to volt on the ADC input (battery voltage)
*
* This is not the battery voltage, but the intermediate ADC voltage.
* A value of -1 signifies that the board defaults are used, which is
* highly recommended.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f);
/**
* Scaling from ADC counts to volt on the ADC input (battery current)
*
* This is not the battery current, but the intermediate ADC voltage.
* A value of -1 signifies that the board defaults are used, which is
* highly recommended.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0);
/**
* Offset in volt as seen by the ADC input of the current sensor.
*
* This offset will be subtracted before calculating the battery
* current based on the voltage.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);
/**
* Battery monitoring source.
*
* This parameter controls the source of battery data. The value 'Power Module'
* means that measurements are expected to come from a power module. If the value is set to
* 'External' then the system expects to receive mavlink battery status messages.
*
* @min 0
* @max 1
* @value 0 Power Module
* @value 1 External
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_SOURCE, 0);

38
src/lib/battery/battery_params_deprecated.c
View File

@ -146,36 +146,18 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 0);
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
/**
* This parameter is deprecated. Please use BAT1_V_DIV instead.
* This parameter is deprecated. Please use BAT1_SOURCE instead.
*
* This is the divider from battery 1 voltage to 3.3V ADC voltage.
* If using e.g. Mauch power modules the value from the datasheet
* can be applied straight here. A value of -1 means to use
* the board default.
* Battery monitoring source.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
/**
* This parameter is deprecated. Please use BAT1_A_PER_V instead.
*
* The voltage seen by the 3.3V ADC multiplied by this factor
* will determine the battery current. A value of -1 means to use
* the board default.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
/**
* This parameter is deprecated. Please use BAT1_ADC_CHANNEL instead.
*
* This parameter specifies the ADC channel used to monitor voltage of main power battery.
* A value of -1 means to use the board default.
* This parameter controls the source of battery data. The value 'Power Module'
* means that measurements are expected to come from a power module. If the value is set to
* 'External' then the system expects to receive mavlink battery status messages.
*
* @min 0
* @max 1
* @value 0 Power Module
* @value 1 External
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);
PARAM_DEFINE_INT32(BAT_SOURCE, 0);

140
src/lib/battery/module.yaml Normal file
View File

@ -0,0 +1,140 @@
__max_num_config_instances: &max_num_config_instances 2
module_name: battery
parameters:
- group: Battery Calibration
definitions:
BAT${i}_V_EMPTY:
description:
short: Empty cell voltage (5C load)
long: |
Defines the voltage where a single cell of battery 1 is considered empty.
The voltage should be chosen before the steep dropoff to 2.8V. A typical
lithium battery can only be discharged down to 10% before it drops off
to a voltage level damaging the cells.
type: float
unit: V
decimal: 2
increment: 0.01
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [3.5, 3.5]
BAT${i}_V_CHARGED:
description:
short: Full cell voltage (5C load)
long: |
Defines the voltage where a single cell of battery 1 is considered full
under a mild load. This will never be the nominal voltage of 4.2V
type: float
unit: V
decimal: 2
increment: 0.01
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [4.05, 4.05]
BAT${i}_V_LOAD_DROP:
description:
short: Voltage drop per cell on full throttle
long: |
This implicitely defines the internal resistance
to maximum current ratio for battery 1 and assumes linearity.
A good value to use is the difference between the
5C and 20-25C load. Not used if BAT${i}_R_INTERNAL is
set.
type: float
unit: V
min: 0.07
max: 0.5
decimal: 2
increment: 0.01
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [0.3, 0.3]
BAT${i}_R_INTERNAL:
description:
short: Explicitly defines the per cell internal resistance for battery ${i}
long: |
If non-negative, then this will be used in place of
BAT${i}_V_LOAD_DROP for all calculations.
type: float
unit: Ohm
min: -1.0
max: 0.2
decimal: 2
increment: 0.01
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1.0, -1.0]
BAT${i}_N_CELLS:
description:
short: Number of cells for battery ${i}.
long: |
Defines the number of cells the attached battery consists of.
type: enum
values:
2: 2S Battery
3: 3S Battery
4: 4S Battery
5: 5S Battery
6: 6S Battery
7: 7S Battery
8: 8S Battery
9: 9S Battery
10: 10S Battery
11: 11S Battery
12: 12S Battery
13: 13S Battery
14: 14S Battery
15: 15S Battery
16: 16S Battery
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [0, 0]
BAT${i}_CAPACITY:
description:
short: Battery ${i} capacity.
long: |
Defines the capacity of battery ${i} in mAh.
type: float
unit: mAh
min: -1.0
max: 100000
decimal: 0
increment: 50
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1.0, -1.0]
BAT${i}_SOURCE:
description:
short: Battery ${i} monitoring source.
long: |
This parameter controls the source of battery data. The value 'Power Module'
means that measurements are expected to come from a power module. If the value is set to
'External' then the system expects to receive mavlink battery status messages.
type: enum
unit: mAh
values:
0: Power Module
1: External
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [0, 0]

5
src/modules/battery_status/CMakeLists.txt
View File

@ -36,7 +36,10 @@ px4_add_module(
MAIN battery_status
SRCS
battery_status.cpp
parameters.cpp
analog_battery.cpp
MODULE_CONFIG
module.yaml
DEPENDS
battery

151
src/modules/battery_status/analog_battery.cpp Normal file
View File

@ -0,0 +1,151 @@
#include <lib/battery/battery.h>
#include "analog_battery.h"
// Defaults to use if the parameters are not set
#if BOARD_NUMBER_BRICKS > 0
#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST)
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
#else
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
#endif
#else
static constexpr int DEFAULT_V_CHANNEL[1] = {0};
static constexpr int DEFAULT_I_CHANNEL[1] = {0};
#endif
static constexpr float DEFAULT_VOLTS_PER_COUNT = 3.3f / 4096.0f;
AnalogBattery::AnalogBattery(int index, ModuleParams *parent) :
Battery(index, parent)
{
char param_name[17];
_analog_param_handles.cnt_v_volt = param_find("BAT_CNT_V_VOLT");
_analog_param_handles.cnt_v_curr = param_find("BAT_CNT_V_CURR");
_analog_param_handles.v_offs_cur = param_find("BAT_V_OFFS_CURR");
snprintf(param_name, sizeof(param_name), "BAT%d_V_DIV", index);
_analog_param_handles.v_div = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_A_PER_V", index);
_analog_param_handles.a_per_v = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_ADC_CHANNEL", index);
_analog_param_handles.adc_channel = param_find(param_name);
_analog_param_handles.v_div_old = param_find("BAT_V_DIV");
_analog_param_handles.a_per_v_old = param_find("BAT_A_PER_V");
_analog_param_handles.adc_channel_old = param_find("BAT_ADC_CHANNEL");
}
void
AnalogBattery::updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw,
bool selected_source, int priority, float throttle_normalized,
bool armed)
{
float voltage_v = (voltage_raw * _analog_params.cnt_v_volt) * _analog_params.v_div;
float current_a = ((current_raw * _analog_params.cnt_v_curr) - _analog_params.v_offs_cur) * _analog_params.a_per_v;
bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
(BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected,
selected_source, priority, throttle_normalized, armed, _params.source == 0);
}
/**
* Whether the ADC channel for the voltage of this battery is valid.
* Corresponds to BOARD_BRICK_VALID_LIST
*/
bool AnalogBattery::is_valid()
{
#ifdef BOARD_BRICK_VALID_LIST
bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
return valid[_index - 1];
#else
// TODO: Maybe return false instead?
return true;
#endif
}
int AnalogBattery::get_voltage_channel()
{
if (_analog_params.adc_channel >= 0) {
return _analog_params.adc_channel;
} else {
return DEFAULT_V_CHANNEL[_index - 1];
}
}
int AnalogBattery::get_current_channel()
{
// TODO: Possibly implement parameter for current sense channel
return DEFAULT_I_CHANNEL[_index - 1];
}
void
AnalogBattery::updateParams()
{
if (_index == 1) {
migrateParam<float>(_analog_param_handles.v_div_old, _analog_param_handles.v_div, &_analog_params.v_div_old,
&_analog_params.v_div, _first_parameter_update);
migrateParam<float>(_analog_param_handles.a_per_v_old, _analog_param_handles.a_per_v, &_analog_params.a_per_v_old,
&_analog_params.a_per_v, _first_parameter_update);
migrateParam<int>(_analog_param_handles.adc_channel_old, _analog_param_handles.adc_channel,
&_analog_params.adc_channel_old, &_analog_params.adc_channel, _first_parameter_update);
} else {
param_get(_analog_param_handles.v_div, &_analog_params.v_div);
param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
param_get(_analog_param_handles.adc_channel, &_analog_params.adc_channel);
}
param_get(_analog_param_handles.cnt_v_volt, &_analog_params.cnt_v_volt);
param_get(_analog_param_handles.cnt_v_curr, &_analog_params.cnt_v_curr);
param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur);
/* scaling of ADC ticks to battery voltage */
if (_analog_params.cnt_v_volt < 0.0f) {
/* apply scaling according to defaults if set to default */
_analog_params.cnt_v_volt = (BOARD_ADC_POS_REF_V_FOR_VOLTAGE_CHAN / px4_arch_adc_dn_fullcount());
param_set_no_notification(_analog_param_handles.cnt_v_volt, &_analog_params.cnt_v_volt);
}
/* scaling of ADC ticks to battery current */
if (_analog_params.cnt_v_curr < 0.0f) {
/* apply scaling according to defaults if set to default */
_analog_params.cnt_v_curr = (BOARD_ADC_POS_REF_V_FOR_CURRENT_CHAN / px4_arch_adc_dn_fullcount());
param_set_no_notification(_analog_param_handles.cnt_v_curr, &_analog_params.cnt_v_curr);
}
if (_analog_params.v_div <= 0.0f) {
/* apply scaling according to defaults if set to default */
_analog_params.v_div = BOARD_BATTERY1_V_DIV;
param_set_no_notification(_analog_param_handles.v_div, &_analog_params.v_div);
if (_index == 1) {
_analog_params.v_div_old = BOARD_BATTERY1_V_DIV;
param_set_no_notification(_analog_param_handles.v_div_old, &_analog_params.v_div_old);
}
}
if (_analog_params.a_per_v <= 0.0f) {
/* apply scaling according to defaults if set to default */
_analog_params.a_per_v = BOARD_BATTERY1_A_PER_V;
param_set_no_notification(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
if (_index == 1) {
_analog_params.a_per_v_old = BOARD_BATTERY1_A_PER_V;
param_set_no_notification(_analog_param_handles.a_per_v_old, &_analog_params.a_per_v_old);
}
}
Battery::updateParams();
}

104
src/modules/battery_status/analog_battery.h Normal file
View File

@ -0,0 +1,104 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#pragma once
#include <battery/battery.h>
#include <parameters/param.h>
class AnalogBattery : public Battery
{
public:
AnalogBattery(int index = 1, ModuleParams *parent = nullptr);
/**
* Update current battery status message.
*
* @param voltage_raw Battery voltage read from ADC, in raw ADC counts
* @param current_raw Voltage of current sense resistor, in raw ADC counts
* @param timestamp Time at which the ADC was read (use hrt_absolute_time())
* @param selected_source This battery is on the brick that the selected source for selected_source
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
* @param throttle_normalized Throttle of the vehicle, between 0 and 1
* @param armed Arming state of the vehicle
*/
void updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw,
bool selected_source, int priority, float throttle_normalized,
bool armed);
/**
* Whether the ADC channel for the voltage of this battery is valid.
* Corresponds to BOARD_BRICK_VALID_LIST
*/
bool is_valid();
/**
* Which ADC channel is used for voltage reading of this battery
*/
int get_voltage_channel();
/**
* Which ADC channel is used for current reading of this battery
*/
int get_current_channel();
protected:
struct {
param_t cnt_v_volt;
param_t cnt_v_curr;
param_t v_offs_cur;
param_t v_div;
param_t a_per_v;
param_t adc_channel;
param_t v_div_old;
param_t a_per_v_old;
param_t adc_channel_old;
} _analog_param_handles;
struct {
float cnt_v_volt;
float cnt_v_curr;
float v_offs_cur;
float v_div;
float a_per_v;
int adc_channel;
float v_div_old;
float a_per_v_old;
int adc_channel_old;
} _analog_params;
virtual void updateParams() override;
};

25
src/modules/battery_status/sensor_params_battery.c → src/modules/battery_status/analog_battery_params_common.c
View File

@ -65,28 +65,3 @@ PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0);
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);
/**
* Battery monitoring source.
*
* This parameter controls the source of battery data. The value 'Power Module'
* means that measurements are expected to come from a power module. If the value is set to
* 'External' then the system expects to receive mavlink battery status messages.
*
* @min 0
* @max 1
* @value 0 Power Module
* @value 1 External
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_SOURCE, 0);
/**
* Battery ADC Channel
*
* This parameter specifies the ADC channel used to monitor voltage of main power battery.
* A value of -1 means to use the board default.
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);

61
src/modules/battery_status/parameters.h → src/modules/battery_status/analog_battery_params_deprecated.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2019 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
@ -31,54 +31,25 @@
*
****************************************************************************/
#pragma once
/**
* @file parameters.h
* This parameter is deprecated. Please use BAT1_V_DIV.
*
* defines the list of parameters that are used within the sensors module
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
/**
* This parameter is deprecated. Please use BAT1_A_PER_V.
*
* @author Beat Kueng <beat-kueng@gmx.net>
* @group Battery Calibration
* @decimal 8
*/
#include <px4_platform_common/px4_config.h>
#include <drivers/drv_rc_input.h>
#include <parameters/param.h>
#include <mathlib/mathlib.h>
namespace battery_status
{
struct Parameters {
float battery_voltage_scaling;
float battery_current_scaling;
float battery_current_offset;
float battery_v_div;
float battery_a_per_v;
int32_t battery_source;
int32_t battery_adc_channel;
};
struct ParameterHandles {
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t battery_current_offset;
param_t battery_v_div;
param_t battery_a_per_v;
param_t battery_source;
param_t battery_adc_channel;
};
PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
/**
* initialize ParameterHandles struct
* This parameter is deprecated. Please use BAT1_ADC_CHANNEL.
*
* @group Battery Calibration
*/
void initialize_parameter_handles(ParameterHandles &parameter_handles);
/**
* Read out the parameters using the handles into the parameters struct.
* @return 0 on success, <0 on error
*/
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
} /* namespace sensors */
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);

121
src/modules/battery_status/battery_status.cpp
View File

@ -66,10 +66,9 @@
#include <DevMgr.hpp>
#include "parameters.h"
#include "analog_battery.h"
using namespace DriverFramework;
using namespace battery_status;
using namespace time_literals;
/**
@ -137,8 +136,18 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */
Battery _battery[BOARD_NUMBER_BRICKS]; /**< Helper lib to publish battery_status topic. */
AnalogBattery _battery1;
#if BOARD_NUMBER_BRICKS > 1
AnalogBattery _battery2;
#endif
AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] {
&_battery1,
#if BOARD_NUMBER_BRICKS > 1
&_battery2,
#endif
}; // End _analogBatteries
#if BOARD_NUMBER_BRICKS > 1
int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */
@ -146,16 +155,6 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _last_config_update{0};
Parameters _parameters{}; /**< local copies of interesting parameters */
ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Do adc-related initialisation.
*/
@ -178,13 +177,13 @@ private:
BatteryStatus::BatteryStatus() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_battery1(1, this),
#if BOARD_NUMBER_BRICKS > 1
_battery2(2, this),
#endif
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
{
initialize_parameter_handles(_parameter_handles);
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
_battery[b].setParent(this);
}
updateParams();
}
BatteryStatus::~BatteryStatus()
@ -192,19 +191,6 @@ BatteryStatus::~BatteryStatus()
ScheduleClear();
}
int
BatteryStatus::parameters_update()
{
/* read the parameter values into _parameters */
int ret = update_parameters(_parameter_handles, _parameters);
if (ret) {
return ret;
}
return ret;
}
int
BatteryStatus::adc_init()
{
@ -228,7 +214,6 @@ BatteryStatus::parameter_update_poll(bool forced)
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
parameters_update();
updateParams();
}
}
@ -242,30 +227,15 @@ BatteryStatus::adc_poll()
/* read all channels available */
int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
//todo:abosorb into new class Power
/* For legacy support we publish the battery_status for the Battery that is
* associated with the Brick that is the selected source for VDD_5V_IN
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
* Like in the FMUv4
*/
/* The ADC channels that are associated with each brick, in power controller
* priority order highest to lowest, as defined by the board config.
*/
int bat_voltage_v_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
int bat_voltage_i_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
if (_parameters.battery_adc_channel >= 0) { // overwrite default
bat_voltage_v_chan[0] = _parameters.battery_adc_channel;
}
/* The valid signals (HW dependent) are associated with each brick */
bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
/* Per Brick readings with default unread channels at 0 */
float bat_current_a[BOARD_NUMBER_BRICKS] = {0.0f};
float bat_voltage_v[BOARD_NUMBER_BRICKS] = {0.0f};
int32_t bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
int32_t bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
/* Based on the valid_chan, used to indicate the selected the lowest index
* (highest priority) supply that is the source for the VDD_5V_IN
@ -284,7 +254,7 @@ BatteryStatus::adc_poll()
/* Once we have subscriptions, Do this once for the lowest (highest priority
* supply on power controller) that is valid.
*/
if (_battery_pub[b] != nullptr && selected_source < 0 && valid_chan[b]) {
if (selected_source < 0 && _analogBatteries[b]->is_valid()) {
/* Indicate the lowest brick (highest priority supply on power controller)
* that is valid as the one that is the selected source for the
* VDD_5V_IN
@ -294,54 +264,41 @@ BatteryStatus::adc_poll()
/* Move the selected_source to instance 0 */
if (_battery_pub_intance0ndx != selected_source) {
orb_advert_t tmp_h = _battery_pub[_battery_pub_intance0ndx];
_battery_pub[_battery_pub_intance0ndx] = _battery_pub[selected_source];
_battery_pub[selected_source] = tmp_h;
_analogBatteries[_battery_pub_intance0ndx]->swapUorbAdvert(
*_analogBatteries[selected_source]
);
_battery_pub_intance0ndx = selected_source;
}
# endif /* BOARD_NUMBER_BRICKS > 1 */
}
// todo:per brick scaling
/* look for specific channels and process the raw voltage to measurement data */
if (bat_voltage_v_chan[b] == buf_adc[i].am_channel) {
if (_analogBatteries[b]->get_voltage_channel() == buf_adc[i].am_channel) {
/* Voltage in volts */
bat_voltage_v[b] = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div;
bat_voltage_adc_readings[b] = buf_adc[i].am_data;
} else if (bat_voltage_i_chan[b] == buf_adc[i].am_channel) {
bat_current_a[b] = ((buf_adc[i].am_data * _parameters.battery_current_scaling)
- _parameters.battery_current_offset) * _parameters.battery_a_per_v;
} else if (_analogBatteries[b]->get_current_channel() == buf_adc[i].am_channel) {
bat_current_adc_readings[b] = buf_adc[i].am_data;
}
}
}
}
if (_parameters.battery_source == 0) {
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
/* Consider the brick connected if there is a voltage */
bool connected = bat_voltage_v[b] > BOARD_ADC_OPEN_CIRCUIT_V;
/* In the case where the BOARD_ADC_OPEN_CIRCUIT_V is
* greater than the BOARD_VALID_UV let the HW qualify that it
* is connected.
*/
if (BOARD_ADC_OPEN_CIRCUIT_V > BOARD_VALID_UV) {
connected &= valid_chan[b];
}
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
if (_analogBatteries[b]->source() == 0) {
actuator_controls_s ctrl{};
_actuator_ctrl_0_sub.copy(&ctrl);
battery_status_s battery_status{};
_battery[b].updateBatteryStatus(hrt_absolute_time(), bat_voltage_v[b], bat_current_a[b],
connected, selected_source == b, b,
ctrl.control[actuator_controls_s::INDEX_THROTTLE],
_armed, &battery_status);
int instance;
orb_publish_auto(ORB_ID(battery_status), &_battery_pub[b], &battery_status, &instance, ORB_PRIO_DEFAULT);
_analogBatteries[b]->updateBatteryStatusRawADC(
hrt_absolute_time(),
bat_voltage_adc_readings[b],
bat_current_adc_readings[b],
selected_source == b,
b,
ctrl.control[actuator_controls_s::INDEX_THROTTLE],
_armed
);
}
}
}

50
src/modules/battery_status/module.yaml Normal file
View File

@ -0,0 +1,50 @@
__max_num_config_instances: &max_num_config_instances 2
module_name: battery_status
parameters:
- group: Battery Calibration
definitions:
BAT${i}_V_DIV:
description:
short: Battery ${i} voltage divider (V divider)
long: |
This is the divider from battery ${i} voltage to 3.3V ADC voltage.
If using e.g. Mauch power modules the value from the datasheet
can be applied straight here. A value of -1 means to use
the board default.
type: float
decimal: 8
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1.0, -1.0]
BAT${i}_A_PER_V:
description:
short: Battery ${i} current per volt (A/V)
long: |
The voltage seen by the 3.3V ADC multiplied by this factor
will determine the battery current. A value of -1 means to use
the board default.
type: float
decimal: 8
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1.0, -1.0]
BAT${i}_ADC_CHANNEL:
description:
short: Battery ${i} ADC Channel
long: |
This parameter specifies the ADC channel used to monitor voltage of main power battery.
A value of -1 means to use the board default.
type: int32
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1, -1]

119
src/modules/battery_status/parameters.cpp
View File

@ -1,119 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016-2019 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 parameters.cpp
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include "parameters.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/px4_config.h>
#include <drivers/drv_adc.h>
namespace battery_status
{
void initialize_parameter_handles(ParameterHandles &parameter_handles)
{
parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT");
parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR");
parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
parameter_handles.battery_v_div = param_find("BAT_V_DIV");
parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
parameter_handles.battery_source = param_find("BAT_SOURCE");
parameter_handles.battery_adc_channel = param_find("BAT_ADC_CHANNEL");
}
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
{
int ret = PX4_OK;
const char *paramerr = "FAIL PARM LOAD";
/* scaling of ADC ticks to battery voltage */
if (param_get(parameter_handles.battery_voltage_scaling, &(parameters.battery_voltage_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_voltage_scaling = (BOARD_ADC_POS_REF_V_FOR_VOLTAGE_CHAN / px4_arch_adc_dn_fullcount());
param_set_no_notification(parameter_handles.battery_voltage_scaling, &parameters.battery_voltage_scaling);
}
/* scaling of ADC ticks to battery current */
if (param_get(parameter_handles.battery_current_scaling, &(parameters.battery_current_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_current_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_current_scaling = (BOARD_ADC_POS_REF_V_FOR_CURRENT_CHAN / px4_arch_adc_dn_fullcount());
param_set_no_notification(parameter_handles.battery_current_scaling, &parameters.battery_current_scaling);
}
if (param_get(parameter_handles.battery_current_offset, &(parameters.battery_current_offset)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.battery_v_div, &(parameters.battery_v_div)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_v_div = 0.0f;
} else if (parameters.battery_v_div <= 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_v_div = BOARD_BATTERY1_V_DIV;
param_set_no_notification(parameter_handles.battery_v_div, &parameters.battery_v_div);
}
if (param_get(parameter_handles.battery_a_per_v, &(parameters.battery_a_per_v)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_a_per_v = 0.0f;
} else if (parameters.battery_a_per_v <= 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_a_per_v = BOARD_BATTERY1_A_PER_V;
param_set_no_notification(parameter_handles.battery_a_per_v, &parameters.battery_a_per_v);
}
param_get(parameter_handles.battery_source, &(parameters.battery_source));
param_get(parameter_handles.battery_adc_channel, &(parameters.battery_adc_channel));
return ret;
}
} /* namespace battery_status */

3
src/modules/mavlink/mavlink_messages.cpp
View File

@ -693,7 +693,8 @@ protected:
if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status)) {
/* battery status message with higher resolution */
mavlink_battery_status_t bat_msg{};
bat_msg.id = i;
// TODO: Determine how to better map between battery ID within the firmware and in MAVLink
bat_msg.id = battery_status.id - 1;
bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL;
bat_msg.type = MAV_BATTERY_TYPE_LIPO;
bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1;

2
src/modules/sensors/CMakeLists.txt
View File

@ -43,8 +43,6 @@ px4_add_module(
sensors.cpp
parameters.cpp
temperature_compensation.cpp
power.cpp
analog_battery.cpp
DEPENDS
airspeed

141
src/modules/sensors/analog_battery.cpp
View File

@ -1,141 +0,0 @@
#include "analog_battery.h"
// Defaults to use if the parameters are not set
#if BOARD_NUMBER_BRICKS > 0
#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST)
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
#else
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
#endif
#else
static constexpr int DEFAULT_V_CHANNEL[1] = {0};
static constexpr int DEFAULT_I_CHANNEL[1] = {0};
#endif
static constexpr float DEFAULT_VOLTS_PER_COUNT = 3.3f / 4096.0f;
AnalogBattery::AnalogBattery() :
ModuleParams(nullptr)
{}
void
AnalogBattery::updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw,
bool selected_source, int priority, float throttle_normalized,
bool armed)
{
// TODO: Check that there was actually a parameter update
_get_battery_base().updateParams();
updateParams();
float voltage_v = (voltage_raw * _get_cnt_v_volt()) * _get_v_div();
float current_a = ((current_raw * _get_cnt_v_curr()) - _get_v_offs_cur()) * _get_a_per_v();
bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
(BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
_get_battery_base().updateBatteryStatus(timestamp, voltage_v, current_a, connected,
selected_source, priority, throttle_normalized, armed, false);
// Before refactoring and adding the BatteryBase and AnalogBattery classes, the only place that checked the
// value of the BAT_SOURCE parameter was in the ADC polling in sensors.cpp. So I keep that logic here for now.
if (_get_source() == 0) {
_get_battery_base().publish();
}
}
/**
* Whether the ADC channel for the voltage of this battery is valid.
* Corresponds to BOARD_BRICK_VALID_LIST
*/
bool AnalogBattery::is_valid()
{
#ifdef BOARD_BRICK_VALID_LIST
bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
return valid[_get_brick_index()];
#else
// TODO: Maybe return false instead?
return true;
#endif
}
int AnalogBattery::getVoltageChannel()
{
if (_get_adc_channel() >= 0) {
return _get_adc_channel();
} else {
return DEFAULT_V_CHANNEL[_get_brick_index()];
}
}
int AnalogBattery::getCurrentChannel()
{
// TODO: Possibly implement parameter for current sense channel
return DEFAULT_I_CHANNEL[_get_brick_index()];
}
float
AnalogBattery::_get_cnt_v_volt()
{
float val = _get_cnt_v_volt_raw();
if (val < 0.0f) {
// TODO: This magic constant was hardcoded into sensors.cpp before I did the refactor. I don't know
// what the best way is to make it not a magic number.
return DEFAULT_VOLTS_PER_COUNT;
} else {
return val;
}
}
float
AnalogBattery::_get_cnt_v_curr()
{
float val = _get_cnt_v_curr_raw();
if (val < 0.0f) {
return DEFAULT_VOLTS_PER_COUNT;
} else {
return val;
}
}
float
AnalogBattery::_get_v_div()
{
float val = _get_v_div_raw();
if (val <= 0.0f) {
return BOARD_BATTERY1_V_DIV;
} else {
return val;
}
}
float
AnalogBattery::_get_a_per_v()
{
float val = _get_a_per_v_raw();
if (val <= 0.0f) {
return BOARD_BATTERY1_A_PER_V;
} else {
return val;
}
}
#if BOARD_NUMBER_BRICKS > 0
AnalogBattery1::AnalogBattery1()
{
Battery1::migrateParam(_param_old_a_per_v, _param_a_per_v, "A_PER_V", -1.0f);
Battery1::migrateParam(_param_old_adc_channel, _param_adc_channel, "ADC_CHANNEL", -1);
Battery1::migrateParam(_param_old_v_div, _param_v_div, "V_DIV", -1.0f);
}
#endif

163
src/modules/sensors/analog_battery.h
View File

@ -1,163 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#pragma once
#include <battery/battery_base.h>
#include <battery/battery.h>
class AnalogBattery : public ModuleParams
{
public:
AnalogBattery();
/**
* Update current battery status message.
*
* @param voltage_raw Battery voltage read from ADC, in raw ADC counts
* @param current_raw Voltage of current sense resistor, in raw ADC counts
* @param timestamp Time at which the ADC was read (use hrt_absolute_time())
* @param selected_source This battery is on the brick that the selected source for selected_source
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
* @param throttle_normalized Throttle of the vehicle, between 0 and 1
* @param armed Arming state of the vehicle
*/
void updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw,
bool selected_source, int priority, float throttle_normalized,
bool armed);
/**
* Whether the ADC channel for the voltage of this battery is valid.
* Corresponds to BOARD_BRICK_VALID_LIST
*/
bool is_valid();
/**
* Which ADC channel is used for voltage reading of this battery
*/
int getVoltageChannel();
/**
* Which ADC channel is used for current reading of this battery
*/
int getCurrentChannel();
protected:
virtual float _get_cnt_v_volt_raw() = 0;
virtual float _get_cnt_v_curr_raw() = 0;
virtual float _get_v_offs_cur() = 0;
virtual float _get_v_div_raw() = 0;
virtual float _get_a_per_v_raw() = 0;
virtual int _get_adc_channel() = 0;
virtual int _get_source() = 0;
virtual int _get_brick_index() = 0;
virtual BatteryBase &_get_battery_base() = 0;
float _get_cnt_v_volt();
float _get_cnt_v_curr();
float _get_v_div();
float _get_a_per_v();
};
#if BOARD_NUMBER_BRICKS > 0
class AnalogBattery1 : public AnalogBattery
{
public:
AnalogBattery1();
protected:
Battery1 _base{};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_V_DIV>) _param_old_v_div,
(ParamFloat<px4::params::BAT_A_PER_V>) _param_old_a_per_v,
(ParamInt<px4::params::BAT_ADC_CHANNEL>) _param_old_adc_channel,
(ParamFloat<px4::params::BAT1_V_DIV>) _param_v_div,
(ParamFloat<px4::params::BAT1_A_PER_V>) _param_a_per_v,
(ParamInt<px4::params::BAT1_ADC_CHANNEL>) _param_adc_channel,
(ParamFloat<px4::params::BAT_CNT_V_VOLT>) _param_cnt_v_volt,
(ParamFloat<px4::params::BAT_CNT_V_CURR>) _param_cnt_v_curr,
(ParamFloat<px4::params::BAT_V_OFFS_CURR>) _param_v_offs_cur,
(ParamInt<px4::params::BAT_SOURCE>) _param_source
)
float _get_v_div_raw() override {return _param_v_div.get(); }
float _get_a_per_v_raw() override {return _param_a_per_v.get(); }
int _get_adc_channel() override {return _param_adc_channel.get(); }
float _get_cnt_v_volt_raw() override {return _param_cnt_v_volt.get(); }
float _get_cnt_v_curr_raw() override {return _param_cnt_v_curr.get(); }
float _get_v_offs_cur() override {return _param_v_offs_cur.get(); }
int _get_source() override {return _param_source.get();}
int _get_brick_index() override {return 0; }
BatteryBase &_get_battery_base() override {return _base;}
};
#endif // #if BOARD_NUMBER_BRICKS > 0
#if BOARD_NUMBER_BRICKS > 1
class AnalogBattery2 : public AnalogBattery
{
protected:
Battery2 _base{};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT2_V_DIV>) _param_v_div,
(ParamFloat<px4::params::BAT2_A_PER_V>) _param_a_per_v,
(ParamInt<px4::params::BAT2_ADC_CHANNEL>) _param_adc_channel,
(ParamFloat<px4::params::BAT_CNT_V_VOLT>) _param_cnt_v_volt,
(ParamFloat<px4::params::BAT_CNT_V_CURR>) _param_cnt_v_curr,
(ParamFloat<px4::params::BAT_V_OFFS_CURR>) _param_v_offs_cur,
(ParamInt<px4::params::BAT_SOURCE>) _param_source
)
float _get_v_div_raw() override {return _param_v_div.get(); }
float _get_a_per_v_raw() override {return _param_a_per_v.get(); }
int _get_adc_channel() override {return _param_adc_channel.get(); }
float _get_cnt_v_volt_raw() override {return _param_cnt_v_volt.get(); }
float _get_cnt_v_curr_raw() override {return _param_cnt_v_curr.get(); }
float _get_v_offs_cur() override {return _param_v_offs_cur.get(); }
int _get_source() override {return _param_source.get();}
int _get_brick_index() override {return 1; }
BatteryBase &_get_battery_base() override {return _base;}
};
#endif // #if BOARD_NUMBER_BRICKS > 1

91
src/modules/sensors/power.cpp
View File

@ -1,91 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 power.cpp
*
* @author Timothy Scott <timothy@auterion.com>
*/
#include "power.h"
Power::Power() {}
void Power::update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, float throttle, bool armed)
{
#if BOARD_NUMBER_BRICKS > 0
/* For legacy support we publish the battery_status for the Battery that is
* associated with the Brick that is the selected source for VDD_5V_IN
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
* Like in the FMUv4
*/
/* Per Brick readings with default unread channels at 0 */
int32_t bat_current_cnt[BOARD_NUMBER_BRICKS] = {0};
int32_t bat_voltage_cnt[BOARD_NUMBER_BRICKS] = {0};
// The channel readings are not necessarily in a nice order, so we iterate through
// to find every relevant channel.
for (int i = 0; i < nchannels; i++) {
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
/* look for specific channels and process the raw voltage to measurement data */
if (_analogBatteries[b]->getVoltageChannel() == buf_adc[i].am_channel) {
/* Voltage in ADC counts */
bat_voltage_cnt[b] = buf_adc[i].am_data;
} else if (_analogBatteries[b]->getCurrentChannel() == buf_adc[i].am_channel) {
/* Voltage at current sense resistor in ADC counts */
bat_current_cnt[b] = buf_adc[i].am_data;
}
}
}
/* Based on the valid_chan, used to indicate the selected the lowest index
* (highest priority) supply that is the source for the VDD_5V_IN
* When < 0 none selected
*/
int selected_source = -1;
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
if (_analogBatteries[b]->is_valid() && selected_source < 0) {
selected_source = b;
}
_analogBatteries[b]->updateBatteryStatusRawADC(hrt_absolute_time(), bat_voltage_cnt[b], bat_current_cnt[b],
selected_source == b, b, throttle, armed);
}
#endif /* BOARD_NUMBER_BRICKS > 0 */
}

103
src/modules/sensors/power.h
View File

@ -1,103 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 power.h
*
* @author Timothy Scott <timothy@auterion.com>
*/
#pragma once
#include <board_config.h>
#include "analog_battery.h"
/**
* Measures voltage, current, etc. of all batteries connected to the vehicle, both
* analog and digital.
*/
class Power
{
public:
Power();
/**
* Updates the measurements of each battery.
*
* If the parameter `BAT_SOURCE` == 0, this function will also publish an instance of the uORB topic
* `battery_status` for each battery. For reasons of backwards compability, instance 0 will always be the
* primary battery. However, this may change in the future! In the future, please use orb_priority() to find
* the primary battery.
* @param buf_adc Buffer of ADC readings
* @param nchannels Number of valid ADC readings in `buf_adc`
* @param throttle Normalized throttle (between 0 and 1, or maybe between -1 and 1 in the future)
* @param armed True if the vehicle is armed
*/
void update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, float throttle, bool armed);
private:
/*
* All of these #if's are doing one thing: Building an array of `BatteryBase` objects, one
* for each possible connected battery. A `BatteryBase` object does not mean that a battery IS connected,
* it just means that one CAN be connected.
*
* For an example of what this looks like after preprocessing, assume that BOARD_NUMBER_BRICKS = 2:
* ```
* AnalogBattery1 _battery0;
* AnalogBattery2 _battery1;
*
* BatteryBase *_analogBatteries[2] {
* &_battery0,
* &_battery1,
* }
* ```
*
* The #if BOARD_NUMBER_BRICKS > 0 wraps the entire declaration because otherwise, an empty array is declared
* which then is unused. In some configurations, an unused variable throws a compile error.
*/
#if BOARD_NUMBER_BRICKS > 0
AnalogBattery1 _battery0;
#if BOARD_NUMBER_BRICKS > 1
AnalogBattery2 _battery1;
#endif
AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] {
&_battery0,
#if BOARD_NUMBER_BRICKS > 1
&_battery1,
#endif
}; // End _analogBatteries
#endif // End #if BOARD_NUMBER_BRICKS > 0
};

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

@ -72,7 +72,6 @@
#include "parameters.h"
#include "voted_sensors_update.h"
#include "power.h"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"

33
src/modules/simulator/simulator.h
View File

@ -42,7 +42,7 @@
#pragma once
#include <battery/battery_base.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
@ -73,6 +73,7 @@
#include <v2.0/common/mavlink.h>
#include <v2.0/mavlink_types.h>
#include <lib/battery/battery.h>
namespace simulator
{
@ -237,22 +238,22 @@ private:
hrt_abstime _last_sitl_timestamp{0};
hrt_abstime _last_battery_timestamp{0};
// Because the simulator doesn't actually care about the real values of these, just stick
// in some good defaults.
// This is an anonymous class, because BatteryBase is abstract and can't directly be instantiated.
class : public BatteryBase
class : public Battery
{
float _get_bat_v_empty() override { return 3.5; }
float _get_bat_v_charged() override { return 4.05; }
int _get_bat_n_cells() override { return 4; }
float _get_bat_capacity() override { return 10.0; }
float _get_bat_v_load_drop() override { return 0; }
float _get_bat_r_internal() override { return 0; }
float _get_bat_low_thr() override { return 0.15; }
float _get_bat_crit_thr() override { return 0.07; }
float _get_bat_emergen_thr() override { return 0.05; }
int _get_source() override { return 0; }
} _battery {};
virtual void updateParams() override
{
_params.v_empty = 3.5f;
_params.v_charged = 4.05f;
_params.n_cells = 4;
_params.capacity = 10.0f;
_params.v_load_drop = 0.0f;
_params.r_internal = 0.0f;
_params.low_thr = 0.15f;
_params.crit_thr = 0.07f;
_params.emergen_thr = 0.05f;
_params.source = 0;
}
} _battery;
#ifndef __PX4_QURT

2
validation/module_schema.yaml
View File

@ -141,7 +141,7 @@ parameters:
'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2',
'hPa', 'kg', 'kg/m^2',
'mm', 'm', 'm/s', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
'Ohm',
'Ohm', 'V',
'us', 'ms', 's' ]
bit:
# description of all bits for type bitmask.