diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index ce5e08de6f..846e3ef53a 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -449,7 +449,6 @@ Compass::Compass(void) : _compass_count(0), _board_orientation(ROTATION_NONE), _null_init_done(false), - _thr_or_curr(0.0f), _hil_mode(false) { AP_Param::setup_object_defaults(this, var_info); @@ -1192,7 +1191,7 @@ void Compass::motor_compensation_type(const uint8_t comp_type) { if (_motor_comp_type <= AP_COMPASS_MOT_COMP_CURRENT && _motor_comp_type != (int8_t)comp_type) { _motor_comp_type = (int8_t)comp_type; - _thr_or_curr = 0; // set current current or throttle to zero + _thr = 0; // set current throttle to zero for (uint8_t i=0; i #include #include +#include #include "CompassCalibrator.h" #include "AP_Compass_Backend.h" @@ -247,15 +248,7 @@ public: /// @param thr_pct throttle expressed as a percentage from 0 to 1.0 void set_throttle(float thr_pct) { if (_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { - _thr_or_curr = thr_pct; - } - } - - /// Set the current used by system in amps - /// @param amps current flowing to the motors expressed in amps - void set_current(float amps) { - if (_motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) { - _thr_or_curr = amps; + _thr = thr_pct; } } @@ -415,8 +408,8 @@ private: // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current AP_Int8 _motor_comp_type; - // throttle expressed as a percentage from 0 ~ 1.0 or current expressed in amps - float _thr_or_curr; + // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation + float _thr; struct mag_state { AP_Int8 external; diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 6f5ecac6d6..76ab44c3b1 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -72,9 +72,13 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) if (_compass._per_motor.enabled() && i == 0) { // per-motor correction is only valid for first compass _compass._per_motor.compensate(state.motor_offset); - } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE || - _compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) { - state.motor_offset = mot * _compass._thr_or_curr; + } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { + 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(); + } } /*