AP_Compass: use apply_correction_function to eliminate duplication

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -50,18 +50,9 @@ void AP_Compass_HIL::_setup_earth_field(void)
 
 bool AP_Compass_HIL::read()
 {
-    // get offsets
-    Vector3f ofs = _offset[0].get();
-
-    // apply motor compensation
-    if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-        _motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
-    }else{
-        _motor_offset[0].zero();
-    }
+    _field[0] = _hil_mag;
     
-    // return last values provided by setHIL function
-    _field[0] = _hil_mag + ofs + _motor_offset[0];
+    apply_corrections(_field[0],0);
 
     // values set by setHIL function
     last_update = hal.scheduler->micros();      // record time of update

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -371,15 +371,7 @@ bool AP_Compass_HMC5843::read()
         _field[0].rotate(_board_orientation);
     }
 
-    _field[0] += _offset[0].get();
-
-    // apply motor compensation
-    if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-        _motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
-        _field[0] += _motor_offset[0];
-    }else{
-        _motor_offset[0].zero();
-    }
+    apply_corrections(_field[0],0);
 
     _healthy[0] = true;
 

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -116,17 +116,8 @@ bool AP_Compass_PX4::read(void)
             _sum[i].rotate(_board_orientation);
         }
         
-        _sum[i] += _offset[i].get();
-
-        // apply motor compensation
-        if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-            _motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
-            _sum[i] += _motor_offset[i];
-        } else {
-            _motor_offset[i].zero();
-        }
-    
         _field[i] = _sum[i];
+        apply_corrections(_field[i],i);
     
         _sum[i].zero();
         _count[i] = 0;

libraries/AP_Compass/AP_Compass_VRBRAIN.cpp

@@ -125,17 +125,8 @@ bool AP_Compass_VRBRAIN::read(void)
             _sum[i].rotate(_board_orientation);
         }
     
-        _sum[i] += _offset[i].get();
-
-        // apply motor compensation
-        if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-            _motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
-            _sum[i] += _motor_offset[i];
-        } else {
-            _motor_offset[i].zero();
-        }
-    
         _field[i] = _sum[i];
+        apply_corrections(_field[i],i);
     
         _sum[i].zero();
         _count[i] = 0;

libraries/AP_Compass/Compass.cpp

@@ -462,3 +462,17 @@ bool Compass::configured(void)
     }
     return all_configured;
 }
+
+void Compass::apply_corrections(Vector3f &mag, uint8_t i)
+{
+    const Vector3f &offsets = _offset[i].get();
+    const Vector3f &mot = _motor_compensation[i].get();
+    
+    mag += offsets;
+    if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
+        _motor_offset[i] = mot * _thr_or_curr;
+        mag += _motor_offset[i];
+    }else{
+        _motor_offset[i].zero();
+    }
+}

libraries/AP_Compass/Compass.h

@@ -282,5 +282,7 @@ protected:
 
     // board orientation from AHRS
     enum Rotation _board_orientation;
+    
+    void apply_corrections(Vector3f &mag, uint8_t i);
 };
 #endif