diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.cpp index 424264644e..dfb7921abe 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.cpp @@ -49,7 +49,7 @@ void AP_BattMonitor_Torqeedo::read(void) 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.consumed_mah += _state.current_amps * diff_us * 1e-6f / 3600.0 * 1000.0; } _state.last_time_micros = tnow_us; _state.healthy = true; @@ -77,4 +77,3 @@ bool AP_BattMonitor_Torqeedo::capacity_remaining_pct(uint8_t &percentage) const } #endif // AP_BATTERY_TORQEEDO_ENABLED -