Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

battery: fix duplicate uORB publish and minor cleanup

Browse Source 
- run battery_status module on adc_report publications rather than a fixed schedule
This commit is contained in:
Daniel Agar
2020-10-05 11:01:58 -04:00
committed by GitHub
parent be28c4d309
commit 016ee6ea59
3 changed files with 37 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/battery/battery.cpp
+11 -26
View File
@@ -49,9 +49,7 @@ using namespace time_literals;
Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) : Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) :
ModuleParams(parent), ModuleParams(parent),
_index(index < 1 || index > 9 ? 1 : index), _index(index < 1 || index > 9 ? 1 : index)
_warning(battery_status_s::BATTERY_WARNING_NONE),
_last_timestamp(0)
{ {
const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s; const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
_voltage_filter_v.setParameters(expected_filter_dt, 1.f); _voltage_filter_v.setParameters(expected_filter_dt, 1.f);
@@ -105,8 +103,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us)
updateParams(); updateParams();
} }
void void Battery::reset()
Battery::reset()
{ {
memset(&_battery_status, 0, sizeof(_battery_status)); memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.current_a = -1.f; _battery_status.current_a = -1.f;
@@ -121,13 +118,10 @@ Battery::reset()
_battery_status.id = (uint8_t) _index; _battery_status.id = (uint8_t) _index;
} }
void void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, int source, int priority, float throttle_normalized)
bool connected, int source, int priority,
float throttle_normalized)
{ {
reset(); reset();
_battery_status.timestamp = timestamp;
if (!_battery_initialized) { if (!_battery_initialized) {
_voltage_filter_v.reset(voltage_v); _voltage_filter_v.reset(voltage_v);
@@ -169,24 +163,18 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
} }
} }
_battery_status.timestamp = timestamp; if (source == _params.source) {
const bool should_publish = (source == _params.source);
if (should_publish) {
_battery_status_pub.publish(_battery_status);
publish(); publish();
} }
} }
void void Battery::publish()
Battery::publish()
{ {
_battery_status.timestamp = hrt_absolute_time();
_battery_status_pub.publish(_battery_status); _battery_status_pub.publish(_battery_status);
} }
void void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{ {
// Not a valid measurement // Not a valid measurement
if (current_a < 0.f) { if (current_a < 0.f) {
@@ -207,8 +195,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
_last_timestamp = timestamp; _last_timestamp = timestamp;
} }
void void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
{ {
// remaining battery capacity based on voltage // remaining battery capacity based on voltage
float cell_voltage = voltage_v / _params.n_cells; float cell_voltage = voltage_v / _params.n_cells;
@@ -246,8 +233,7 @@ Battery::estimateRemaining(const float voltage_v, const float current_a, const f
} }
} }
void void Battery::determineWarning(bool connected)
Battery::determineWarning(bool connected)
{ {
if (connected) { if (connected) {
// propagate warning state only if the state is higher, otherwise remain in current warning state // propagate warning state only if the state is higher, otherwise remain in current warning state
@@ -266,8 +252,7 @@ Battery::determineWarning(bool connected)
} }
} }
void void Battery::computeScale()
Battery::computeScale()
{ {
const float voltage_range = (_params.v_charged - _params.v_empty); const float voltage_range = (_params.v_charged - _params.v_empty);
src/lib/battery/battery.h
+23 -22
View File
@@ -42,17 +42,18 @@
#pragma once #pragma once
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/battery_status.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
#include <parameters/param.h>
#include <board_config.h>
#include <px4_platform_common/board_common.h>
#include <math.h> #include <math.h>
#include <float.h> #include <float.h>
#include <board_config.h>
#include <px4_platform_common/board_common.h>
#include <px4_platform_common/module_params.h>
#include <drivers/drv_hrt.h>
#include <lib/parameters/param.h>
#include <lib/ecl/AlphaFilter/AlphaFilter.hpp> #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/battery_status.h>
/** /**
* BatteryBase is a base class for any type of battery. * BatteryBase is a base class for any type of battery.
@@ -96,7 +97,7 @@ public:
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 * @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 throttle_normalized: Throttle of the vehicle, between 0 and 1
*/ */
void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected, 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, float throttle_normalized);
/** /**
@@ -126,7 +127,7 @@ protected:
param_t v_load_drop_old; param_t v_load_drop_old;
param_t r_internal_old; param_t r_internal_old;
param_t source_old; param_t source_old;
} _param_handles; } _param_handles{};
struct { struct {
float v_empty; float v_empty;
@@ -149,9 +150,9 @@ protected:
float v_load_drop_old; float v_load_drop_old;
float r_internal_old; float r_internal_old;
int source_old; int source_old;
} _params; } _params{};
battery_status_s _battery_status; battery_status_s _battery_status{};
const int _index; const int _index;
@@ -192,25 +193,25 @@ protected:
} }
} }
bool isFloatEqual(float a, float b) { return fabsf(a - b) > FLT_EPSILON; } bool isFloatEqual(float a, float b) const { return fabsf(a - b) > FLT_EPSILON; }
private: private:
void sumDischarged(hrt_abstime timestamp, float current_a); void sumDischarged(const hrt_abstime &timestamp, float current_a);
void estimateRemaining(const float voltage_v, const float current_a, const float throttle); void estimateRemaining(const float voltage_v, const float current_a, const float throttle);
void determineWarning(bool connected); void determineWarning(bool connected);
void computeScale(); void computeScale();
uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)}; uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
bool _battery_initialized = false; bool _battery_initialized{false};
AlphaFilter<float> _voltage_filter_v; AlphaFilter<float> _voltage_filter_v;
AlphaFilter<float> _current_filter_a; AlphaFilter<float> _current_filter_a;
AlphaFilter<float> _throttle_filter; AlphaFilter<float> _throttle_filter;
float _discharged_mah = 0.f; float _discharged_mah{0.f};
float _discharged_mah_loop = 0.f; float _discharged_mah_loop{0.f};
float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage 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 _remaining{-1.f}; ///< normalized battery charge level, selected based on config param
float _scale = 1.f; float _scale{1.f};
uint8_t _warning; uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _last_timestamp; hrt_abstime _last_timestamp{0};
}; };
src/modules/battery_status/battery_status.cpp
+3 -4
View File
@@ -57,6 +57,7 @@
#include <lib/battery/battery.h> #include <lib/battery/battery.h>
#include <lib/conversion/rotation.h> #include <lib/conversion/rotation.h>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
@@ -101,7 +102,7 @@ private:
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */ uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100; static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100;
static constexpr uint32_t SAMPLE_INTERVAL_US = 1_s / SAMPLE_FREQUENCY_HZ; static constexpr uint32_t SAMPLE_INTERVAL_US = 1_s / SAMPLE_FREQUENCY_HZ;
@@ -280,9 +281,7 @@ BatteryStatus::task_spawn(int argc, char *argv[])
bool bool
BatteryStatus::init() BatteryStatus::init()
{ {
ScheduleOnInterval(SAMPLE_INTERVAL_US); return _adc_report_sub.registerCallback();
return true;
} }
int BatteryStatus::custom_command(int argc, char *argv[]) int BatteryStatus::custom_command(int argc, char *argv[])