Compass: basic compensation for motor interference

ArduCopter/Attitude.pde

@@ -758,6 +758,9 @@ void set_throttle_out( int16_t throttle_out, bool apply_angle_boost )
         // clear angle_boost for logging purposes
         angle_boost = 0;
     }
+
+    // update compass with throttle value
+    compass.set_throttle((float)g.rc_3.servo_out/1000.0f);
 }
 
 // set_throttle_accel_target - to be called by upper throttle controllers to set desired vertical acceleration in earth frame

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -330,6 +330,17 @@ bool AP_Compass_HMC5843::read()
     rot_mag.rotate(_board_orientation);
 
     rot_mag += _offset.get();
+
+    // add in motor compensation
+    if( _throttle_pct != 0 ) {
+        _motor_offset = _motor_compensation.get() * _throttle_pct;
+        rot_mag += _motor_offset;
+    }else{
+        _motor_offset.x = 0;
+        _motor_offset.y = 0;
+        _motor_offset.z = 0;
+    }
+
     mag_x = rot_mag.x;
     mag_y = rot_mag.y;
     mag_z = rot_mag.z;

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -67,6 +67,16 @@ bool AP_Compass_PX4::read(void)
     _sum.rotate(_board_orientation);
     _sum += _offset.get();
 
+    // add in motor compensation
+    if( _throttle_pct != 0 ) {
+        _motor_offset = _motor_compensation.get() * _throttle_pct;
+        _sum += _motor_offset;
+    }else{
+        _motor_offset.x = 0;
+        _motor_offset.y = 0;
+        _motor_offset.z = 0;
+    }
+
     mag_x = _sum.x;
     mag_y = _sum.y;
     mag_z = _sum.z;

libraries/AP_Compass/Compass.cpp

@@ -54,7 +54,27 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     // @Values: 0:Disabled,1:Enabled
     // @User: Advanced
     AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination, 1),
+
+    // @Param: MOT_X
+    // @DisplayName: Motor interference compensation for body frame X axis
+    // @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference
+    // @Range: -400 400
+    // @Increment: 1
+
+    // @Param: MOT_Y
+    // @DisplayName: Motor interference compensation for body frame Y axis
+    // @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference
+    // @Range: -400 400
+    // @Increment: 1
+
+    // @Param: MOT_Z
+    // @DisplayName: Motor interference compensation for body frame Z axis
+    // @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference
+    // @Range: -400 400
+    // @Increment: 1
+    AP_GROUPINFO("MOT",    6, Compass, _motor_compensation, 0),
 #endif
+
     AP_GROUPEND
 };
 
@@ -102,6 +122,18 @@ Compass::get_offsets()
     return _offset;
 }
 
+void
+Compass::set_motor_compensation(const Vector3f &motor_comp_factor)
+{
+    _motor_compensation.set(motor_comp_factor);
+}
+
+void
+Compass::save_motor_compensation()
+{
+    _motor_compensation.save();
+}
+
 void
 Compass::set_initial_location(int32_t latitude, int32_t longitude)
 {

libraries/AP_Compass/Compass.h

@@ -121,13 +121,47 @@ public:
     virtual void set_declination(float radians);
     float get_declination();
 
-    static const struct AP_Param::GroupInfo var_info[];
-
     // set overall board orientation
     void set_board_orientation(enum Rotation orientation) {
         _board_orientation = orientation;
     }
 
+    /// Set the motor compensation factor x/y/z values.
+    ///
+    /// @param  offsets             Offsets multiplied by the throttle value and added to the raw mag_ values.
+    ///
+    virtual void set_motor_compensation(const Vector3f &motor_comp_factor);
+
+    /// Set new motor compensation factors as a vector
+    ///
+    /// @param  x                   multiplied by throttle value and added to the raw mag_x value.
+    /// @param  y                   multiplied by throttle value and added to the raw mag_y value.
+    /// @param  z                   multiplied by throttle value and added to the raw mag_z value.
+    ///
+    void set_motor_compensation(float x, float y, float z) {
+        set_motor_compensation(Vector3f(x, y, z));
+    }
+
+    /// Saves the current motor compensation x/y/z values.
+    ///
+    /// This should be invoked periodically to save the offset values calculated by the motor compensation auto learning
+    ///
+    virtual void save_motor_compensation();
+
+    /// Returns the current motor compensation offset values
+    ///
+    /// @returns                    The current compass offsets.
+    ///
+    virtual Vector3f &get_motor_offsets() { return _motor_offset; }
+
+    /// Set the throttle as a percentage from 0.0 to 1.0
+    /// @param thr_pct              throttle expressed as a percentage from 0 to 1.0
+    void set_throttle(float thr_pct) {
+        _throttle_pct = thr_pct;
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
 protected:
     enum Rotation _orientation;
     AP_Vector3f _offset;
@@ -143,6 +177,11 @@ protected:
     uint8_t _mag_history_index;
     Vector3i _mag_history[_mag_history_size];
 
+    // motor compensation
+    AP_Vector3f _motor_compensation;            // factors multiplied by throttle and added to compass outputs
+    Vector3f _motor_offset;                     // latest compensation added to compass
+    float _throttle_pct;                        // throttle expressed as a percentage from 0.0 ~ 1.0
+
     // board orientation from AHRS
     enum Rotation _board_orientation;
 };