diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index a92493f583..105b0c9b53 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -16,6 +16,7 @@ #include "AP_BattMonitor_MPPT_PacketDigital.h" #include "AP_BattMonitor_INA231.h" #include "AP_BattMonitor_LTC2946.h" +#include "AP_BattMonitor_Torqeedo.h" #include @@ -306,6 +307,11 @@ AP_BattMonitor::init() case Type::LTC2946: drivers[instance] = new AP_BattMonitor_LTC2946(*this, state[instance], _params[instance]); break; +#endif +#if HAL_TORQEEDO_ENABLED + case Type::Torqeedo: + drivers[instance] = new AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]); + break; #endif case Type::NONE: default: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 775fabffa6..a8134d8891 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -48,6 +48,7 @@ class AP_BattMonitor_Generator; class AP_BattMonitor_MPPT_PacketDigital; class AP_BattMonitor_INA231; class AP_BattMonitor_LTC2946; +class AP_BattMonitor_Torqeedo; class AP_BattMonitor { @@ -67,6 +68,8 @@ class AP_BattMonitor friend class AP_BattMonitor_INA231; friend class AP_BattMonitor_LTC2946; + friend class AP_BattMonitor_Torqeedo; + public: // battery failsafes must be defined in levels of severity so that vehicles wont fall backwards @@ -99,6 +102,7 @@ public: MPPT_PacketDigital = 20, INA231 = 21, LTC2946 = 22, + Torqeedo = 23, }; 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 25325bc906..250ce84a94 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:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA231,22:LTC2946 + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA231,22:LTC2946,23:Torqeedo // @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_Torqeedo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.cpp new file mode 100644 index 0000000000..7f15852cb6 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.cpp @@ -0,0 +1,77 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + + +#include +#include "AP_BattMonitor_Torqeedo.h" + +#if HAL_TORQEEDO_ENABLED + +#define AP_BATTMON_TORQEEDO_TIMEOUT_US 5000000 + +extern const AP_HAL::HAL &hal; + +void AP_BattMonitor_Torqeedo::read(void) +{ + AP_Torqeedo *torqeedo = AP_Torqeedo::get_singleton(); + if (torqeedo == nullptr) { + _state.healthy = false; + return; + } + + // get voltage, current, temp and remaining capacity percentage + float volts; + float current_amps; + float temp_C; + if (torqeedo->get_batt_info(volts, current_amps, temp_C, remaining_pct)) { + have_info = true; + _state.voltage = volts; + _state.current_amps = current_amps; + _state.temperature = temp_C; + _state.temperature_time = AP_HAL::millis(); + + // update total current draw + const uint32_t tnow_us = AP_HAL::micros(); + const uint32_t diff_us = tnow_us - _state.last_time_micros; + if (diff_us < AP_BATTMON_TORQEEDO_TIMEOUT_US) { + _state.consumed_mah += _state.current_amps * diff_us / 1000000.0 / 3600.0 * 1000.0; + } + _state.last_time_micros = tnow_us; + _state.healthy = true; + } + + // read battery pack capacity + if (!have_capacity) { + uint16_t batt_capacity_ah; + if (torqeedo->get_batt_capacity_Ah(batt_capacity_ah)) { + have_capacity = true; + if (batt_capacity_ah * 1000 != _params._pack_capacity) { + _params._pack_capacity.set_and_notify(batt_capacity_ah * 1000); + } + } + } +} + +// capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument +// returns false if the battery is unhealthy, does not have current monitoring, or the pack_capacity is too small +bool AP_BattMonitor_Torqeedo::capacity_remaining_pct(uint8_t &percentage) const +{ + if (have_info) { + percentage = remaining_pct; + } + return have_info; +} + +#endif // HAL_TORQEEDO_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.h b/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.h new file mode 100644 index 0000000000..9ecc1affe1 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.h @@ -0,0 +1,52 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include +#include +#include +#include "AP_BattMonitor_Backend.h" + +#if HAL_TORQEEDO_ENABLED + +class AP_BattMonitor_Torqeedo: public AP_BattMonitor_Backend +{ +public: + // constructor. This incorporates initialisation as well. + AP_BattMonitor_Torqeedo(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms): + AP_BattMonitor_Backend(mon, mon_state, params) + {}; + + // read the latest battery voltage + void read() override; + + /// returns true if battery monitor instance provides current info + bool has_current() const override { return have_info; }; + + // returns true if battery monitor provides temperature + bool has_temperature() const override { return have_info; }; + + // capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument + // returns false if the battery is unhealthy, does not have current monitoring, or the pack_capacity is too small + bool capacity_remaining_pct(uint8_t &percentage) const override WARN_IF_UNUSED; + +private: + + bool have_info; // true if torqeedo has provided battery info at least once + bool have_capacity; // true once torqeedo has provided battery capacity + uint8_t remaining_pct; // battery remaining capacity as a percentage +}; + +#endif