fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan/sensors/battery: add the option to filter raw data

Browse Source 
Useful for power modules that send just pure voltage and current without algorithmic prediction of voltage drop, time estimation, etc.
This commit is contained in:
Igor Mišić 2022-07-07 17:33:09 +02:00 committed by Beat Küng
parent 27b65481ba
commit 90e2ac60ce
3 changed files with 72 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/drivers/uavcan/sensors/battery.cpp
View File

@ -50,6 +50,19 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) :
int UavcanBatteryBridge::init()
{
int32_t uavcan_sub_bat = 1;
param_get(param_find("UAVCAN_SUB_BAT"), &uavcan_sub_bat);
for (uint8_t instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
if (uavcan_sub_bat == FILTER_DATA) {
_batt_update_mod[instance] = BatteryDataType::Filter;
} else {
_batt_update_mod[instance] = BatteryDataType::Raw;
}
}
int res = _sub_battery.start(BatteryInfoCbBinder(this, &UavcanBatteryBridge::battery_sub_cb));
if (res < 0) {
@ -82,6 +95,12 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
return;
}
if (_batt_update_mod[instance] == BatteryDataType::Filter) {
filterData(msg, instance);
return;
}
_battery_status[instance].timestamp = hrt_absolute_time();
_battery_status[instance].voltage_v = msg.voltage;
_battery_status[instance].voltage_filtered_v = msg.voltage;
@ -89,7 +108,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
_battery_status[instance].current_filtered_a = msg.current;
_battery_status[instance].current_average_a = msg.current;
if (battery_aux_support[instance] == false) {
if (_batt_update_mod[instance] == BatteryDataType::Raw) {
sumDischarged(_battery_status[instance].timestamp, _battery_status[instance].current_a);
_battery_status[instance].discharged_mah = _discharged_mah;
}
@ -110,7 +129,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
_battery_status[instance].serial_number = msg.model_instance_id;
_battery_status[instance].id = msg.getSrcNodeID().get();
if (battery_aux_support[instance] == false) {
if (_batt_update_mod[instance] == BatteryDataType::Raw) {
// Mavlink 2 needs individual cell voltages or cell[0] if cell voltages are not available.
_battery_status[instance].voltage_cell_v[0] = msg.voltage;
@ -125,7 +144,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
determineWarning(_battery_status[instance].remaining);
_battery_status[instance].warning = _warning;
if (battery_aux_support[instance] == false) {
if (_batt_update_mod[instance] == BatteryDataType::Raw) {
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
}
}
@ -146,7 +165,11 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
return;
}
battery_aux_support[instance] = true;
if (_batt_update_mod[instance] == BatteryDataType::Filter) {
return;
}
_batt_update_mod[instance] = BatteryDataType::RawAux;
_battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh -
_battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
@ -203,3 +226,21 @@ UavcanBatteryBridge::determineWarning(float remaining)
_warning = battery_status_s::BATTERY_WARNING_LOW;
}
}
void
UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg,
uint8_t instance)
{
_battery.setConnected(true);
_battery.updateVoltage(msg.voltage);
_battery.updateCurrent(msg.current);
_battery.updateBatteryStatus(hrt_absolute_time());
/* Override data that is expected to arrive from UAVCAN msg*/
_battery_status[instance] = _battery.getBatteryStatus();
_battery_status[instance].temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius
_battery_status[instance].serial_number = msg.model_instance_id;
_battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
}

19
src/drivers/uavcan/sensors/battery.hpp
View File

@ -41,9 +41,12 @@
#include <uORB/topics/battery_status.h>
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
using namespace time_literals;
class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
{
public:
@ -57,10 +60,18 @@ public:
private:
/* Different options to update the battery status */
enum class BatteryDataType {
Raw, // data from BatteryInfo message only
RawAux, // data combination from BatteryInfo and BatteryInfoAux messages
Filter, // filter data from BatteryInfo message with Battery library
};
void battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg);
void battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &msg);
void sumDischarged(hrt_abstime timestamp, float current_a);
void determineWarning(float remaining);
void filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg, uint8_t instance);
typedef uavcan::MethodBinder < UavcanBatteryBridge *,
void (UavcanBatteryBridge::*)
@ -85,5 +96,11 @@ private:
uint8_t _warning;
hrt_abstime _last_timestamp;
battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {};
bool battery_aux_support[battery_status_s::MAX_INSTANCES] {false};
BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {};
static constexpr int FILTER_DATA = 2;
static constexpr int BATTERY_INDEX = 1;
static constexpr int SAMPLE_INTERVAL_US = 20_ms; // assume higher frequency UAVCAN feedback than 50Hz
Battery _battery{BATTERY_INDEX, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
};

10
src/drivers/uavcan/uavcan_params.c
View File

@ -228,7 +228,15 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0);
* uavcan::equipment::power::BatteryInfo
* ardupilot::equipment::power::BatteryInfoAux
*
* @boolean
* 0 - Disable
* 1 - Use raw data. Recommended for Smart battery
* 2 - Filter the data with internal battery library
*
* @min 0
* @max 2
* @value 0 Disable
* @value 1 Raw data
* @value 2 Filter data
* @reboot_required true
* @group UAVCAN
*/