Skip to content

Commit

Permalink
Remove filter option; read serial number
Browse files Browse the repository at this point in the history
  • Loading branch information
@sdomoszlai13
sdomoszlai13 committed Jan 20, 2025
1 parent e2bba22 commit 6efa3fa
Show file tree
Hide file tree
Showing 3 changed files with 5 additions and 81 deletions.
69 changes: 5 additions & 64 deletions src/drivers/uavcan/sensors/battery.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,9 +65,6 @@ int UavcanBatteryBridge::init()

} else if (uavcan_sub_bat == RAW_AUX_CBAT_DATA) {
_batt_update_mod[instance] = BatteryDataType::RawAuxCBAT;

} else if (uavcan_sub_bat == FILTER_DATA) {
_batt_update_mod[instance] = BatteryDataType::Filter;
}
}

Expand All @@ -85,8 +82,6 @@ int UavcanBatteryBridge::init()
return res;
}

return 0;

res = _sub_cbat.start(CBATCbBinder(this, &UavcanBatteryBridge::cbat_sub_cb));

if (res < 0) {
Expand All @@ -100,8 +95,6 @@ int UavcanBatteryBridge::init()
void
UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg)
{
PX4_INFO("Battery Sub Cb called");

uint8_t instance = 0;

for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
Expand All @@ -114,12 +107,6 @@ 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].current_a = msg.current;
Expand All @@ -130,7 +117,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
_battery_status[instance].discharged_mah = _discharged_mah;
}

_battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1
_battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // Between 0 and 1
// _battery_status[instance].scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
// _battery_status[instance].cell_count = msg.;
Expand Down Expand Up @@ -163,17 +150,13 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::

if (_batt_update_mod[instance] == BatteryDataType::Raw) {
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
PX4_INFO("Battery Sub Cb published");

}
}

void
UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux>
&msg)
{
PX4_INFO("Battery Aux Sub Cb called");

uint8_t instance = 0;

for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
Expand All @@ -186,10 +169,6 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
return;
}

if (_batt_update_mod[instance] == BatteryDataType::Filter) {
return;
}

_battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh -
_battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
1000;
Expand All @@ -208,7 +187,6 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu

if (_batt_update_mod[instance] == BatteryDataType::RawAux) {
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
PX4_INFO("Battery Aux Sub Cb published");
}

}
Expand All @@ -217,18 +195,6 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
void
UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg)
{
/*
if (msg.serial_number != 0){
PX4_ERR("CBAT message received");
}
else{
PX4_ERR("No CBAT message received");
}
*/

PX4_INFO("Battery CBAT Sub Cb called");

uint8_t instance = 0;

for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
Expand All @@ -242,24 +208,17 @@ UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equip
return;
}

if (_batt_update_mod[instance] == BatteryDataType::Filter) {
return;
}

determineWarning(_battery_status[instance].remaining);
_battery_status[instance].warning = _warning;

if (_batt_update_mod[instance] == BatteryDataType::RawAuxCBAT) {
// char ser_num[32] = {0};
// snprintf(ser_num, 32, "%hu", msg.serial_number);
// strncpy(_battery_status[instance].serial_number, ser_num, sizeof(_battery_status[instance].serial_number) - 1);
strncpy(_battery_status[instance].serial_number, "123456", sizeof(_battery_status[instance].serial_number));
char ser_num[32] = {0};
snprintf(ser_num, 32, "%hu", msg.serial_number);
strncpy(_battery_status[instance].serial_number, ser_num, sizeof(_battery_status[instance].serial_number) - 1);

_battery_status[instance].serial_number[sizeof(_battery_status[instance].serial_number) - 1] = '\0';

publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
PX4_INFO("CBAT Sub Cb published");

}
}

Expand Down Expand Up @@ -288,7 +247,7 @@ UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
void
UavcanBatteryBridge::determineWarning(float remaining)
{
// propagate warning state only if the state is higher, otherwise remain in current warning state
// Propagate warning state only if new state is higher, otherwise remain in current warning state
if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::WARNING_EMERGENCY)) {
_warning = battery_status_s::WARNING_EMERGENCY;

Expand All @@ -299,21 +258,3 @@ UavcanBatteryBridge::determineWarning(float remaining)
_warning = battery_status_s::WARNING_LOW;
}
}

void
UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg,
uint8_t instance)
{
_battery[instance]->setConnected(true);
_battery[instance]->updateVoltage(msg.voltage);
_battery[instance]->updateCurrent(msg.current);
_battery[instance]->updateBatteryStatus(hrt_absolute_time());

/* Override data that is expected to arrive from UAVCAN msg*/
_battery_status[instance] = _battery[instance]->getBatteryStatus();
_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
//_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]);
}
16 changes: 0 additions & 16 deletions src/drivers/uavcan/sensors/battery.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,15 +66,13 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
Raw, // data from BatteryInfo message only
RawAux, // data combination from BatteryInfo and BatteryInfoAux messages
RawAuxCBAT, // data combination from BatteryInfo, BatteryInfoAux, and CBAT 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 cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &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::*)
Expand Down Expand Up @@ -109,19 +107,5 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
static constexpr int RAW_DATA = 1;
static constexpr int RAW_AUX_DATA = 2;
static constexpr int RAW_AUX_CBAT_DATA = 3;
static constexpr int FILTER_DATA = 4;
static constexpr int BATTERY_INDEX_1 = 1;
static constexpr int BATTERY_INDEX_2 = 2;
static constexpr int BATTERY_INDEX_3 = 3;
static constexpr int BATTERY_INDEX_4 = 4;
static constexpr int SAMPLE_INTERVAL_US = 20_ms; // assume higher frequency UAVCAN feedback than 50Hz

static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big");

Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};

Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3, &battery4 };
};
1 change: 0 additions & 1 deletion src/drivers/uavcan/uavcan_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -302,7 +302,6 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0);
* @value 1 Raw data
* @value 2 Raw and auxiliary data
* @value 3 Raw, auxiliary, and CBAT-specific data
* @value 4 Filter data
* @reboot_required true
* @group UAVCAN
*/
Expand Down

0 comments on commit 6efa3fa

Please sign in to comment.