Skip to content

Commit

Permalink
battery: fix duplicate uORB publish and minor cleanup
Browse files Browse the repository at this point in the history 
 - run battery_status module on adc_report publications rather than a fixed schedule
  • Loading branch information
@dagar
dagar committed Oct 5, 2020
1 parent 3188408 commit 9fcbf18
Show file tree
Hide file tree
Showing 3 changed files with 37 additions and 52 deletions.
37 changes: 11 additions & 26 deletions src/lib/battery/battery.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,7 @@ using namespace time_literals;

Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) :
ModuleParams(parent),
_index(index < 1 || index > 9 ? 1 : index),
_warning(battery_status_s::BATTERY_WARNING_NONE),
_last_timestamp(0)
_index(index < 1 || index > 9 ? 1 : index)
{
const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
Expand Down Expand Up @@ -105,8 +103,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us)
updateParams();
}

void
Battery::reset()
void Battery::reset()
{
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.current_a = -1.f;
Expand All @@ -121,13 +118,10 @@ Battery::reset()
_battery_status.id = (uint8_t) _index;
}

void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
bool connected, int source, int priority,
float throttle_normalized)
void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
int source, int priority, float throttle_normalized)
{
reset();
_battery_status.timestamp = timestamp;

if (!_battery_initialized) {
_voltage_filter_v.reset(voltage_v);
Expand Down Expand Up @@ -169,24 +163,18 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
}
}

_battery_status.timestamp = timestamp;

const bool should_publish = (source == _params.source);

if (should_publish) {
_battery_status_pub.publish(_battery_status);
if (source == _params.source) {
publish();
}
}

void
Battery::publish()
void Battery::publish()
{
_battery_status.timestamp = hrt_absolute_time();
_battery_status_pub.publish(_battery_status);
}

void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
{
// Not a valid measurement
if (current_a < 0.f) {
Expand All @@ -207,8 +195,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
_last_timestamp = timestamp;
}

void
Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
{
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _params.n_cells;
Expand Down Expand Up @@ -246,8 +233,7 @@ Battery::estimateRemaining(const float voltage_v, const float current_a, const f
}
}

void
Battery::determineWarning(bool connected)
void Battery::determineWarning(bool connected)
{
if (connected) {
// propagate warning state only if the state is higher, otherwise remain in current warning state
Expand All @@ -266,8 +252,7 @@ Battery::determineWarning(bool connected)
}
}

void
Battery::computeScale()
void Battery::computeScale()
{
const float voltage_range = (_params.v_charged - _params.v_empty);

Expand Down
45 changes: 23 additions & 22 deletions src/lib/battery/battery.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,17 +42,18 @@

#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 <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 <uORB/PublicationMulti.hpp>
#include <uORB/topics/battery_status.h>

/**
* BatteryBase is a base class for any type of battery.
Expand Down Expand Up @@ -96,7 +97,7 @@ class Battery : public ModuleParams
* @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(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);

/**
Expand Down Expand Up @@ -126,7 +127,7 @@ class Battery : public ModuleParams
param_t v_load_drop_old;
param_t r_internal_old;
param_t source_old;
} _param_handles;
} _param_handles{};

struct {
float v_empty;
Expand All @@ -149,9 +150,9 @@ class Battery : public ModuleParams
float v_load_drop_old;
float r_internal_old;
int source_old;
} _params;
} _params{};

battery_status_s _battery_status;
battery_status_s _battery_status{};

const int _index;

Expand Down Expand Up @@ -192,25 +193,25 @@ class Battery : public ModuleParams
}
}

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:
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 determineWarning(bool connected);
void computeScale();

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> _current_filter_a;
AlphaFilter<float> _throttle_filter;
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;
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{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _last_timestamp{0};
};
7 changes: 3 additions & 4 deletions src/modules/battery_status/battery_status.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@
#include <lib/battery/battery.h>
#include <lib/conversion/rotation.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
Expand Down Expand Up @@ -101,7 +102,7 @@ class BatteryStatus : public ModuleBase<BatteryStatus>, public ModuleParams, pub

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 _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_INTERVAL_US = 1_s / SAMPLE_FREQUENCY_HZ;
Expand Down Expand Up @@ -280,9 +281,7 @@ BatteryStatus::task_spawn(int argc, char *argv[])
bool
BatteryStatus::init()
{
ScheduleOnInterval(SAMPLE_INTERVAL_US);

return true;
return _adc_report_sub.registerCallback();
}

int BatteryStatus::custom_command(int argc, char *argv[])
Expand Down

0 comments on commit 9fcbf18

Please sign in to comment.