diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 0c1739e96d..7f3204ab1e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -4,6 +4,7 @@ #include "AP_BattMonitor_SMBus_Solo.h" #include "AP_BattMonitor_SMBus_Generic.h" #include "AP_BattMonitor_SMBus_Maxell.h" +#include "AP_BattMonitor_SMBus_Rotoye.h" #include "AP_BattMonitor_Bebop.h" #include "AP_BattMonitor_BLHeliESC.h" #include "AP_BattMonitor_SMBus_SUI.h" @@ -149,6 +150,11 @@ AP_BattMonitor::init() hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR, 100000, true, 20)); break; + case Type::Rotoye: + drivers[instance] = new AP_BattMonitor_SMBus_Rotoye(*this, state[instance], _params[instance], + hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR, + 100000, true, 20)); + break; #endif // HAL_BATTMON_SMBUS_ENABLE case Type::BEBOP: #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index eb289db25a..1e5661065f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -36,6 +36,7 @@ class AP_BattMonitor_SMBus; class AP_BattMonitor_SMBus_Solo; class AP_BattMonitor_SMBus_Generic; class AP_BattMonitor_SMBus_Maxell; +class AP_BattMonitor_SMBus_Rotoye; class AP_BattMonitor_UAVCAN; class AP_BattMonitor_Generator; @@ -47,6 +48,7 @@ class AP_BattMonitor friend class AP_BattMonitor_SMBus_Solo; friend class AP_BattMonitor_SMBus_Generic; friend class AP_BattMonitor_SMBus_Maxell; + friend class AP_BattMonitor_SMBus_Rotoye; friend class AP_BattMonitor_UAVCAN; friend class AP_BattMonitor_Sum; friend class AP_BattMonitor_FuelFlow; @@ -80,6 +82,7 @@ public: NeoDesign = 15, MAXELL = 16, Generator = 17, + Rotoye = 19, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 5dafc86f4a..930f11119b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -13,7 +13,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: MONITOR // @DisplayName: Battery monitoring // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator,19:Rotoye // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index 26bc4c9722..a077052d9d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -70,7 +70,7 @@ protected: // reads the temperature word from the battery // returns true if the read was successful - bool read_temp(void); + virtual bool read_temp(void); // reads the serial number if it's not already known // returns true if the read was successful, or the number was already known diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp new file mode 100644 index 0000000000..25e5b5d393 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp @@ -0,0 +1,25 @@ +#include "AP_BattMonitor_SMBus_Rotoye.h" + +#include + +// Specific to Rotoye Batmon +#define BATTMONITOR_SMBUS_TEMP_EXT 0x07 + +// return the maximum of the internal and external temperature sensors +bool AP_BattMonitor_SMBus_Rotoye::read_temp(void) { + + uint16_t t_int, t_ext; + + /* Both internal and external values will always be sent by Batmon. + If no external thermistor is used, a zero-value is sent. */ + if (read_word(BATTMONITOR_SMBUS_TEMP, t_int) && + read_word(BATTMONITOR_SMBUS_TEMP_EXT, t_ext)) { + uint16_t t; + _state.temperature_time = AP_HAL::millis(); + t = ((t_ext > t_int) ? t_ext : t_int); + _state.temperature = 0.1f * (float)t - C_TO_KELVIN; + return true; + } + + return false; +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.h new file mode 100644 index 0000000000..126b558a83 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.h @@ -0,0 +1,14 @@ +#pragma once + +#include "AP_BattMonitor_SMBus_Generic.h" + +class AP_BattMonitor_SMBus_Rotoye : public AP_BattMonitor_SMBus_Generic +{ + using AP_BattMonitor_SMBus_Generic::AP_BattMonitor_SMBus_Generic; + +private: + + // Rotoye Batmon has two temperature readings + bool read_temp(void) override; + +}; \ No newline at end of file