diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index c4f71cbdc3..14a22ea8f9 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -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 diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 4fa3f664cc..9869e267c5 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -34,6 +34,7 @@ #endif #include #include "rc_in.h" +#include "batt_balance.h" #include #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 diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 6e4ce775ea..872ef3da48 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -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 @@ -580,7 +587,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(ahrs, "AHRS_", AP_AHRS), #endif #endif // AP_SIM_ENABLED - + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 03b9ba6282..ff412b0e93 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -81,6 +81,7 @@ public: k_param_g_rcin, k_param_sitl, k_param_ahrs, + k_param_battery_balance, }; AP_Int16 format_version; diff --git a/Tools/AP_Periph/batt_balance.cpp b/Tools/AP_Periph/batt_balance.cpp new file mode 100644 index 0000000000..fd1fec4207 --- /dev/null +++ b/Tools/AP_Periph/batt_balance.cpp @@ -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 . + */ + +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE + +#include + +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; ichannel(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; ivoltage_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 + diff --git a/Tools/AP_Periph/batt_balance.h b/Tools/AP_Periph/batt_balance.h new file mode 100644 index 0000000000..961875c660 --- /dev/null +++ b/Tools/AP_Periph/batt_balance.h @@ -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 +