AP_Compass: Refactor battery current backend

This commit is contained in:
Michael du Breuil 2019-07-07 14:36:39 +00:00 committed by WickedShell
parent b07d65c1f3
commit 58dbbd6d0e
1 changed files with 3 additions and 2 deletions

View File

@ -81,8 +81,9 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
state.motor_offset = mot * _compass._thr;
} else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
AP_BattMonitor &battery = AP::battery();
if (battery.has_current()) {
state.motor_offset = mot * battery.current_amps();
float current;
if (battery.current_amps(current)) {
state.motor_offset = mot * current;
}
}