fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adding BatMon smart battery as a module and refactoring SMBUS based SBS 1.1 spec to a library

Browse Source
This commit is contained in:
echoG 2021-06-09 12:17:45 -04:00 committed by GitHub
parent 8be1034346
commit 6bc09138c1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
66 changed files with 1012 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -26,6 +26,12 @@ then
batt_smbus start -X
fi
# Start batmon driver if enabled using BATMON_DRIVER_EN
if param compare -s BATMON_DRIVER_EN 1
then
batmon start -X #start on external bus
fi
# Sensors on the PWM interface bank
if param compare -s SENS_EN_LL40LS 1
then

1
boards/airmind/mindpx-v2/default.cmake
View File

@ -46,6 +46,7 @@ px4_add_board(
rc_input
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/atlflight/eagle/default.cmake
View File

@ -54,6 +54,7 @@ px4_add_board(
pwm_out_sim
qshell/posix
rc_input
smart_battery/batmon
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

1
boards/atlflight/excelsior/default.cmake
View File

@ -53,6 +53,7 @@ px4_add_board(
pwm_out_sim
qshell/posix
rc_input
smart_battery/batmon
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

1
boards/av/x-v1/default.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
rc_input
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
#tone_alarm
uavcan

1
boards/beaglebone/blue/default.cmake
View File

@ -33,6 +33,7 @@ px4_add_board(
magnetometer/hmc5883
pwm_out_sim
rc_input
smart_battery/batmon
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

1
boards/cuav/nora/default.cmake
View File

@ -52,6 +52,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/cuav/x7pro/default.cmake
View File

@ -53,6 +53,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/cubepilot/cubeorange/default.cmake
View File

@ -49,6 +49,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/cubepilot/cubeorange/test.cmake
View File

@ -49,6 +49,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
test_ppm
tone_alarm

1
boards/cubepilot/cubeyellow/default.cmake
View File

@ -47,6 +47,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/cubepilot/cubeyellow/test.cmake
View File

@ -47,6 +47,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
test_ppm
tone_alarm

1
boards/emlid/navio2/default.cmake
View File

@ -33,6 +33,7 @@ px4_add_board(
magnetometer/lsm9ds1_mag
pwm_out_sim
rc_input
smart_battery/batmon
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

1
boards/holybro/durandal-v1/default.cmake
View File

@ -47,6 +47,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/holybro/pix32v5/default.cmake
View File

@ -51,6 +51,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/modalai/fc-v1/default.cmake
View File

@ -44,6 +44,7 @@ px4_add_board(
rc_input
roboclaw
rpm
smart_battery/batmon
safety_button
telemetry # all available telemetry drivers
#tone_alarm

1
boards/mro/ctrl-zero-f7-oem/default.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/mro/ctrl-zero-f7/default.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

2
boards/mro/ctrl-zero-h7-oem/default.cmake
View File

@ -46,6 +46,8 @@ px4_add_board(
rc_input
roboclaw
rpm
#safety_button TODO
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

2
boards/mro/ctrl-zero-h7/default.cmake
View File

@ -46,6 +46,8 @@ px4_add_board(
rc_input
roboclaw
rpm
#safety_button TODO
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/mro/pixracerpro/default.cmake
View File

@ -47,6 +47,7 @@ px4_add_board(
rc_input
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/mro/x21-777/default.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/mro/x21/default.cmake
View File

@ -49,6 +49,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/nxp/fmuk66-e/default.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/nxp/fmuk66-e/rtps.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/nxp/fmuk66-e/socketcan.cmake
View File

@ -44,6 +44,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan_v1

1
boards/nxp/fmuk66-v3/default.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/nxp/fmuk66-v3/rtps.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/nxp/fmuk66-v3/socketcan.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan_v1

1
boards/nxp/fmurt1062-v1/default.cmake
View File

@ -46,6 +46,7 @@ px4_add_board(
#roboclaw
#rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
#uavcan

1
boards/px4/fmu-v2/rover.cmake
View File

@ -34,6 +34,7 @@ px4_add_board(
optical_flow/px4flow
pwm_out
px4io
smart_battery/batmon
tone_alarm
MODULES

1
boards/px4/fmu-v3/default.cmake
View File

@ -50,6 +50,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v4/default.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

2
boards/px4/fmu-v4/test.cmake
View File

@ -49,6 +49,8 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
tap_esc
telemetry # all available telemetry drivers
test_ppm
tone_alarm

1
boards/px4/fmu-v4pro/default.cmake
View File

@ -49,6 +49,7 @@ px4_add_board(
px4io
roboclaw
rpm
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5/ctrlalloc.cmake
View File

@ -50,6 +50,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5/default.cmake
View File

@ -49,6 +49,7 @@ px4_add_board(
px4io
rc_input
roboclaw
smart_battery/batmon
rpm
safety_button
telemetry # all available telemetry drivers

1
boards/px4/fmu-v5/fixedwing.cmake
View File

@ -47,6 +47,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5/multicopter.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5/optimized.cmake
View File

@ -56,6 +56,7 @@ px4_add_board(
#roboclaw
#rpm
safety_button
smart_battery/batmon
#telemetry # all available telemetry drivers
test_ppm
tone_alarm

1
boards/px4/fmu-v5/rover.cmake
View File

@ -40,6 +40,7 @@ px4_add_board(
rc_input
roboclaw
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5/rtps.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -53,6 +53,7 @@ px4_add_board(
#roboclaw
#rpm
safety_button
#smart_battery/batmon
telemetry # all available telemetry drivers
#test_ppm
tone_alarm

1
boards/px4/fmu-v5/uavcanv0periph.cmake
View File

@ -54,6 +54,7 @@ px4_add_board(
#roboclaw
#rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5/uavcanv1.cmake
View File

@ -50,6 +50,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
#uavcan # legacy v0

1
boards/px4/fmu-v5x/base_phy_DP83848C.cmake
View File

@ -51,6 +51,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v5x/default.cmake
View File

@ -52,6 +52,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v6u/default.cmake
View File

@ -47,6 +47,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/fmu-v6x/default.cmake
View File

@ -52,6 +52,7 @@ px4_add_board(
roboclaw
rpm
safety_button
smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
uavcan

1
boards/px4/raspberrypi/default.cmake
View File

@ -30,6 +30,7 @@ px4_add_board(
pwm_out_sim
rc_input
rpi_rc_in
smart_battery/batmon
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

1
boards/scumaker/pilotpi/arm64.cmake
View File

@ -31,6 +31,7 @@ px4_add_board(
pca9685_pwm_out
pwm_out_sim
rc_input
smart_battery/batmon
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

1
boards/scumaker/pilotpi/default.cmake
View File

@ -31,6 +31,7 @@ px4_add_board(
pca9685_pwm_out
pwm_out_sim
rc_input
smart_battery/batmon
#telemetry # all available telemetry drivers
MODULES
airspeed_selector

1
boards/spracing/h7extreme/default.cmake
View File

@ -43,6 +43,7 @@ px4_add_board(
pwm_out
#roboclaw
rc_input
#smart_battery/batmon
telemetry # all available telemetry drivers
tone_alarm
MODULES

8
platforms/common/i2c_spi_buses.cpp
View File

@ -640,7 +640,13 @@ int I2CSPIDriverBase::module_custom_method(const BusCLIArguments &cli, BusInstan
void I2CSPIDriverBase::print_status()
{
bool is_i2c_bus = _bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal;
PX4_INFO("Running on %s Bus %i", is_i2c_bus ? "I2C" : "SPI", _bus);
if (is_i2c_bus) {
PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, _i2c_address);
} else {
PX4_INFO("Running on SPI Bus %i", _bus);
}
}
void I2CSPIDriverBase::request_stop_and_wait()

4
src/drivers/batt_smbus/batt_smbus.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -537,7 +537,7 @@ $ batt_smbus -X write_flash 19069 2 27 0
I2CSPIDriverBase *BATT_SMBUS::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
SMBus *interface = new SMBus(iterator.bus(), cli.i2c_address);
SMBus *interface = new SMBus(DRV_BAT_DEVTYPE_SMBUS, iterator.bus(), cli.i2c_address);
if (interface == nullptr) {
PX4_ERR("alloc failed");
return nullptr;

9
src/drivers/batt_smbus/batt_smbus.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -118,11 +118,10 @@ using namespace time_literals;
#define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT 0xcf
#define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED 0xce
enum class SMBUS_DEVICE_TYPE {
UNDEFINED = 0,
BQ40Z50 = 1,
BQ40Z80 = 2,
UNDEFINED = 0,
BQ40Z50 = 1,
BQ40Z80 = 2,
};
class BATT_SMBUS : public I2CSPIDriver<BATT_SMBUS>

1
src/drivers/drv_sensor.h
View File

@ -178,6 +178,7 @@
#define DRV_DIST_DEVTYPE_SIM 0x9a
#define DRV_DIST_DEVTYPE_SRF05 0x9b
#define DRV_DIST_DEVTYPE_GY_US42 0x9c
#define DRV_BAT_DEVTYPE_BATMON_SMBUS 0x9d
#define DRV_GPS_DEVTYPE_ASHTECH 0xA0

43
src/drivers/smart_battery/batmon/CMakeLists.txt Normal file
View File

@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__batmon
MAIN batmon
COMPILE_FLAGS
SRCS
batmon.cpp
DEPENDS
drivers__smbus_sbs
)

351
src/drivers/smart_battery/batmon/batmon.cpp Normal file
View File

@ -0,0 +1,351 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file batmon.cpp
*
* BatMon module for Smart Battery utilizing SBS 1.1 specifications
* Setup/usage information: https://rotoye.com/batmon-tutorial/
*
* @author Eohan George <eohan@rotoye.com>
* @author Nick Belanger <nbelanger@mail.skymul.com>
*/
#include "batmon.h"
#include <mathlib/mathlib.h>
extern "C" __EXPORT int batmon_main(int argc, char *argv[]);
Batmon::Batmon(I2CSPIBusOption bus_option, const int bus, SMBus *interface):
SMBUS_SBS_BaseClass(bus_option, bus, interface)
{
}
Batmon::~Batmon()
{
// Needn't change the parameter since there are other modules that may use it.
//int battsource = 0;
//param_set(param_find("BAT_SOURCE"), &battsource);
}
I2CSPIDriverBase *Batmon::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
//TODO there may be a better way to pass the driver ID, since iterator object should have it
SMBus *interface = new SMBus(DRV_BAT_DEVTYPE_BATMON_SMBUS, iterator.bus(), cli.i2c_address);
int32_t batmon_en_param = 0;
param_get(param_find("BATMON_DRIVER_EN"), &batmon_en_param);
if (batmon_en_param == 0) { // BATMON_DRIVER_EN is set to disabled. Do not start driver
return nullptr; // TODO: add option for autodetect I2C address
}
if (interface == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
Batmon *instance = new Batmon(iterator.configuredBusOption(), iterator.bus(), interface);
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
int ret = instance->get_startup_info();
ret |= instance->get_batmon_startup_info();
if (ret != PX4_OK) {
delete instance;
return nullptr;
}
// Setting the BAT_SOURCE to "external"
int32_t battsource = 1;
param_set(param_find("BAT_SOURCE"), &battsource);
instance->ScheduleOnInterval(SBS_MEASUREMENT_INTERVAL_US);
return instance;
}
void Batmon::print_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Driver for SMBUS Communication with BatMon enabled smart-battery
Setup/usage information: https://rotoye.com/batmon-tutorial/
### Examples
To start at address 0x0B, on bus 4
$ batmon start -X -a 11 -b 4
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("batmon", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0B);
PRINT_MODULE_USAGE_COMMAND_DESCR("man_info", "Prints manufacturer info.");
PRINT_MODULE_USAGE_COMMAND_DESCR("suspend", "Suspends the driver from rescheduling the cycle.");
PRINT_MODULE_USAGE_COMMAND_DESCR("resume", "Resumes the driver from suspension.");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
void Batmon::RunImpl()
{
int ret = PX4_OK;
// Temporary variable for storing SMBUS reads.
uint16_t result;
// Read data from sensor.
battery_status_s new_report = {};
new_report.id = 1;
// Set time of reading.
new_report.timestamp = hrt_absolute_time();
new_report.connected = true;
ret |= _interface->read_word(BATT_SMBUS_VOLTAGE, result);
ret |= get_cell_voltages();
for (int i = 0; i < _cell_count; i++) {
new_report.voltage_cell_v[i] = _cell_voltages[i];
}
// Convert millivolts to volts.
new_report.voltage_v = ((float)result) / 1000.0f;
new_report.voltage_filtered_v = new_report.voltage_v;
// Read current.
ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
new_report.current_filtered_a = new_report.current_a;
// Read average current.
ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
new_report.average_current_a = average_current;
// Read run time to empty (minutes).
ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
new_report.run_time_to_empty = result;
// Read average time to empty (minutes).
ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
new_report.average_time_to_empty = result;
// Read remaining capacity.
ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, result);
// Calculate total discharged amount in mah.
new_report.discharged_mah = _batt_startup_capacity - (float)result;
// Read Relative SOC.
ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);
// Normalize 0.0 to 1.0
new_report.remaining = (float)result / 100.0f;
// Read Max Error
//ret |= _interface->read_word(BATT_SMBUS_MAX_ERROR, result); //TODO: to be implemented
//new_report.max_error = result;
// Read battery temperature and covert to Celsius.
ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
new_report.temperature = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
// Only publish if no errors.
if (ret == PX4_OK) {
new_report.capacity = _batt_capacity;
new_report.cycle_count = _cycle_count;
new_report.serial_number = _serial_number;
new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
new_report.cell_count = _cell_count;
new_report.state_of_health = _state_of_health;
// TODO: This critical setting should be set with BMS info or through a paramter
// Setting a hard coded BATT_CELL_VOLTAGE_THRESHOLD_FAILED may not be appropriate
//if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
// new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
if (new_report.remaining > _low_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
} else if (new_report.remaining > _crit_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
} else if (new_report.remaining > _emergency_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else {
new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
}
new_report.interface_error = perf_event_count(_interface->_interface_errors);
int instance = 0;
orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance);
_last_report = new_report;
}
}
int Batmon::get_batmon_startup_info()
{
int ret = PX4_OK;
// Read battery threshold params on startup.
param_get(param_find("BAT_CRIT_THR"), &_crit_thr);
param_get(param_find("BAT_LOW_THR"), &_low_thr);
param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr);
// Read BatMon specific data
uint16_t num_cells;
ret = _interface->read_word(BATT_SMBUS_CELL_COUNT, num_cells);
_cell_count = math::min((uint8_t)num_cells, (uint8_t)MAX_CELL_COUNT);
int32_t _num_cells = num_cells;
param_set(param_find("BAT_N_CELLS"), &_num_cells);
return ret;
}
void Batmon::custom_method(const BusCLIArguments &cli)
{
switch(cli.custom1) {
case 1:
// TODO: analyze why these statements are not printed
PX4_INFO("The manufacturer name: %s", _manufacturer_name);
PX4_INFO("The manufacturer date: %d", _manufacture_date);
PX4_INFO("The serial number: %d", _serial_number);
break;
case 4:
suspend();
break;
case 5:
resume();
break;
}
}
int Batmon::get_cell_voltages()
{
// Temporary variable for storing SMBUS reads.
uint16_t result = 0;
uint8_t ret = 0;
// Making the assumption that the register value of BATT_SMBUS_CELL_1_VOLTAGE and BATT_SMBUS_CELL_10_VOLTAGE are sequential and decreasing order.
for (int i = 0 ; i < _cell_count; i++) {
ret |= _interface->read_word(BATT_SMBUS_CELL_1_VOLTAGE - i, result);
// Convert millivolts to volts.
_cell_voltages[i] = ((float)result) * 0.001f;
}
//Calculate max cell delta
_min_cell_voltage = _cell_voltages[0];
float max_cell_voltage = _cell_voltages[0];
for (uint8_t i = 1; (i < _cell_count && i < (sizeof(_cell_voltages) / sizeof(_cell_voltages[0]))); i++) {
_min_cell_voltage = math::min(_min_cell_voltage, _cell_voltages[i]);
max_cell_voltage = math::max(max_cell_voltage, _cell_voltages[i]);
}
// Calculate the max difference between the min and max cells with complementary filter.
_max_cell_voltage_delta = (0.5f * (max_cell_voltage - _min_cell_voltage)) +
(0.5f * _last_report.max_cell_voltage_delta);
return ret;
}
extern "C" __EXPORT int batmon_main(int argc, char *argv[])
{
using ThisDriver = Batmon;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 100000;
int32_t batmon_addr_batt1 = BATMON_DEFAULT_SMBUS_ADDR;
param_get(param_find("BATMON_ADDR_DFLT"), &batmon_addr_batt1);
cli.i2c_address = batmon_addr_batt1;
const char *verb = cli.parseDefaultArguments(argc, argv);
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BAT_DEVTYPE_BATMON_SMBUS);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "man_info")) {
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
if (!strcmp(verb, "suspend")) {
cli.custom1 = 4;
return ThisDriver::module_custom_method(cli, iterator);
}
if (!strcmp(verb, "resume")) {
cli.custom1 = 5;
return ThisDriver::module_custom_method(cli, iterator);
}
ThisDriver::print_usage();
return -1;
}

114
src/drivers/smart_battery/batmon/batmon.h Normal file
View File

@ -0,0 +1,114 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file batmon.h
*
* BatMon module for Smart Battery utilizing SBS 1.1 specifications
* Setup/usage information: https://rotoye.com/batmon-tutorial/
*
* @author Eohan George <eohan@rotoye.com>
* @author Nick Belanger <nbelanger@mail.skymul.com>
*/
#pragma once
#include <lib/drivers/smbus_sbs/SBS.hpp>
#include <px4_platform_common/module.h>
#define MAX_CELL_COUNT 16
#define BATMON_DEFAULT_SMBUS_ADDR 0x0B ///< Default 7 bit address I2C address. 8 bit = 0x16
class Batmon : public SMBUS_SBS_BaseClass<Batmon>
{
public:
Batmon(I2CSPIBusOption bus_option, const int bus, SMBus *interface);
~Batmon();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void RunImpl();
enum {
BATT_SMBUS_TEMP_EXTERNAL_1 = 0x48,
BATT_SMBUS_TEMP_EXTERNAL_2 = 0x49,
BATT_SMBUS_CELL_1_VOLTAGE = 0x3F,
BATT_SMBUS_CELL_2_VOLTAGE = 0x3E,
BATT_SMBUS_CELL_3_VOLTAGE = 0x3D,
BATT_SMBUS_CELL_4_VOLTAGE = 0x3C,
BATT_SMBUS_CELL_5_VOLTAGE = 0x3B,
BATT_SMBUS_CELL_6_VOLTAGE = 0x3A,
BATT_SMBUS_CELL_7_VOLTAGE = 0x39,
BATT_SMBUS_CELL_8_VOLTAGE = 0x38,
BATT_SMBUS_CELL_9_VOLTAGE = 0x37,
BATT_SMBUS_CELL_10_VOLTAGE = 0x36,
BATT_SMBUS_CELL_11_VOLTAGE = 0x35,
BATT_SMBUS_CELL_12_VOLTAGE = 0x34,
BATT_SMBUS_CELL_13_VOLTAGE = 0x33,
BATT_SMBUS_CELL_14_VOLTAGE = 0x32,
BATT_SMBUS_CELL_15_VOLTAGE = 0x31,
BATT_SMBUS_CELL_16_VOLTAGE = 0x30,
BATT_SMBUS_CELL_COUNT = 0x40
} BATMON_REGISTERS;
private:
float _cell_voltages[MAX_CELL_COUNT] = {};
float _max_cell_voltage_delta{0};
float _min_cell_voltage{0};
/** @param _last_report Last published report, used finding v deltas */
battery_status_s _last_report{};
void custom_method(const BusCLIArguments &cli) override;
int get_cell_voltages();
int get_batmon_startup_info();
/** @param _crit_thr Critical battery threshold param. */
float _crit_thr{0.f};
/** @param _emergency_thr Emergency battery threshold param. */
float _emergency_thr{0.f};
/** @param _low_thr Low battery threshold param. */
float _low_thr{0.f};
};

61
src/drivers/smart_battery/batmon/batmon_params.c Normal file
View File

@ -0,0 +1,61 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file batmon_params.c
* Smart battery driver parameters for BatMon (rotoye.com/batmon)
*
* @author Eohan George <eohan@rotoye.com>
*/
/**
* Parameter to enable BatMon module
*
* @reboot_required true
* @min 0
* @max 2
* @group Sensors
* @value 0 Disabled
* @value 1 Start on default I2C addr(BATMON_ADDR_DFLT)
* @value 2 Autodetect I2C address (TODO)
*/
PARAM_DEFINE_INT32(BATMON_DRIVER_EN, 0);
/**
* I2C address for BatMon battery 1
*
* @reboot_required true
* @decimal 1
* @group Sensors
*/
PARAM_DEFINE_INT32(BATMON_ADDR_DFLT, 11); //0x0B

1
src/lib/drivers/CMakeLists.txt
View File

@ -40,3 +40,4 @@ add_subdirectory(led)
add_subdirectory(magnetometer)
add_subdirectory(rangefinder)
add_subdirectory(smbus)
add_subdirectory(smbus_sbs)

4
src/lib/drivers/smbus/SMBus.cpp
View File

@ -47,8 +47,8 @@
#include "SMBus.hpp"
#include <mathlib/mathlib.h>
SMBus::SMBus(int bus_num, uint16_t address) :
I2C(DRV_BAT_DEVTYPE_SMBUS, MODULE_NAME, bus_num, address, 100000)
SMBus::SMBus(uint8_t device_id, int bus_num, uint16_t address) :
I2C(device_id, MODULE_NAME, bus_num, address, 100000)
{
}

2
src/lib/drivers/smbus/SMBus.hpp
View File

@ -50,7 +50,7 @@ class SMBus : public device::I2C
public:
static constexpr uint8_t MAX_BLOCK_LEN = 34;
SMBus(int bus_num, uint16_t address);
SMBus(uint8_t device_id, int bus_num, uint16_t address);
~SMBus() override;
/**

34
src/lib/drivers/smbus_sbs/CMakeLists.txt Normal file
View File

@ -0,0 +1,34 @@
############################################################################
#
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(drivers__smbus_sbs SBS.hpp)

330
src/lib/drivers/smbus_sbs/SBS.hpp Normal file
View File

@ -0,0 +1,330 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SBS.hpp
*
* Header for a core SBS 1.1 specification of Smart Battery
* http://sbs-forum.org/specs/sbdat110.pdf
*
* @author Eohan George <eohan@rotoye.com>
* @author Nick Belanger <nbelanger@mail.skymul.com>
*/
#pragma once
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/smbus/SMBus.hpp>
#include <uORB/topics/battery_status.h>
#include <px4_platform_common/param.h>
#include <ecl/geo/geo.h>
using namespace time_literals;
#define SBS_MEASUREMENT_INTERVAL_US 100_ms ///< time in microseconds, measure at 10Hz (not part of SBS spec)
template<class T>
class SMBUS_SBS_BaseClass : public I2CSPIDriver<T>
{
public:
SMBUS_SBS_BaseClass(I2CSPIBusOption bus_option, const int bus, SMBus *interface);
SMBUS_SBS_BaseClass();
~SMBUS_SBS_BaseClass();
static void print_usage();
friend SMBus;
int populate_smbus_data(battery_status_s &msg);
virtual void RunImpl(); // Can be overriden by derived implimentation
virtual void custom_method(const BusCLIArguments &cli) = 0; //Has be overriden by derived implimentation
/**
* @brief Returns the SBS serial number of the battery device.
* @return Returns the SBS serial number of the battery device.
*/
uint16_t get_serial_number();
/**
* @brief Read info from battery on startup.
* @return Returns PX4_OK on success, PX4_ERROR on failure. Can be overriden by derived implimentation
*/
virtual int get_startup_info();
/**
* @brief Gets the SBS manufacture date of the battery.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacture_date();
/**
* @brief Gets the SBS manufacturer name of the battery device.
* @param manufacturer_name Pointer to a buffer into which the manufacturer name is to be written.
* @param max_length The maximum number of bytes to attempt to read from the manufacturer name register,
* including the null character that is appended to the end.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacturer_name(uint8_t *manufacturer_name, const uint8_t length);
/**
* @brief Enables or disables the cell under voltage protection emergency shut off.
*/
void suspend();
void resume();
enum {
BATT_SMBUS_TEMP = 0x08, ///< temperature register
BATT_SMBUS_VOLTAGE = 0x09, ///< voltage register
BATT_SMBUS_CURRENT = 0x0A, ///< current register
BATT_SMBUS_AVERAGE_CURRENT = 0x0B, ///< average current register
BATT_SMBUS_MAX_ERROR = 0x0C, ///< max error
BATT_SMBUS_RELATIVE_SOC = 0x0D, ///< Relative State Of Charge
BATT_SMBUS_ABSOLUTE_SOC = 0x0E, ///< Absolute State of charge
BATT_SMBUS_REMAINING_CAPACITY = 0x0F, ///< predicted remaining battery capacity as a percentage
BATT_SMBUS_FULL_CHARGE_CAPACITY = 0x10, ///< capacity when fully charged
BATT_SMBUS_RUN_TIME_TO_EMPTY = 0x11, ///< predicted remaining battery capacity based on the present rate of discharge in min
BATT_SMBUS_AVERAGE_TIME_TO_EMPTY = 0x12, ///< predicted remaining battery capacity based on the present rate of discharge in min
BATT_SMBUS_CYCLE_COUNT = 0x17, ///< number of cycles the battery has experienced
BATT_SMBUS_DESIGN_CAPACITY = 0x18, ///< design capacity register
BATT_SMBUS_DESIGN_VOLTAGE = 0x19, ///< design voltage register
BATT_SMBUS_MANUFACTURER_NAME = 0x20, ///< manufacturer name
BATT_SMBUS_MANUFACTURER_NAME_SIZE = 21, ///< manufacturer name data size (not part of SBS spec)
BATT_SMBUS_DEVICE_NAME = 0x21, ///< character string that contains the battery's name
BATT_SMBUS_MANUFACTURE_DATE = 0x1B, ///< manufacture date register
BATT_SMBUS_SERIAL_NUMBER = 0x1C, ///< serial number register
BATT_SMBUS_MANUFACTURER_ACCESS = 0x00,
BATT_SMBUS_MANUFACTURER_DATA = 0x23
} SBS_REGISTERS;
protected:
SMBus *_interface;
perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batmon_cycle")}; // TODO
/** @param _batt_topic uORB battery topic. */
orb_advert_t _batt_topic{nullptr};
/** @param _cell_count Number of series cell (retrieved from cell_count PX4 params) */
uint8_t _cell_count{0};
/** @param _batt_capacity Battery design capacity in mAh (0 means unknown). */
uint16_t _batt_capacity{0};
/** @param _batt_startup_capacity Battery remaining capacity in mAh on startup. */
uint16_t _batt_startup_capacity{0};
/** @param _cycle_count The number of cycles the battery has experienced. */
uint16_t _cycle_count{0};
/** @param _serial_number Serial number register. */
uint16_t _serial_number{0};
/** @param _manufacturer_name Name of the battery manufacturer. */
char _manufacturer_name[BATT_SMBUS_MANUFACTURER_NAME_SIZE + 1] {}; // Plus one for terminator
/** @param _manufacture_date Date of the battery manufacturing. */
uint16_t _manufacture_date{0};
/** @param _state_of_health state of health as read on connection */
float _state_of_health{0.f}; // Not part of SBS, can be calculated from FullChargeCapacity
// and DesignCapacity
void print_man_info();
};
template<class T>
SMBUS_SBS_BaseClass<T>::SMBUS_SBS_BaseClass(I2CSPIBusOption bus_option, const int bus, SMBus *interface):
I2CSPIDriver<T>(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus,
interface->get_device_address()),
_interface(interface)
{
static int SBS_instance_number = 0;
battery_status_s new_report = {};
SBS_instance_number++;
// TODO: there maybe a better way to assign instance number. This would
// result in continuously increasing instance number and possible overflow
_batt_topic = orb_advertise_multi(ORB_ID(battery_status), &new_report, &SBS_instance_number);
_interface->init();
}
template<class T>
SMBUS_SBS_BaseClass<T>::~SMBUS_SBS_BaseClass()
{
orb_unadvertise(_batt_topic);
perf_free(_cycle); // TODO
if (_interface != nullptr) {
delete _interface;
}
}
template<class T>
void SMBUS_SBS_BaseClass<T>::suspend()
{
this->ScheduleClear();
}
template<class T>
void SMBUS_SBS_BaseClass<T>::resume()
{
this->ScheduleOnInterval(SBS_MEASUREMENT_INTERVAL_US);
}
template<class T>
uint16_t SMBUS_SBS_BaseClass<T>::get_serial_number()
{
uint16_t serial_num = 0;
if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num) == PX4_OK) {
return serial_num;
}
return PX4_ERROR;
}
template<class T>
int SMBUS_SBS_BaseClass<T>::get_startup_info()
{
int ret = PX4_OK;
ret |= _interface->block_read(BATT_SMBUS_MANUFACTURER_NAME, _manufacturer_name, BATT_SMBUS_MANUFACTURER_NAME_SIZE,
true);
_manufacturer_name[sizeof(_manufacturer_name) - 1] = '\0';
uint16_t serial_num;
ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num);
uint16_t remaining_cap;
ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap);
uint16_t cycle_count;
ret |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count);
uint16_t full_cap;
ret |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap);
uint16_t manufacture_date;
ret |= _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, manufacture_date);
if (!ret) {
_serial_number = serial_num;
_batt_startup_capacity = (uint16_t)((float)remaining_cap);
_cycle_count = cycle_count;
_batt_capacity = (uint16_t)((float)full_cap);
_manufacture_date = manufacture_date;
}
return ret;
}
template<class T>
int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
{
// Temporary variable for storing SMBUS reads.
uint16_t result;
int ret = _interface->read_word(BATT_SMBUS_VOLTAGE, result);
// Convert millivolts to volts.
data.voltage_v = ((float)result) * 0.001f;
data.voltage_filtered_v = data.voltage_v;
// Read current.
ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
data.current_a = (-1.0f * ((float)(*(int16_t *)&result)) * 0.001f);
data.current_filtered_a = data.current_a;
// Read remaining capacity.
ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);
data.remaining = (float)result * 0.01f;
// Read remaining capacity.
ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, result);
data.discharged_mah = _batt_startup_capacity - result;
// Read full capacity.
ret |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, result);
data.capacity = result;
// Read cycle count.
ret |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, result);
data.cycle_count = result;
// Read serial number.
ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, result);
data.serial_number = result;
// Read battery temperature and covert to Celsius.
ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
data.temperature = ((float)result * 0.1f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
return ret;
}
template<class T>
void SMBUS_SBS_BaseClass<T>::RunImpl()
{
// Get the current time.
uint64_t now = hrt_absolute_time();
// Read data from sensor.
battery_status_s new_report = {};
new_report.id = 1;
// Set time of reading.
new_report.timestamp = now;
new_report.connected = true;
int ret = populate_smbus_data(new_report);
new_report.cell_count = _cell_count;
// Only publish if no errors.
if (!ret) {
orb_publish(ORB_ID(battery_status), _batt_topic, &new_report);
}
}