mirror of https://github.com/ArduPilot/ardupilot
129 lines
4.1 KiB
C++
129 lines
4.1 KiB
C++
#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++) {
|
|
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
|
|
|