From a894b93762e1c8247538549ed6114168e6f6378a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 12 Jan 2022 23:03:24 +1100 Subject: [PATCH] AP_BattMonitor: make C_TO_KELVIN a function macro; create KELVIN_TO_C These are in celsius --- libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp | 2 +- libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp | 2 +- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp index 14608ced05..12317ed2cd 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp @@ -117,7 +117,7 @@ void AP_BattMonitor_SMBus::read_temp(void) _has_temperature = true; _state.temperature_time = AP_HAL::millis(); - _state.temperature = (data * 0.1f) - C_TO_KELVIN; + _state.temperature = KELVIN_TO_C(0.1f * data); } // reads the serial number if it's not already known diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp index d649346b49..3ab3dc1e03 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Rotoye.cpp @@ -17,6 +17,6 @@ void AP_BattMonitor_SMBus_Rotoye::read_temp(void) { 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; + _state.temperature = KELVIN_TO_C(0.1f * (float)t); } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index f8d8c7190e..87530c8a6e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -130,7 +130,7 @@ void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const floa if (!isnanf(temperature_K) && temperature_K > 0) { // Temperature reported from battery in kelvin and stored internally in Celsius. - _interim_state.temperature = temperature_K - C_TO_KELVIN; + _interim_state.temperature = KELVIN_TO_C(temperature_K); _interim_state.temperature_time = AP_HAL::millis(); } @@ -185,7 +185,7 @@ void AP_BattMonitor_UAVCAN::handle_mppt_stream(const MpptStreamCb &cb) const uint8_t soc = 127; // convert C to Kelvin - const float temperature_K = isnanf(cb.msg->temperature) ? 0 : cb.msg->temperature + C_TO_KELVIN; + const float temperature_K = isnanf(cb.msg->temperature) ? 0 : C_TO_KELVIN(cb.msg->temperature); update_interim_state(voltage, current, temperature_K, soc);