|
|
|
@ -81,8 +81,9 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
@@ -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; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|