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

battery: fetch throttle value inside of class

Browse Source
This commit is contained in:
Matthias Grob
2021-11-25 10:53:31 +01:00
parent ddc6b6bc9c
commit b965923c08
17 changed files with 38 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
+1 -1
View File
@@ -405,7 +405,7 @@ Syslink::handle_message(syslink_message_t *msg)
//memcpy(&iset, &msg->data[5], sizeof(float));
_battery.updateBatteryStatus(t, vbat, -1, true,
battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0);
// Update battery charge state
src/drivers/power_monitor/ina226/ina226.cpp
+3 -8
View File
@@ -89,8 +89,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) :
0.0,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0
0
);
}
@@ -231,16 +230,13 @@ INA226::collect()
_bus_voltage = _power = _current = _shunt = 0;
}
_actuators_sub.copy(&_actuator_controls);
_battery.updateBatteryStatus(
hrt_absolute_time(),
(float) _bus_voltage * INA226_VSCALE,
(float) _current * _current_lsb,
success,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
0
);
perf_end(_sample_perf);
@@ -310,8 +306,7 @@ INA226::RunImpl()
0.0f,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0f
0
);
if (init() != PX4_OK) {
src/drivers/power_monitor/ina226/ina226.h
-5
View File
@@ -45,9 +45,7 @@
#include <lib/perf/perf_counter.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -202,10 +200,7 @@ private:
float _current_lsb{_max_current / DN_MAX};
float _power_lsb{25.0f * _current_lsb};
actuator_controls_s _actuator_controls{};
Battery _battery;
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int read(uint8_t address, int16_t &data);
src/drivers/power_monitor/ina228/ina228.cpp
+3 -8
View File
@@ -91,8 +91,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
0.0,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0
0
);
}
@@ -312,16 +311,13 @@ INA228::collect()
_bus_voltage = _power = _current = _shunt = 0;
}
_actuators_sub.copy(&_actuator_controls);
_battery.updateBatteryStatus(
hrt_absolute_time(),
(float) _bus_voltage * INA228_VSCALE,
(float) _current * _current_lsb,
success,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
0
);
perf_end(_sample_perf);
@@ -391,8 +387,7 @@ INA228::RunImpl()
0.0f,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0f
0
);
if (init() != PX4_OK) {
src/drivers/power_monitor/ina228/ina228.h
-5
View File
@@ -45,9 +45,7 @@
#include <lib/perf/perf_counter.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -352,10 +350,7 @@ private:
float _current_lsb{_max_current / DN_MAX};
float _power_lsb{25.0f * _current_lsb};
actuator_controls_s _actuator_controls{};
Battery _battery;
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int read(uint8_t address, int16_t &data);
src/drivers/power_monitor/ina238/ina238.cpp
+3 -8
View File
@@ -74,8 +74,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
0.0,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0
0
);
}
@@ -199,16 +198,13 @@ int INA238::collect()
bus_voltage = current = 0;
}
_actuators_sub.copy(&_actuator_controls);
_battery.updateBatteryStatus(
hrt_absolute_time(),
(float) bus_voltage * INA238_VSCALE,
(float) current * _current_lsb,
success,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
0
);
perf_end(_sample_perf);
@@ -269,8 +265,7 @@ void INA238::RunImpl()
0.0f,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0f
0
);
if (init() != PX4_OK) {
src/drivers/power_monitor/ina238/ina238.h
-5
View File
@@ -41,9 +41,7 @@
#include <lib/perf/perf_counter.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -338,10 +336,7 @@ private:
float _current_lsb;
int16_t _range;
actuator_controls_s _actuator_controls{};
Battery _battery;
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int read(uint8_t address, uint16_t &data);
src/drivers/power_monitor/voxlpm/voxlpm.cpp
+3 -8
View File
@@ -77,8 +77,7 @@ VOXLPM::init()
0.0,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0
0
);
}
@@ -346,15 +345,12 @@ VOXLPM::measure()
if (ret == PX4_OK) {
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT: {
_actuators_sub.copy(&_actuator_controls);
_battery.updateBatteryStatus(tnow,
_voltage,
_amperage,
true,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]);
0);
}
// fallthrough
@@ -382,8 +378,7 @@ VOXLPM::measure()
0.0,
true,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0);
0);
}
break;
src/drivers/power_monitor/voxlpm/voxlpm.hpp
-4
View File
@@ -88,9 +88,7 @@
#include <battery/battery.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/power_monitor.h>
#include <uORB/topics/parameter_update.h>
@@ -267,8 +265,6 @@ private:
int16_t _cal{0};
Battery _battery;
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
actuator_controls_s _actuator_controls{};
uint8_t read_reg(uint8_t addr);
int read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
src/lib/battery/battery.cpp
+12 -5
View File
@@ -97,19 +97,17 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us)
}
void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
int source, int priority, float throttle_normalized)
int source, int priority)
{
if (!_battery_initialized) {
_voltage_filter_v.reset(voltage_v);
_current_filter_a.reset(current_a);
_throttle_filter.reset(throttle_normalized);
}
_voltage_filter_v.update(voltage_v);
_current_filter_a.update(current_a);
_throttle_filter.update(throttle_normalized);
sumDischarged(timestamp, current_a);
estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState());
computeScale();
if (connected && _battery_initialized) {
@@ -169,7 +167,7 @@ void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
_last_timestamp = timestamp;
}
void Battery::estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle)
void Battery::estimateStateOfCharge(const float voltage_v, const float current_a)
{
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _params.n_cells;
@@ -179,6 +177,15 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a
cell_voltage += _params.r_internal * current_a;
} else {
actuator_controls_s actuator_controls{};
_actuator_controls_0_sub.copy(&actuator_controls);
const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
_throttle_filter.update(throttle);
if (!_battery_initialized) {
_throttle_filter.reset(throttle);
}
// assume linear relation between throttle and voltage drop
cell_voltage += throttle * _params.v_load_drop;
}
src/lib/battery/battery.h
+5 -3
View File
@@ -54,6 +54,8 @@
#include <lib/parameters/param.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
/**
@@ -91,10 +93,9 @@ public:
* @param timestamp: Time at which the ADC was read (use hrt_absolute_time())
* @param source: Source type in relation to BAT%d_SOURCE param.
* @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
*/
void updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
int source, int priority, float throttle_normalized);
int source, int priority);
protected:
struct {
@@ -130,11 +131,12 @@ protected:
private:
void sumDischarged(const hrt_abstime &timestamp, float current_a);
void estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle);
void estimateStateOfCharge(const float voltage_v, const float current_a);
uint8_t determineWarning(float state_of_charge);
void computeScale();
float computeRemainingTime(float current_a);
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
bool _battery_initialized{false};
src/modules/battery_status/analog_battery.cpp
+2 -2
View File
@@ -71,7 +71,7 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i
void
AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
int source, int priority, float throttle_normalized)
int source, int priority)
{
float voltage_v = voltage_raw * _analog_params.v_div;
float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v;
@@ -81,7 +81,7 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw,
Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected,
source, priority, throttle_normalized);
source, priority);
}
bool AnalogBattery::is_valid()
src/modules/battery_status/analog_battery.h
+1 -2
View File
@@ -49,10 +49,9 @@ public:
* @param timestamp Time at which the ADC was read (use hrt_absolute_time())
* @param source The source as defined by param BAT%d_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
*/
void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
int source, int priority, float throttle_normalized);
int source, int priority);
/**
* Whether the ADC channel for the voltage of this battery is valid.
src/modules/battery_status/battery_status.cpp
+2 -8
View File
@@ -56,10 +56,9 @@
#include <lib/perf/perf_counter.h>
#include <lib/battery/battery.h>
#include <lib/conversion/rotation.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/adc_report.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -100,7 +99,6 @@ public:
private:
void Run() override;
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< notification of parameter updates */
uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
@@ -220,16 +218,12 @@ BatteryStatus::adc_poll()
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
actuator_controls_s ctrl{};
_actuator_ctrl_0_sub.copy(&ctrl);
_analogBatteries[b]->updateBatteryStatusADC(
hrt_absolute_time(),
bat_voltage_adc_readings[b],
bat_current_adc_readings[b],
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
b,
ctrl.control[actuator_controls_s::INDEX_THROTTLE]
b
);
}
}
src/modules/esc_battery/EscBattery.cpp
+1 -5
View File
@@ -102,17 +102,13 @@ EscBattery::Run()
const bool connected = true;
const int priority = 0;
actuator_controls_s ctrl;
_actuator_ctrl_0_sub.copy(&ctrl);
_battery.updateBatteryStatus(
esc_status.timestamp,
average_voltage_v,
total_current_a,
connected,
battery_status_s::BATTERY_SOURCE_ESCS,
priority,
ctrl.control[actuator_controls_s::INDEX_THROTTLE]);
priority);
}
}
src/modules/esc_battery/EscBattery.hpp
+1 -3
View File
@@ -40,11 +40,10 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls.h>
#include <battery/battery.h>
using namespace time_literals;
@@ -72,7 +71,6 @@ private:
void parameters_updated();
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};
uORB::SubscriptionCallbackWorkItem _esc_status_sub{this, ORB_ID(esc_status)};
static constexpr uint32_t ESC_BATTERY_INTERVAL_US = 20_ms; // assume higher frequency esc feedback than 50Hz
src/modules/simulator/battery_simulator/BatterySimulator.cpp
+1 -1
View File
@@ -99,7 +99,7 @@ void BatterySimulator::Run()
float vbatt = math::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage());
vbatt *= _battery.cell_count();
_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0.f);
_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0);
perf_end(_loop_perf);
}