From 6bc09138c13dbde8927655eb9b7e7b1782377073 Mon Sep 17 00:00:00 2001 From: echoG <518314+echoGee@users.noreply.github.com> Date: Wed, 9 Jun 2021 12:17:45 -0400 Subject: [PATCH] Adding BatMon smart battery as a module and refactoring SMBUS based SBS 1.1 spec to a library --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 + boards/airmind/mindpx-v2/default.cmake | 1 + boards/atlflight/eagle/default.cmake | 1 + boards/atlflight/excelsior/default.cmake | 1 + boards/av/x-v1/default.cmake | 1 + boards/beaglebone/blue/default.cmake | 1 + boards/cuav/nora/default.cmake | 1 + boards/cuav/x7pro/default.cmake | 1 + boards/cubepilot/cubeorange/default.cmake | 1 + boards/cubepilot/cubeorange/test.cmake | 1 + boards/cubepilot/cubeyellow/default.cmake | 1 + boards/cubepilot/cubeyellow/test.cmake | 1 + boards/emlid/navio2/default.cmake | 1 + boards/holybro/durandal-v1/default.cmake | 1 + boards/holybro/pix32v5/default.cmake | 1 + boards/modalai/fc-v1/default.cmake | 1 + boards/mro/ctrl-zero-f7-oem/default.cmake | 1 + boards/mro/ctrl-zero-f7/default.cmake | 1 + boards/mro/ctrl-zero-h7-oem/default.cmake | 2 + boards/mro/ctrl-zero-h7/default.cmake | 2 + boards/mro/pixracerpro/default.cmake | 1 + boards/mro/x21-777/default.cmake | 1 + boards/mro/x21/default.cmake | 1 + boards/nxp/fmuk66-e/default.cmake | 1 + boards/nxp/fmuk66-e/rtps.cmake | 1 + boards/nxp/fmuk66-e/socketcan.cmake | 1 + boards/nxp/fmuk66-v3/default.cmake | 1 + boards/nxp/fmuk66-v3/rtps.cmake | 1 + boards/nxp/fmuk66-v3/socketcan.cmake | 1 + boards/nxp/fmurt1062-v1/default.cmake | 1 + boards/px4/fmu-v2/rover.cmake | 1 + boards/px4/fmu-v3/default.cmake | 1 + boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4/test.cmake | 2 + boards/px4/fmu-v4pro/default.cmake | 1 + boards/px4/fmu-v5/ctrlalloc.cmake | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/fixedwing.cmake | 1 + boards/px4/fmu-v5/multicopter.cmake | 1 + boards/px4/fmu-v5/optimized.cmake | 1 + boards/px4/fmu-v5/rover.cmake | 1 + boards/px4/fmu-v5/rtps.cmake | 1 + boards/px4/fmu-v5/stackcheck.cmake | 1 + boards/px4/fmu-v5/uavcanv0periph.cmake | 1 + boards/px4/fmu-v5/uavcanv1.cmake | 1 + boards/px4/fmu-v5x/base_phy_DP83848C.cmake | 1 + boards/px4/fmu-v5x/default.cmake | 1 + boards/px4/fmu-v6u/default.cmake | 1 + boards/px4/fmu-v6x/default.cmake | 1 + boards/px4/raspberrypi/default.cmake | 1 + boards/scumaker/pilotpi/arm64.cmake | 1 + boards/scumaker/pilotpi/default.cmake | 1 + boards/spracing/h7extreme/default.cmake | 1 + platforms/common/i2c_spi_buses.cpp | 8 +- src/drivers/batt_smbus/batt_smbus.cpp | 4 +- src/drivers/batt_smbus/batt_smbus.h | 9 +- src/drivers/drv_sensor.h | 1 + .../smart_battery/batmon/CMakeLists.txt | 43 +++ src/drivers/smart_battery/batmon/batmon.cpp | 351 ++++++++++++++++++ src/drivers/smart_battery/batmon/batmon.h | 114 ++++++ .../smart_battery/batmon/batmon_params.c | 61 +++ src/lib/drivers/CMakeLists.txt | 1 + src/lib/drivers/smbus/SMBus.cpp | 4 +- src/lib/drivers/smbus/SMBus.hpp | 2 +- src/lib/drivers/smbus_sbs/CMakeLists.txt | 34 ++ src/lib/drivers/smbus_sbs/SBS.hpp | 330 ++++++++++++++++ 66 files changed, 1012 insertions(+), 11 deletions(-) create mode 100644 src/drivers/smart_battery/batmon/CMakeLists.txt create mode 100644 src/drivers/smart_battery/batmon/batmon.cpp create mode 100644 src/drivers/smart_battery/batmon/batmon.h create mode 100644 src/drivers/smart_battery/batmon/batmon_params.c create mode 100644 src/lib/drivers/smbus_sbs/CMakeLists.txt create mode 100644 src/lib/drivers/smbus_sbs/SBS.hpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5c1838c1eb..ace0571afc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -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 diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 3ccd60f3e6..0b4b5fa835 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -46,6 +46,7 @@ px4_add_board( rc_input roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index be0111bfc3..92f5c80452 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -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 diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 3b6db7e5fb..530b983f9f 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -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 diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 03d5f9ef93..88c43e9cb1 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -45,6 +45,7 @@ px4_add_board( rc_input roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers #tone_alarm uavcan diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index be1f8976a5..a98fcd3120 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -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 diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake index cdeb629762..034eb66833 100644 --- a/boards/cuav/nora/default.cmake +++ b/boards/cuav/nora/default.cmake @@ -52,6 +52,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index 3306df91ce..740cab5a48 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -53,6 +53,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index 51c6a128d9..8ed42b3618 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -49,6 +49,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/cubepilot/cubeorange/test.cmake b/boards/cubepilot/cubeorange/test.cmake index 23cad23a4b..3651658625 100644 --- a/boards/cubepilot/cubeorange/test.cmake +++ b/boards/cubepilot/cubeorange/test.cmake @@ -49,6 +49,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers test_ppm tone_alarm diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index dbd1851c3c..658bf21ff1 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -47,6 +47,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/cubepilot/cubeyellow/test.cmake b/boards/cubepilot/cubeyellow/test.cmake index 8a06a367b4..2846f952f5 100644 --- a/boards/cubepilot/cubeyellow/test.cmake +++ b/boards/cubepilot/cubeyellow/test.cmake @@ -47,6 +47,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers test_ppm tone_alarm diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index f8db8a75a5..865b3cd61a 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -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 diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index e9666a2b76..b0590e0db6 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -47,6 +47,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 98c681422b..0e7f1b49ec 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -51,6 +51,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index cfed7048c7..7a21df918b 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -44,6 +44,7 @@ px4_add_board( rc_input roboclaw rpm + smart_battery/batmon safety_button telemetry # all available telemetry drivers #tone_alarm diff --git a/boards/mro/ctrl-zero-f7-oem/default.cmake b/boards/mro/ctrl-zero-f7-oem/default.cmake index 3abfbb2f11..7047ef0574 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.cmake +++ b/boards/mro/ctrl-zero-f7-oem/default.cmake @@ -48,6 +48,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 2e6f081af5..dce6756bee 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -48,6 +48,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/mro/ctrl-zero-h7-oem/default.cmake b/boards/mro/ctrl-zero-h7-oem/default.cmake index 9a5903c138..7620b988e0 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.cmake +++ b/boards/mro/ctrl-zero-h7-oem/default.cmake @@ -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 diff --git a/boards/mro/ctrl-zero-h7/default.cmake b/boards/mro/ctrl-zero-h7/default.cmake index f586b98422..e0fe51bbf1 100644 --- a/boards/mro/ctrl-zero-h7/default.cmake +++ b/boards/mro/ctrl-zero-h7/default.cmake @@ -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 diff --git a/boards/mro/pixracerpro/default.cmake b/boards/mro/pixracerpro/default.cmake index 7c3a07404b..8a3fd6f4a7 100644 --- a/boards/mro/pixracerpro/default.cmake +++ b/boards/mro/pixracerpro/default.cmake @@ -47,6 +47,7 @@ px4_add_board( rc_input roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 4d619071bd..08f530c9e3 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -48,6 +48,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index f776ba56ec..d7209deb48 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -49,6 +49,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake index 53b0a29631..e88d27617a 100644 --- a/boards/nxp/fmuk66-e/default.cmake +++ b/boards/nxp/fmuk66-e/default.cmake @@ -45,6 +45,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/nxp/fmuk66-e/rtps.cmake b/boards/nxp/fmuk66-e/rtps.cmake index 325c939082..5aa0d3ed0b 100644 --- a/boards/nxp/fmuk66-e/rtps.cmake +++ b/boards/nxp/fmuk66-e/rtps.cmake @@ -45,6 +45,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake index 12e45c1e82..70a912f41a 100644 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ b/boards/nxp/fmuk66-e/socketcan.cmake @@ -44,6 +44,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan_v1 diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 7a3c307f85..35c9d53869 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -45,6 +45,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake index 934e0615b3..1decc7e645 100644 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ b/boards/nxp/fmuk66-v3/rtps.cmake @@ -45,6 +45,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index 7e5d131f72..f0aec2f77e 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -45,6 +45,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan_v1 diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index 51048b6569..6e0ee9eb92 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -46,6 +46,7 @@ px4_add_board( #roboclaw #rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm #uavcan diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index 1955913e26..c6332b80ec 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -34,6 +34,7 @@ px4_add_board( optical_flow/px4flow pwm_out px4io + smart_battery/batmon tone_alarm MODULES diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index f72a5b29c6..30c49f1839 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -50,6 +50,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index e420738e70..5dc2f8171c 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -48,6 +48,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v4/test.cmake b/boards/px4/fmu-v4/test.cmake index f87dfed600..e9748d9bc8 100644 --- a/boards/px4/fmu-v4/test.cmake +++ b/boards/px4/fmu-v4/test.cmake @@ -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 diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 218fad1cbd..08a20f747c 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -49,6 +49,7 @@ px4_add_board( px4io roboclaw rpm + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index 859f8d4bab..6495dcc1ec 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -50,6 +50,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index fd38e3ddac..9a9bebf07f 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -49,6 +49,7 @@ px4_add_board( px4io rc_input roboclaw + smart_battery/batmon rpm safety_button telemetry # all available telemetry drivers diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 4ae506a289..31d7b286d9 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -47,6 +47,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index f2bfcc5ad1..230c5a3c8a 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -48,6 +48,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index 10d69ad912..2078eefb3a 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -56,6 +56,7 @@ px4_add_board( #roboclaw #rpm safety_button + smart_battery/batmon #telemetry # all available telemetry drivers test_ppm tone_alarm diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 9dde786ed0..0a4c93571b 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -40,6 +40,7 @@ px4_add_board( rc_input roboclaw safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index be6ef4eb90..ba8015f543 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -48,6 +48,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 0880c905bd..39fa72da46 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -53,6 +53,7 @@ px4_add_board( #roboclaw #rpm safety_button + #smart_battery/batmon telemetry # all available telemetry drivers #test_ppm tone_alarm diff --git a/boards/px4/fmu-v5/uavcanv0periph.cmake b/boards/px4/fmu-v5/uavcanv0periph.cmake index dbca72ce3c..3e6897702f 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.cmake +++ b/boards/px4/fmu-v5/uavcanv0periph.cmake @@ -54,6 +54,7 @@ px4_add_board( #roboclaw #rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index a21c434755..be00ab3ed9 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -50,6 +50,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm #uavcan # legacy v0 diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index 49d71826c8..584c0a0b0d 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -51,6 +51,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 98579eb1f7..ec34a4fdbd 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -52,6 +52,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index 4550ba9256..4ceb5ad77f 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -47,6 +47,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 16391f234c..2f5aebf267 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -52,6 +52,7 @@ px4_add_board( roboclaw rpm safety_button + smart_battery/batmon telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 1523199dec..c1407d649f 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -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 diff --git a/boards/scumaker/pilotpi/arm64.cmake b/boards/scumaker/pilotpi/arm64.cmake index c4e74856ee..408e5730c6 100644 --- a/boards/scumaker/pilotpi/arm64.cmake +++ b/boards/scumaker/pilotpi/arm64.cmake @@ -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 diff --git a/boards/scumaker/pilotpi/default.cmake b/boards/scumaker/pilotpi/default.cmake index de0486cf20..0851f9c653 100644 --- a/boards/scumaker/pilotpi/default.cmake +++ b/boards/scumaker/pilotpi/default.cmake @@ -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 diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index 30ff1d28e4..250247064e 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -43,6 +43,7 @@ px4_add_board( pwm_out #roboclaw rc_input + #smart_battery/batmon telemetry # all available telemetry drivers tone_alarm MODULES diff --git a/platforms/common/i2c_spi_buses.cpp b/platforms/common/i2c_spi_buses.cpp index e483ced62e..189efaa14d 100644 --- a/platforms/common/i2c_spi_buses.cpp +++ b/platforms/common/i2c_spi_buses.cpp @@ -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() diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 8952a2c638..dca69b2060 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -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; diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index 1babc9c47f..a707e14f32 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -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 diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 09258b3933..7f6833741f 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 diff --git a/src/drivers/smart_battery/batmon/CMakeLists.txt b/src/drivers/smart_battery/batmon/CMakeLists.txt new file mode 100644 index 0000000000..384a60d8aa --- /dev/null +++ b/src/drivers/smart_battery/batmon/CMakeLists.txt @@ -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 + ) diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp new file mode 100644 index 0000000000..4a92565a3d --- /dev/null +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -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 + * @author Nick Belanger + */ + +#include "batmon.h" +#include + +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; +} diff --git a/src/drivers/smart_battery/batmon/batmon.h b/src/drivers/smart_battery/batmon/batmon.h new file mode 100644 index 0000000000..1700e28a84 --- /dev/null +++ b/src/drivers/smart_battery/batmon/batmon.h @@ -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 + * @author Nick Belanger + */ + +#pragma once + +#include +#include + +#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 +{ + +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}; + +}; diff --git a/src/drivers/smart_battery/batmon/batmon_params.c b/src/drivers/smart_battery/batmon/batmon_params.c new file mode 100644 index 0000000000..66a2bd28fc --- /dev/null +++ b/src/drivers/smart_battery/batmon/batmon_params.c @@ -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 + */ + +/** + * 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 diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index b74962c5b9..b4c5b637ab 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -40,3 +40,4 @@ add_subdirectory(led) add_subdirectory(magnetometer) add_subdirectory(rangefinder) add_subdirectory(smbus) +add_subdirectory(smbus_sbs) diff --git a/src/lib/drivers/smbus/SMBus.cpp b/src/lib/drivers/smbus/SMBus.cpp index 3a5b62c161..cabde93c55 100644 --- a/src/lib/drivers/smbus/SMBus.cpp +++ b/src/lib/drivers/smbus/SMBus.cpp @@ -47,8 +47,8 @@ #include "SMBus.hpp" #include -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) { } diff --git a/src/lib/drivers/smbus/SMBus.hpp b/src/lib/drivers/smbus/SMBus.hpp index 949103eadf..7b1e158052 100644 --- a/src/lib/drivers/smbus/SMBus.hpp +++ b/src/lib/drivers/smbus/SMBus.hpp @@ -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; /** diff --git a/src/lib/drivers/smbus_sbs/CMakeLists.txt b/src/lib/drivers/smbus_sbs/CMakeLists.txt new file mode 100644 index 0000000000..f865429ede --- /dev/null +++ b/src/lib/drivers/smbus_sbs/CMakeLists.txt @@ -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) diff --git a/src/lib/drivers/smbus_sbs/SBS.hpp b/src/lib/drivers/smbus_sbs/SBS.hpp new file mode 100644 index 0000000000..3810bf3b1a --- /dev/null +++ b/src/lib/drivers/smbus_sbs/SBS.hpp @@ -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 + * @author Nick Belanger + */ + +#pragma once + +#include +#include +#include +#include +#include + + +using namespace time_literals; + +#define SBS_MEASUREMENT_INTERVAL_US 100_ms ///< time in microseconds, measure at 10Hz (not part of SBS spec) + +template +class SMBUS_SBS_BaseClass : public I2CSPIDriver +{ +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 +SMBUS_SBS_BaseClass::SMBUS_SBS_BaseClass(I2CSPIBusOption bus_option, const int bus, SMBus *interface): + I2CSPIDriver(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 +SMBUS_SBS_BaseClass::~SMBUS_SBS_BaseClass() +{ + orb_unadvertise(_batt_topic); + perf_free(_cycle); // TODO + + if (_interface != nullptr) { + delete _interface; + } + +} + +template +void SMBUS_SBS_BaseClass::suspend() +{ + this->ScheduleClear(); +} + +template +void SMBUS_SBS_BaseClass::resume() +{ + this->ScheduleOnInterval(SBS_MEASUREMENT_INTERVAL_US); +} + +template +uint16_t SMBUS_SBS_BaseClass::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 +int SMBUS_SBS_BaseClass::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 +int SMBUS_SBS_BaseClass::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 +void SMBUS_SBS_BaseClass::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); + } +}