ardupilot/Tools/AP_Periph/battery.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

129 lines
4.1 KiB
C++
Raw Normal View History

#include "AP_Periph.h"
#ifdef HAL_PERIPH_ENABLE_BATTERY
/*
battery support
*/
#include <dronecan_msgs.h>
extern const AP_HAL::HAL &hal;
#ifndef AP_PERIPH_BATTERY_MODEL_NAME
#define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME
#endif
/*
update CAN battery monitor
*/
void AP_Periph_FW::can_battery_update(void)
{
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - battery.last_can_send_ms < 100) {
return;
}
battery.last_can_send_ms = now_ms;
const uint8_t battery_instances = battery_lib.num_instances();
for (uint8_t i=0; i<battery_instances; i++) {
2023-09-07 19:07:59 -03:00
if (BIT_IS_SET(g.battery_hide_mask, i)) {
// do not transmit this battery
continue;
}
if (!battery_lib.healthy(i)) {
continue;
}
uavcan_equipment_power_BatteryInfo pkt {};
// if a battery serial number is assigned, use that as the ID. Else, use the index.
const int32_t serial_number = battery_lib.get_serial_number(i);
pkt.battery_id = (serial_number >= 0) ? serial_number : i+1;
pkt.voltage = battery_lib.voltage(i);
float current;
if (battery_lib.current_amps(current, i)) {
pkt.current = current;
}
float temperature;
if (battery_lib.get_temperature(temperature, i)) {
// Battery lib reports temperature in Celsius.
// Convert Celsius to Kelvin for transmission on CAN.
pkt.temperature = C_TO_KELVIN(temperature);
}
pkt.state_of_health_pct = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN;
uint8_t percentage = 0;
if (battery_lib.capacity_remaining_pct(percentage, i)) {
pkt.state_of_charge_pct = percentage;
}
pkt.model_instance_id = i+1;
#if !defined(HAL_PERIPH_BATTERY_SKIP_NAME)
// example model_name: "org.ardupilot.ap_periph SN 123"
hal.util->snprintf((char*)pkt.model_name.data, sizeof(pkt.model_name.data), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number);
pkt.model_name.len = strnlen((char*)pkt.model_name.data, sizeof(pkt.model_name.data));
#endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME)
uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {};
const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE,
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
// Send individual cell information if available
if (battery_lib.has_cell_voltages(i)) {
can_battery_send_cells(i);
}
}
}
/*
send individual cell voltages if available
*/
void AP_Periph_FW::can_battery_send_cells(uint8_t instance)
{
// allocate space for the packet. This is a large
// packet that won't fit on the stack, so dynamically allocate
auto* pkt = new ardupilot_equipment_power_BatteryInfoAux;
uint8_t* buffer = new uint8_t[ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_MAX_SIZE];
if (pkt == nullptr || buffer == nullptr) {
delete pkt;
delete [] buffer;
return;
}
const auto &cell_voltages = battery_lib.get_cell_voltages(instance);
for (uint8_t i = 0; i < ARRAY_SIZE(cell_voltages.cells); i++) {
if (cell_voltages.cells[i] == 0xFFFFU) {
break;
}
pkt->voltage_cell.data[i] = cell_voltages.cells[i]*0.001;
pkt->voltage_cell.len = i+1;
}
pkt->max_current = nanf("");
pkt->nominal_voltage = nanf("");
// encode and send message:
const uint16_t total_size = ardupilot_equipment_power_BatteryInfoAux_encode(pkt, buffer, !periph.canfdout());
canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_SIGNATURE,
ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_ID,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
total_size);
// Delete temporary buffers
delete pkt;
delete [] buffer;
}
#endif // HAL_PERIPH_ENABLE_BATTERY