Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

lib/battery: fix duplicate uORB publish and minor cleanup #15880

Merged
merged 1 commit into from
Oct 5, 2020
Merged

lib/battery: fix duplicate uORB publish and minor cleanup #15880

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 11 additions & 26 deletions src/lib/battery/battery.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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