Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Created new AnalogBattery class

Browse Source
This commit is contained in:
Timothy Scott
2019-07-25 16:46:32 +02:00
committed by Julian Oes
parent bff1df7080
commit d7bb5d46bb
20 changed files with 687 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
+1 -1
View File
@@ -411,7 +411,7 @@ Syslink::handle_message(syslink_message_t *msg)
memcpy(&vbat, &msg->data[1], sizeof(float));
//memcpy(&iset, &msg->data[5], sizeof(float));
_battery.updateBatteryStatus(vbat, -1, t, true, 0, 0, false);
_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, true);
// Update battery charge state
src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
+1 -1
View File
@@ -220,7 +220,7 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
// TODO Check if this is the right way for the Bebop
// We don't have current measurements
_battery.updateBatteryStatus(data.battery_voltage_v, 0.0, timestamp, true, 0, _last_throttle, _armed);
_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed, true);
if (_esc_topic == nullptr) {
_esc_topic = orb_advertise(ORB_ID(esc_status), &esc_status);
src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp
+4 -3
View File
@@ -131,16 +131,17 @@ int DfLtc2946Wrapper::stop()
int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data)
{
hrt_abstime t = hrt_absolute_time();
bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V;
actuator_controls_s ctrl;
orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
vehicle_control_mode_s vcontrol_mode;
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
_battery.updateBatteryStatus(data.battery_voltage_V, data.battery_current_A, t,
true, 1,
_battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A,
connected, true, 1,
ctrl.control[actuator_controls_s::INDEX_THROTTLE],
vcontrol_mode.flag_armed);
vcontrol_mode.flag_armed, true);
return 0;
}
src/lib/battery/CMakeLists.txt
+1 -1
View File
@@ -31,4 +31,4 @@
#
############################################################################
px4_add_library(battery battery_base.cpp)
px4_add_library(battery battery_base.cpp battery.cpp)
src/lib/battery/battery.cpp
+14
View File
@@ -0,0 +1,14 @@
#include "battery.h"
Battery1::Battery1()
{
updateParams();
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);
}
src/lib/battery/battery.h
+74 -65
View File
@@ -33,12 +33,18 @@
#pragma once
#include "battery_base.h"
#include <px4_log.h>
#include <math.h>
/**
* @file battery.h
* Implementations of BatteryBase
* 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 `BAT_*`, while Battery 2 uses `BAT2_*`). To avoid code duplication, inheritance is being 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.
*
@@ -47,54 +53,89 @@
* 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);
* // A real implementation would have to handle the case where battery_index == 1 and there is no number in the param name.
* 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 new battery, just create a new implementation of BatteryBase and implement all of the _get_* methods,
* then add all of the new parameters necessary for calibration.
* 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.
*/
#include "battery_base.h"
/**
* Battery1 represents a battery calibrated by BAT1_* parameters.
*/
class Battery1 : public BatteryBase
{
public:
Battery1() : BatteryBase()
Battery1();
/**
* 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_*(...)
*/
template<class P1, class P2, typename T> static void
migrateParam(P1 &oldParam, P2 &newParam, const char *name, T defaultValue)
{
// Can't do this in the constructor because virtual functions
if (_get_adc_channel() >= 0) {
vChannel = _get_adc_channel();
float diffOld = fabs((float) oldParam.get() - defaultValue);
float diffNew = fabs((float) newParam.get() - defaultValue);
} else {
vChannel = DEFAULT_V_CHANNEL[0];
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();
}
// TODO: Add parameter, like with V
iChannel = DEFAULT_I_CHANNEL[0];
}
private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_V_EMPTY>) _param_bat_v_empty,
(ParamFloat<px4::params::BAT_V_CHARGED>) _param_bat_v_charged,
(ParamInt<px4::params::BAT_N_CELLS>) _param_bat_n_cells,
(ParamFloat<px4::params::BAT_CAPACITY>) _param_bat_capacity,
(ParamFloat<px4::params::BAT_V_LOAD_DROP>) _param_bat_v_load_drop,
(ParamFloat<px4::params::BAT_R_INTERNAL>) _param_bat_r_internal,
(ParamFloat<px4::params::BAT_V_DIV>) _param_v_div,
(ParamFloat<px4::params::BAT_A_PER_V>) _param_a_per_v,
(ParamInt<px4::params::BAT_ADC_CHANNEL>) _param_adc_channel,
(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,
(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,
(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
)
@@ -107,34 +148,16 @@ private:
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(); }
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(); }
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_source() override {return _param_source.get(); }
int _get_adc_channel() override {return _param_adc_channel.get(); }
int _get_brick_index() override {return 0; }
};
/**
* Battery2 represents a battery calibrated by BAT2_* parameters.
*/
class Battery2 : public BatteryBase
{
public:
Battery2() : BatteryBase()
{
// Can't do this in the constructor because virtual functions
if (_get_adc_channel() >= 0) {
vChannel = _get_adc_channel();
} else {
vChannel = DEFAULT_V_CHANNEL[1];
}
// TODO: Add parameter, like with V
iChannel = DEFAULT_I_CHANNEL[1];
}
Battery2() {}
private:
DEFINE_PARAMETERS(
@@ -144,16 +167,10 @@ private:
(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::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_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,
(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
)
@@ -166,13 +183,5 @@ private:
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(); }
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(); }
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_source() override {return _param_source.get(); }
int _get_adc_channel() override {return _param_adc_channel.get(); }
int _get_brick_index() override {return 1; }
};
};
src/lib/battery/battery_base.cpp
+12 -77
View File
@@ -63,32 +63,13 @@ BatteryBase::reset()
// 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::updateBatteryStatusRawADC(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp,
bool selected_source, int priority,
float throttle_normalized,
bool armed)
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)
{
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();
updateBatteryStatus(voltage_v, current_a, timestamp, selected_source, priority, throttle_normalized, armed);
}
void
BatteryBase::updateBatteryStatus(float voltage_v, float current_a, hrt_abstime timestamp,
bool selected_source, int priority,
float throttle_normalized,
bool armed)
{
updateParams();
bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
(BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
reset();
_battery_status.timestamp = timestamp;
filterVoltage(voltage_v);
@@ -119,11 +100,17 @@ BatteryBase::updateBatteryStatus(float voltage_v, float current_a, hrt_abstime t
_battery_status.timestamp = timestamp;
if (_get_source() == 0) {
orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT);
if (should_publish) {
publish();
}
battery_status->temperature = NAN;
_battery_status.temperature = NAN;
}
void
BatteryBase::publish()
{
orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT);
}
void
@@ -267,55 +254,3 @@ BatteryBase::computeScale()
_scale = 1.f;
}
}
float
BatteryBase::_get_cnt_v_volt()
{
float val = _get_cnt_v_volt_raw();
if (val < 0.0f) {
return 3.3f / 4096.0f;
} else {
return val;
}
}
float
BatteryBase::_get_cnt_v_curr()
{
float val = _get_cnt_v_curr_raw();
if (val < 0.0f) {
return 3.3f / 4096.0f;
} else {
return val;
}
}
float
BatteryBase::_get_v_div()
{
float val = _get_v_div_raw();
if (val <= 0.0f) {
return BOARD_BATTERY1_V_DIV;
} else {
return val;
}
}
float
BatteryBase::_get_a_per_v()
{
float val = _get_a_per_v_raw();
if (val <= 0.0f) {
return BOARD_BATTERY1_A_PER_V;
} else {
return val;
}
}
src/lib/battery/battery_base.h
+49 -65
View File
@@ -53,7 +53,15 @@
/**
* BatteryBase is a base class for any type of battery.
*
* See battery.h for example implementation, and for explanation of why this is designed like it is.
* 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
{
@@ -80,20 +88,6 @@ public:
*/
float full_cell_voltage() { return _get_bat_v_charged(); }
/**
* 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(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp,
bool selected_source, int priority, float throttle_normalized, bool armed);
/**
* Update current battery status message.
*
@@ -104,73 +98,64 @@ public:
* @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(float voltage_v, float current_a, hrt_abstime timestamp,
bool selected_source, int priority, float throttle_normalized, bool armed);
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);
/**
* Which ADC channel is used for voltage reading of this battery
* Publishes the uORB battery_status message with the most recently-updated data.
*/
int vChannel{-1};
/**
* Which ADC channel is used for current reading of this battery
*/
int iChannel{-1};
void publish();
/**
* Whether the ADC channel for the voltage of this battery is valid.
* Corresponds to BOARD_BRICK_VALID_LIST
*/
bool is_valid()
{
#ifdef BOARD_BRICK_VALID_LIST
bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
return valid[_get_brick_index()];
#else
return true;
#endif
}
// TODO: Check that there was actually a parameter update
void updateParams() {ModuleParams::updateParams();}
protected:
// 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[0] = {};
static constexpr int DEFAULT_I_CHANNEL[0] = {};
#endif
// The following are all of the parameters needed for the batteries.
// See battery.h for example implementation.
/**
* @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;
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;
/**
* @return Source of battery data. 0 = internal power module. 1 = external, via Mavlink
*/
virtual int _get_source() = 0;
virtual int _get_adc_channel() = 0;
virtual int _get_brick_index() = 0;
float _get_cnt_v_volt();
float _get_cnt_v_curr();
float _get_v_div();
float _get_a_per_v();
battery_status_s _battery_status;
private:
void filterVoltage(float voltage_v);
@@ -195,5 +180,4 @@ private:
orb_advert_t _orbAdvert{nullptr};
int _orbInstance;
battery_status_s _battery_status;
};
src/lib/battery/battery_params_1.c
+11 -11
View File
@@ -56,7 +56,7 @@
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
PARAM_DEFINE_FLOAT(BAT1_V_EMPTY, 3.5f);
/**
* Full cell voltage (5C load)
@@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f);
PARAM_DEFINE_FLOAT(BAT1_V_CHARGED, 4.05f);
/**
* Voltage drop per cell on full throttle
@@ -78,7 +78,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f);
* 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_R_INTERNAL is
* 5C and 20-25C load. Not used if BAT1_R_INTERNAL is
* set.
*
* @group Battery Calibration
@@ -89,13 +89,13 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f);
* @increment 0.01
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
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
* BAT_V_LOAD_DROP for all calculations.
* BAT1_V_LOAD_DROP for all calculations.
*
* @group Battery Calibration
* @unit Ohms
@@ -103,7 +103,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
* @max 0.2
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f);
PARAM_DEFINE_FLOAT(BAT1_R_INTERNAL, -1.0f);
/**
@@ -131,7 +131,7 @@ PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f);
* @value 16 16S Battery
* @reboot_required true
*/
PARAM_DEFINE_INT32(BAT_N_CELLS, 0);
PARAM_DEFINE_INT32(BAT1_N_CELLS, 0);
/**
* Battery 1 capacity.
@@ -146,7 +146,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 0);
* @increment 50
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
PARAM_DEFINE_FLOAT(BAT1_CAPACITY, -1.0f);
/**
* Battery 1 voltage divider (V divider)
@@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
PARAM_DEFINE_FLOAT(BAT1_V_DIV, -1.0);
/**
* Battery 1 current per volt (A/V)
@@ -171,7 +171,7 @@ PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
PARAM_DEFINE_FLOAT(BAT1_A_PER_V, -1.0);
/**
* Battery 1 ADC Channel
@@ -181,4 +181,4 @@ PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);
PARAM_DEFINE_INT32(BAT1_ADC_CHANNEL, -1);
src/lib/battery/battery_params_2.c
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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
src/lib/battery/battery_params_common.c
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* 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
src/lib/battery/battery_params_deprecated.c
+181
View File
@@ -0,0 +1,181 @@
/****************************************************************************
*
* 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_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.
*/
/**
* This parameter is deprecated. Please use BAT1_V_EMPTY instead.
*
* 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(BAT_V_EMPTY, 3.5f);
/**
* This parameter is deprecated. Please use BAT1_V_CHARGED instead.
*
* 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(BAT_V_CHARGED, 4.05f);
/**
* This parameter is deprecated. Please use BAT1_V_LOAD_DROP instead.
*
* 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_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(BAT_V_LOAD_DROP, 0.3f);
/**
* This parameter is deprecated. Please use BAT1_R_INTERNAL instead.
*
* If non-negative, then this will be used in place of
* BAT_V_LOAD_DROP for all calculations.
*
* @group Battery Calibration
* @unit Ohms
* @min -1.0
* @max 0.2
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f);
/**
* This parameter is deprecated. Please use BAT1_N_CELLS instead.
*
* 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(BAT_N_CELLS, 0);
/**
* This parameter is deprecated. Please use BAT1_CAPACITY instead.
*
* 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(BAT_CAPACITY, -1.0f);
/**
* This parameter is deprecated. Please use BAT1_V_DIV 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.
*
* @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.
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);
src/modules/sensors/CMakeLists.txt
+1
View File
@@ -44,6 +44,7 @@ px4_add_module(
parameters.cpp
temperature_compensation.cpp
power.cpp
analog_battery.cpp
DEPENDS
airspeed
src/modules/sensors/analog_battery.cpp
+141
View File
@@ -0,0 +1,141 @@
#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
src/modules/sensors/analog_battery.h
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* 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
src/modules/sensors/power.cpp
+3 -3
View File
@@ -59,11 +59,11 @@ void Power::update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, f
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]->vChannel == buf_adc[i].am_channel) {
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]->iChannel == buf_adc[i].am_channel) {
} 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;
}
@@ -82,7 +82,7 @@ void Power::update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, f
selected_source = b;
}
_analogBatteries[b]->updateBatteryStatusRawADC(bat_voltage_cnt[b], bat_current_cnt[b], hrt_absolute_time(),
_analogBatteries[b]->updateBatteryStatusRawADC(hrt_absolute_time(), bat_voltage_cnt[b], bat_current_cnt[b],
selected_source == b, b, throttle, armed);
}
src/modules/sensors/power.h
+6 -15
View File
@@ -40,13 +40,7 @@
#pragma once
#include <board_config.h>
#include <battery/battery.h>
#ifdef BOARD_NUMBER_DIGITAL_BRICKS
#define TOTAL_BRICKS (BOARD_NUMBER_BRICKS + BOARD_NUMBER_DIGITAL_BRICKS)
#else
#define TOTAL_BRICKS BOARD_NUMBER_BRICKS
#endif
#include "analog_battery.h"
/**
* Measures voltage, current, etc. of all batteries connected to the vehicle, both
@@ -80,8 +74,8 @@ private:
*
* For an example of what this looks like after preprocessing, assume that BOARD_NUMBER_BRICKS = 2:
* ```
* Battery1 _battery0;
* Battery2 _battery1;
* AnalogBattery1 _battery0;
* AnalogBattery2 _battery1;
*
* BatteryBase *_analogBatteries[2] {
* &_battery0,
@@ -92,17 +86,14 @@ private:
* 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.
*/
// TODO: Add digital batteries
#if BOARD_NUMBER_BRICKS > 0
Battery1 _battery0;
AnalogBattery1 _battery0;
#if BOARD_NUMBER_BRICKS > 1
Battery2 _battery1;
AnalogBattery2 _battery1;
#endif
BatteryBase *_analogBatteries[BOARD_NUMBER_BRICKS] {
AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] {
&_battery0,
#if BOARD_NUMBER_BRICKS > 1
&_battery1,
src/modules/sensors/sensors.cpp
+1 -1
View File
@@ -72,7 +72,7 @@
#include "parameters.h"
#include "voted_sensors_update.h"
#include "power.h"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
src/modules/simulator/simulator.h
+18 -6
View File
@@ -42,7 +42,7 @@
#pragma once
#include <battery/battery.h>
#include <battery/battery_base.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
@@ -183,8 +183,6 @@ private:
_gps.writeData(&gps_data);
_battery_status.timestamp = hrt_absolute_time();
_px4_accel.set_sample_rate(250);
_px4_gyro.set_sample_rate(250);
}
@@ -237,10 +235,24 @@ private:
hrt_abstime _last_sim_timestamp{0};
hrt_abstime _last_sitl_timestamp{0};
hrt_abstime _last_battery_timestamp{0};
// Lib used to do the battery calculations.
Battery1 _battery {};
battery_status_s _battery_status{};
// 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
{
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 {};
#ifndef __PX4_QURT
src/modules/simulator/simulator_mavlink.cpp
+4 -2
View File
@@ -346,7 +346,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
static uint64_t last_integration_us = 0;
// battery simulation (limit update to 100Hz)
if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) {
if (hrt_elapsed_time(&_last_battery_timestamp) >= 10_ms) {
const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000;
@@ -370,7 +370,9 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
vbatt *= _battery.cell_count();
const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
_battery.updateBatteryStatus(vbatt, ibatt, now_us, true, 0, throttle, armed);
_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, true);
_last_battery_timestamp = now_us;
}
}