Skip to content

Commit

Permalink
AP_BattMonitor: add PacketDigital battery manager packets
Browse files Browse the repository at this point in the history
  • Loading branch information
@magicrub
magicrub committed Jun 17, 2024
1 parent 7003647 commit 208c414
Show file tree
Hide file tree
Showing 3 changed files with 392 additions and 0 deletions.
371 changes: 371 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,11 @@

#define LOG_TAG "BattMon"

#if AP_BATTMONITOR_DRONECAN_PACKETDIGITAL_ENABLE
#include <algorithm>
#include <AP_Logger/AP_Logger.h>
#endif

extern const AP_HAL::HAL& hal;

const AP_Param::GroupInfo AP_BattMonitor_DroneCAN::var_info[] = {
Expand Down Expand Up @@ -59,6 +64,16 @@ void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("mppt_stream_sub");
}

#if AP_BATTMONITOR_DRONECAN_PACKETDIGITAL_ENABLE
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_detail_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("battinfo_detail_sub");
}
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_cells_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("batt_cells_sub");
}
#endif

}

/*
Expand Down Expand Up @@ -305,6 +320,10 @@ void AP_BattMonitor_DroneCAN::read()
if (_mppt.is_detected) {
mppt_check_powered_state();
}

#if AP_BATTMONITOR_DRONECAN_PACKETDIGITAL_ENABLE
PD_Amprius_status_flags();
#endif
}

// Return true if the DroneCAN state of charge should be used.
Expand Down Expand Up @@ -366,6 +385,358 @@ bool AP_BattMonitor_DroneCAN::get_cycle_count(uint16_t &cycles) const
return false;
}

#if AP_BATTMONITOR_DRONECAN_PACKETDIGITAL_ENABLE
void AP_BattMonitor_DroneCAN::handle_battery_info_detail_trampoline(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_packetdigital_equipment_power_BatteryInfoDetail &msg)
{
AP_BattMonitor_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, transfer.source_node_id);
if (driver == nullptr) {
return;
}
driver->handle_battery_info_detail(msg);
}

void AP_BattMonitor_DroneCAN::handle_battery_cells_trampoline(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryCells &msg)
{
AP_BattMonitor_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, transfer.source_node_id);
if (driver == nullptr) {
return;
}
driver->handle_battery_cells(msg);
}

void AP_BattMonitor_DroneCAN::handle_battery_cells(const ardupilot_equipment_power_BatteryCells &msg)
{
if (_batteryInfoDetails.battery_id == 0) {
// not ready yet
return;
}

if (msg.index <= _batteryInfoDetails.battery_id*100 || msg.index > _batteryInfoDetails.battery_id*100 + 50) {
// invalid index
return;
}

// convert msg.index = 7401...7410 to subpack_index = 0...9
uint16_t subpack_index = (msg.index % 100) - 1;
const uint16_t subpack_count = _batteryInfoDetails.subpacks.len; // should be == 10

if (subpack_index >= subpack_count) {
// _batteryInfoDetails.subpacks needs to get populated before we fill it with cells
return;
}

for (uint16_t cell=0; cell<msg.voltages.len; cell++) {
_batteryInfoDetails.subpacks.data[subpack_index].cell_voltage.data[cell] = msg.voltages.data[cell];
}

if (subpack_index == subpack_count - 1) {
// if it's the last one, update all the things
handle_battery_info_detail(_batteryInfoDetails);
}
}

void AP_BattMonitor_DroneCAN::handle_battery_info_detail(const com_packetdigital_equipment_power_BatteryInfoDetail &msg)
{
// ignore model_instance_id - always 0
// ignore model_name - always "BATTERY MODEL NAME"
// battery_id was already checked in trampoline
// only thing left to parse is subpacks.data[<=10]

const uint16_t pack_count = constrain_int16(msg.subpacks.len, 0, ARRAY_SIZE(_interim_state.cell_voltages.cells));

if (pack_count == 0) {
return;
}

const uint64_t now_us = AP_HAL::micros64();

uavcan_equipment_power_BatteryInfo summary_batteryInfo {};
// these need to be 16bit so the uint7 accumulation doesn't wrap
uint16_t summary_state_of_health_pct = 0;
uint16_t summary_state_of_charge_pct = 0;
uint16_t summary_state_of_charge_pct_stdev = 0;

const uint16_t subpack_cells_count = constrain_int16(msg.subpacks.data[0].cell_voltage.len, 0, 12);

const uint16_t voltages_all_size = 60; // == (pack_count * subpack_cells_count) but clang doesn't like "voltages_all[variable]""
float voltages_all[voltages_all_size] {};

for (uint16_t pack=0; pack<pack_count; pack++) {
const uavcan_equipment_power_BatteryInfo subpack_battyInfo = msg.subpacks.data[pack].battery_info;

const uint16_t base_index = pack * subpack_cells_count;
for (uint16_t cell=0; cell<subpack_cells_count; cell++) {
if ((base_index + cell) >= ARRAY_SIZE(voltages_all)) {
break;
}
voltages_all[base_index + cell] = msg.subpacks.data[pack].cell_voltage.data[cell];
}

Log_Battery_Info(now_us, _instance, pack+1, subpack_battyInfo);
Log_Battery_Info_Details_Voltages(now_us, _instance, pack+1, &voltages_all[base_index], subpack_cells_count);

if (summary_batteryInfo.temperature < subpack_battyInfo.temperature) {
summary_batteryInfo.temperature = subpack_battyInfo.temperature;
}

summary_batteryInfo.voltage += subpack_battyInfo.voltage;
summary_batteryInfo.current += subpack_battyInfo.current;
summary_batteryInfo.average_power_10sec += subpack_battyInfo.average_power_10sec;
summary_batteryInfo.remaining_capacity_wh += subpack_battyInfo.remaining_capacity_wh;
summary_batteryInfo.full_charge_capacity_wh += subpack_battyInfo.full_charge_capacity_wh;
summary_batteryInfo.hours_to_full_charge += subpack_battyInfo.hours_to_full_charge;
summary_batteryInfo.status_flags |= subpack_battyInfo.status_flags;

// these need to be done separately so they don't wrap
summary_state_of_health_pct += (uint16_t)subpack_battyInfo.state_of_health_pct;
summary_state_of_charge_pct += (uint16_t)subpack_battyInfo.state_of_charge_pct;
summary_state_of_charge_pct_stdev += (uint16_t)subpack_battyInfo.state_of_charge_pct_stdev;
}

summary_batteryInfo.voltage /= pack_count;
summary_batteryInfo.current /= pack_count;
summary_batteryInfo.average_power_10sec /= pack_count;
summary_batteryInfo.remaining_capacity_wh /= pack_count;
summary_batteryInfo.full_charge_capacity_wh /= pack_count;
summary_batteryInfo.hours_to_full_charge /= pack_count;
//summary_batteryInfo.status_flags // already set
summary_batteryInfo.state_of_health_pct = summary_state_of_health_pct / pack_count;
summary_batteryInfo.state_of_charge_pct = summary_state_of_charge_pct / pack_count;
summary_batteryInfo.state_of_charge_pct_stdev = summary_state_of_charge_pct_stdev / pack_count;

Log_Battery_Info(now_us, _instance, 0, summary_batteryInfo);

uint16_t cells_count = ARRAY_SIZE(_interim_state.cell_voltages.cells); // == AP_BATT_MONITOR_CELLS_MAX

#if 1 // TODO
const int8_t cell_select = -1; // _params._cells_select.get();
#endif

switch (cell_select) {
case 1 ... AP_BATT_MONITOR_CELLS_MAX: // show the selected pack
if (cell_select <= pack_count) {
const com_packetdigital_equipment_power_Subpack subpack = msg.subpacks.data[cell_select-1];
update_interim_state(subpack.battery_info.voltage, subpack.battery_info.current, subpack.battery_info.temperature, subpack.battery_info.state_of_charge_pct, subpack.battery_info.state_of_health_pct);
WITH_SEMAPHORE(_sem_battmon);
for (uint16_t cell=0; cell<cells_count; cell++) {
if (cell < subpack_cells_count) {
_interim_state.cell_voltages.cells[cell] = subpack.cell_voltage.data[cell] * 1000; // convert volt to millivolt
} else {
_interim_state.cell_voltages.cells[cell] = 0;
}
}
_interim_state.consumed_wh = subpack.battery_info.full_charge_capacity_wh - subpack.battery_info.remaining_capacity_wh;
_interim_state.consumed_mah = _interim_state.consumed_wh / subpack.battery_info.voltage;

} else {
// out of range, set them all to zero
for (uint16_t cell = 0; cell < cells_count; cell++) {
_interim_state.cell_voltages.cells[cell] = 0;
}
}
break;

case -1: // sorted as lowest to 14th lowest
case -2: // sorted as highest to 14th highest
{
std::sort(voltages_all, voltages_all+voltages_all_size);
update_interim_state(summary_batteryInfo.voltage, summary_batteryInfo.current, summary_batteryInfo.temperature, summary_batteryInfo.state_of_charge_pct, summary_batteryInfo.state_of_health_pct);
WITH_SEMAPHORE(_sem_battmon);
for (uint16_t cell=0; cell<cells_count; cell++) {
if (cell_select == -2) { // ascending order
_interim_state.cell_voltages.cells[cell] = voltages_all[cell] * 1000; // convert volt to millivolt
} else { // descending order
_interim_state.cell_voltages.cells[cell] = voltages_all[voltages_all_size - cell - 1] * 1000; // convert volt to millivolt
}
}
_interim_state.consumed_wh = summary_batteryInfo.full_charge_capacity_wh - summary_batteryInfo.remaining_capacity_wh;
_interim_state.consumed_mah = _interim_state.consumed_wh / summary_batteryInfo.voltage;
}
break;

case 0:
// handled in handle_battery_info
break;

default:
for (uint16_t cell = 0; cell < cells_count; cell++) {
_interim_state.cell_voltages.cells[cell] = 0;
}
break;
}

_has_cell_voltages = true;
}

void AP_BattMonitor_DroneCAN::PD_Amprius_status_flags()
{
const uint16_t subpack_count = _batteryInfoDetails.subpacks.len;
if (subpack_count == 0) {
return;
}

const uint32_t now_ms = AP_HAL::millis();
const uint16_t enabled_flags_mask =
// UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_IN_USE | // The battery is currently used as a power supply
// UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_CHARGING | // Charger is active
// UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_CHARGED | // Charging complete, but the charger is still active
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_TEMP_HOT | // Battery temperature is above normal
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_TEMP_COLD | // Battery temperature is below normal
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_OVERLOAD | // Safe operating area violation
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_BAD_BATTERY | // This battery should not be used anymore (e.g. low SOH)
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_NEED_SERVICE | // This battery requires maintenance (e.g. balancing, full recharge)
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_BMS_ERROR | // Battery management system/controller error, smart battery interface error
// UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_RESERVED_A | // Keep zero
// UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_RESERVED_B | // Keep zero
0;

uint16_t active_flags = 0;
// check if any flags have changed on any of the subpacks
for (uint16_t i=0; i<subpack_count; i++) {
active_flags |= _batteryInfoDetails.subpacks.data[i].battery_info.status_flags;
}

// clear any flags tyhat we don't want to track
active_flags &= enabled_flags_mask;

if (_batteryInfoDetails_status_flags_prev == 0 && active_flags == 0) {
// no previous or new flags. Nothing to report
return;
}

// announce every minute if there are any flags
if (_batteryInfoDetails_status_flags_prev == active_flags && now_ms - _batteryInfoDetails_status_flags_last_ms < 60*1000) {
// no change since last announcement
return;
}
_batteryInfoDetails_status_flags_last_ms = now_ms;


if (active_flags == 0) {
// all the flags cleared. Horray!
GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Amprius Pack: ALL OK");
} else {
for (uint16_t i=0; i<subpack_count; i++) {
if ((_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & enabled_flags_mask) == 0) {
continue;
}
// generate a GCS msgs for each subpack that has any flags
GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Amprius Pack%u:%s%s%s%s%s%s%s%s%s", (unsigned)(i + 1),
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_IN_USE) ? " INUSE" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_CHARGING) ? " CHARGING" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_CHARGED) ? " CHARGED" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_TEMP_HOT) ? " HOT" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_TEMP_COLD) ? " COLD" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_OVERLOAD) ? " OVERLOAD" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_BAD_BATTERY) ? " BAD_BATTERY" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_NEED_SERVICE) ? " NEED_SERVICE" : "",
(_batteryInfoDetails.subpacks.data[i].battery_info.status_flags & UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_BMS_ERROR) ? " BMS_ERROR" : "");
}
}
_batteryInfoDetails_status_flags_prev = active_flags;
}

void AP_BattMonitor_DroneCAN::Log_Battery_Info_Details_Voltages(const uint64_t now_us, const uint8_t instance, const int8_t subpack_index, const float* cell_voltages, const uint16_t cell_voltages_count) const
{
#if HAL_LOGGING_ENABLED
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr || !logger->should_log(_mon._log_battery_bit)) {
return;
}

float voltages[12] {};
for (uint16_t i=0; i<MIN(ARRAY_SIZE(voltages),cell_voltages_count); i++) {
voltages[i] = cell_voltages[i];
}

// @LoggerMessage: PDBV
// @Description: PacketDigital BatteryInfo Details Voltages packet
// @Field: TimeUS: Time since system startup
// @Field: Pack: Pack index starting at 1 for the first pack. Typically 1 through 10
// @Field: Inst: Battery driver instance number starting at 1
// @Field: V1: Pack cell 1 voltage - Volts
// @Field: V2: Pack cell 2 voltage - Volts
// @Field: V3: Pack cell 3 voltage - Volts
// @Field: V4: Pack cell 4 voltage - Volts
// @Field: V5: Pack cell 5 voltage - Volts
// @Field: V6: Pack cell 6 voltage - Volts
// @Field: V7: Pack cell 7 voltage - Volts (value is 0 for Amprius 6S battery)
// @Field: V8: Pack cell 8 voltage - Volts (value is 0 for Amprius 6S battery)
// @Field: V9: Pack cell 9 voltage - Volts (value is 0 for Amprius 6S battery)
// @Field: V10: Pack cell 10 voltage - Volts (value is 0 for Amprius 6S battery)
// @Field: V11: Pack cell 11 voltage - Volts (value is 0 for Amprius 6S battery)
// @Field: V12: Pack cell 12 voltage - Volts (value is 0 for Amprius 6S battery)
AP::logger().Write("PDBV",
"TimeUS,Pack,Inst,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12",
"s#-------------",
"F--------------",
"QbBffffffffffff",
now_us,
subpack_index,
instance+1,
(double)(voltages[0]),
(double)(voltages[1]),
(double)(voltages[2]),
(double)(voltages[3]),
(double)(voltages[4]),
(double)(voltages[5]),
(double)(voltages[6]),
(double)(voltages[7]),
(double)(voltages[8]),
(double)(voltages[9]),
(double)(voltages[10]),
(double)(voltages[11]));

#endif
}

void AP_BattMonitor_DroneCAN::Log_Battery_Info(const uint64_t now_us, const uint8_t instance, const int8_t subpack_index, const uavcan_equipment_power_BatteryInfo &battery_info) const
{
#if HAL_LOGGING_ENABLED
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr || !logger->should_log(_mon._log_battery_bit)) {
return;
}

// @LoggerMessage: PDBI
// @Description: PacketDigital BatteryInfo packet
// @Field: TimeUS: Time since system startup
// @Field: Pack: -1 for summary msg from battery, 0 for summary locally created from subpack msgs, else first subpack number starts at 1
// @Field: Inst: Battery driver instance number starting at 1.
// @Field: T: Temperature - C
// @Field: V: Pack avg voltage - Volts
// @Field: C: Pack current - Amps
// @Field: AP: Average Power 10sec - Watts
// @Field: RC: Remaining Capacity - Watt Hours
// @Field: FCC: Full Charge Capacity - Watt Hours
// @Field: HFC: Hours to Full Charge - Hours
// @Field: F: Status Flag. For "Pack" <= 0 it's an OR of all subpacks. Else it's the individual subpack
// @Field: H: Health of the battery - %
// @Field: SoC: State of Charge - %
// @Field: SoCS: State of Charge error Stdev
AP::logger().Write("PDBI",
"TimeUS,Pack,Inst,T,V,C,AP,RC,FCC,HFC,F,H,SoC,SoCS",
"s#------------",
"F-------------",
"QbBfffffffBBBB",
now_us,
subpack_index,
instance+1,
(double)KELVIN_TO_C(battery_info.temperature),
(double)battery_info.voltage,
(double)(battery_info.current * _curr_mult),
(double)battery_info.average_power_10sec,
(double)battery_info.remaining_capacity_wh,
(double)battery_info.full_charge_capacity_wh,
(double)battery_info.hours_to_full_charge,
battery_info.status_flags,
battery_info.state_of_health_pct,
battery_info.state_of_charge_pct,
battery_info.state_of_charge_pct_stdev);
#endif
}
#endif // AP_BATTMONITOR_DRONECAN_PACKETDIGITAL_ENABLE

// request MPPT board to power on/off depending upon vehicle arming state as specified by BATT_OPTIONS
void AP_BattMonitor_DroneCAN::mppt_check_powered_state()
{
Expand Down
Loading

0 comments on commit 208c414

Please sign in to comment.