AP_Periph: added battery balance plug node

this supports a periph node attached to a LiPo battery balance plug,
reporting via BatteryInfoAux
This commit is contained in:
Andrew Tridgell 2023-08-25 19:50:15 +10:00
parent 04e1dc65c6
commit bc825fb4c1
6 changed files with 180 additions and 1 deletions

View File

@ -465,6 +465,10 @@ void AP_Periph_FW::update()
rcin_update();
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE
batt_balance_update();
#endif
static uint32_t fiftyhz_last_update_ms;
if (now - fiftyhz_last_update_ms >= 20) {
// update at 50Hz

View File

@ -34,6 +34,7 @@
#endif
#include <AP_RCProtocol/AP_RCProtocol_config.h>
#include "rc_in.h"
#include "batt_balance.h"
#include <AP_NMEA_Output/AP_NMEA_Output.h>
#if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS))
@ -290,6 +291,11 @@ public:
Parameters_RCIN g_rcin;
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE
void batt_balance_update();
BattBalance battery_balance;
#endif
#if AP_TEMPERATURE_SENSOR_ENABLED
AP_TemperatureSensor temperature_sensor;
#endif

View File

@ -569,6 +569,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
GOBJECT(g_rcin, "RC", Parameters_RCIN),
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE
// @Group: BAL
// @Path: batt_balance.cpp
GOBJECT(battery_balance, "BAL", BattBalance),
#endif
// NOTE: sim parameters should go last
#if AP_SIM_ENABLED
// @Group: SIM_
// @Path: ../libraries/SITL/SITL.cpp

View File

@ -81,6 +81,7 @@ public:
k_param_g_rcin,
k_param_sitl,
k_param_ahrs,
k_param_battery_balance,
};
AP_Int16 format_version;

View File

@ -0,0 +1,137 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Periph.h"
#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE
#include <dronecan_msgs.h>
extern const AP_HAL::HAL &hal;
#ifndef AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT
#define AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT 0
#endif
#ifndef AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT
#define AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT 1
#endif
#ifndef AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT
#define AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT 1
#endif
#ifndef AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT
#define AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT 0
#endif
const AP_Param::GroupInfo BattBalance::var_info[] {
// @Param: _NUM_CELLS
// @DisplayName: Number of battery cells
// @Description: Number of battery cells to monitor
// @Range: 0 64
AP_GROUPINFO("_NUM_CELLS", 1, BattBalance, num_cells, AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT),
// @Param: _ID
// @DisplayName: Battery ID
// @Description: Battery ID to match against other batteries
// @Range: 0 127
AP_GROUPINFO("_ID", 2, BattBalance, id, AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT),
// @Param: _RATE
// @DisplayName: Send Rate
// @Description: Rate to send cell information
// @Range: 0 20
AP_GROUPINFO("_RATE", 3, BattBalance, rate, AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT),
// @Param: _CELL1_PIN
// @DisplayName: First analog pin
// @Description: Analog pin of the first cell. Later cells must be sequential
// @Range: 0 127
AP_GROUPINFO("_CELL1_PIN", 4, BattBalance, cell1_pin, AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT),
AP_GROUPEND
};
BattBalance::BattBalance(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
void AP_Periph_FW::batt_balance_update()
{
const int8_t ncell = battery_balance.num_cells;
if (ncell <= 0) {
return;
}
// allocate cell sources if needed
if (battery_balance.cells == nullptr) {
battery_balance.cells = new AP_HAL::AnalogSource*[ncell];
if (battery_balance.cells == nullptr) {
return;
}
battery_balance.cells_allocated = ncell;
for (uint8_t i=0; i<battery_balance.cells_allocated; i++) {
battery_balance.cells[i] = hal.analogin->channel(battery_balance.cell1_pin + i);
}
}
const uint32_t now = AP_HAL::millis();
if (now - battery_balance.last_send_ms < 1000.0/battery_balance.rate.get()) {
return;
}
battery_balance.last_send_ms = now;
// 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;
return;
}
pkt->voltage_cell.len = battery_balance.cells_allocated;
float last_cell = 0;
for (uint8_t i=0; i<battery_balance.cells_allocated; i++) {
auto *chan = battery_balance.cells[i];
if (chan == nullptr) {
continue;
}
const float v = chan->voltage_average();
pkt->voltage_cell.data[i] = v - last_cell;
last_cell = v;
}
pkt->max_current = nanf("");
pkt->nominal_voltage = nanf("");
pkt->battery_id = uint8_t(battery_balance.id);
// 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 pkt;
delete buffer;
}
#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE

View File

@ -0,0 +1,24 @@
#pragma once
#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE
class BattBalance {
public:
friend class AP_Periph_FW;
BattBalance(void);
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Int8 num_cells;
AP_Int8 id;
AP_Int8 cell1_pin;
AP_Float rate;
uint32_t last_send_ms;
AP_HAL::AnalogSource **cells;
uint8_t cells_allocated;
};
#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE