SITL: Add magnetic field environment to simulation

Includes parameters enabling a ground level magnetic anomaly to be modelled.
TODO - add automatic setting of declination, inclination and field strength using WGS-84 position.

libraries/SITL/SIM_Aircraft.cpp

@@ -153,6 +153,39 @@ void Aircraft::update_position(void)
 #endif
 }
 
+/*
+   update body magnetic field from position and rotation
+*/
+void Aircraft::update_mag_field_bf()
+{
+    // assume a earth field strength of 450 mGauss
+    Vector3f mag_ef(450, 0, 0);
+
+    // rotate _Bearth for inclination and declination. -66 degrees
+    // is the inclination in Canberra, Australia
+    // TODO implement inclination and declination lookup
+    Matrix3f R;
+    R.from_euler(0, ToRad(66), ToRad(11));
+    mag_ef = R * mag_ef;
+
+    // calculate height of frame above ground
+    float frame_height_agl = fmaxf((-position.z) + home.alt*0.01f - ground_level, 0.0f);
+
+    // calculate scaling factor that varies from 1 at ground level to 1/8 at sitl->mag_anomaly_hgt
+    // Assume magnetic anomaly strength scales with 1/R**3
+    float anomaly_scaler = (sitl->mag_anomaly_hgt / (frame_height_agl + sitl->mag_anomaly_hgt));
+    anomaly_scaler = anomaly_scaler * anomaly_scaler * anomaly_scaler;
+
+    // add scaled anomaly to earth field
+    mag_ef += sitl->mag_anomaly_ned.get() * anomaly_scaler;
+
+    // Rotate into body frame
+    mag_bf = dcm.transposed() * mag_ef;
+
+    // add motor interference
+    mag_bf += sitl->mag_mot.get() * battery_current;
+}
+
 /* advance time by deltat in seconds */
 void Aircraft::time_advance(float deltat)
 {
@@ -297,6 +330,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) const
     fdm.rpm2 = rpm2;
     fdm.rcin_chan_count = rcin_chan_count;
     memcpy(fdm.rcin, rcin, rcin_chan_count*sizeof(float));
+    fdm.bodyMagField = mag_bf;
 }
 
 uint64_t Aircraft::get_wall_time_us() const

libraries/SITL/SIM_Aircraft.h

@@ -96,6 +96,10 @@ public:
     const Matrix3f &get_dcm(void) const {
         return dcm;
     }
+
+    const Vector3f &get_mag_field_bf(void) const {
+        return mag_bf;
+    }
     
 protected:
     SITL *sitl;
@@ -123,6 +127,8 @@ protected:
     uint8_t rcin_chan_count = 0;
     float rcin[8];
 
+    Vector3f mag_bf; // local earth magnetic field vector in Gauss, earth frame
+
     uint64_t time_now_us;
 
     const float gyro_noise;
@@ -143,6 +149,9 @@ protected:
     /* update location from position */
     void update_position(void);
 
+    /* update body frame magnetic field */
+    void update_mag_field_bf(void);
+
     /* advance time by deltat in seconds */
     void time_advance(float deltat);
 

libraries/SITL/SIM_Balloon.cpp

@@ -71,6 +71,9 @@ void Balloon::update(const struct sitl_input &input)
     
     // update lat/lon/altitude
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 } // namespace SITL

libraries/SITL/SIM_CRRCSim.cpp

@@ -152,6 +152,9 @@ void CRRCSim::update(const struct sitl_input &input)
     send_servos(input);
     recv_fdm(input);
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 } // namespace SITL

libraries/SITL/SIM_Calibration.cpp

@@ -46,6 +46,9 @@ void SITL::Calibration::update(const struct sitl_input& input)
     accel_body(0, 0, 0);
     update_dynamics(rot_accel);
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 void SITL::Calibration::_stop_control(const struct sitl_input& input,

libraries/SITL/SIM_FlightAxis.cpp

@@ -346,6 +346,9 @@ void FlightAxis::update(const struct sitl_input &input)
     last_time_s = state.m_currentPhysicsTime_SEC;
 
     last_velocity_ef = velocity_ef;
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 } // namespace SITL

libraries/SITL/SIM_Gazebo.cpp

@@ -126,6 +126,9 @@ void Gazebo::update(const struct sitl_input &input)
     send_servos(input);
     recv_fdm(input);
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 } // namespace SITL

libraries/SITL/SIM_Helicopter.cpp

@@ -173,6 +173,9 @@ void Helicopter::update(const struct sitl_input &input)
 
     // update lat/lon/altitude
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 } // namespace SITL

libraries/SITL/SIM_Multicopter.cpp

@@ -72,6 +72,9 @@ void MultiCopter::update(const struct sitl_input &input)
     
     // update lat/lon/altitude
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 

libraries/SITL/SIM_Plane.cpp

@@ -258,4 +258,7 @@ void Plane::update(const struct sitl_input &input)
     
     // update lat/lon/altitude
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }

libraries/SITL/SIM_QuadPlane.cpp

@@ -97,4 +97,7 @@ void QuadPlane::update(const struct sitl_input &input)
 
     // update lat/lon/altitude
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }

libraries/SITL/SIM_Rover.cpp

@@ -155,6 +155,9 @@ void SimRover::update(const struct sitl_input &input)
 
     // update lat/lon/altitude
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 } // namespace SITL

libraries/SITL/SIM_SingleCopter.cpp

@@ -115,4 +115,7 @@ void SingleCopter::update(const struct sitl_input &input)
 
     // update lat/lon/altitude
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }

libraries/SITL/SIM_Tracker.cpp

@@ -135,6 +135,9 @@ void Tracker::update(const struct sitl_input &input)
     // new velocity vector
     velocity_ef.zero();
     update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
 }
 
 } // namespace SITL

libraries/SITL/SITL.cpp

@@ -79,6 +79,8 @@ const AP_Param::GroupInfo SITL::var_info[] = {
     AP_GROUPINFO("ADSB_COUNT",    45, SITL,  adsb_plane_count, -1),
     AP_GROUPINFO("ADSB_RADIUS",   46, SITL,  adsb_radius_m, 1000),
     AP_GROUPINFO("ADSB_ALT",      47, SITL,  adsb_altitude_m, 1000),
+    AP_GROUPINFO("MAG_ALY",       48, SITL,  mag_anomaly_ned, 0),
+    AP_GROUPINFO("MAG_ALY_HGT",   49, SITL,  mag_anomaly_hgt, 1.0f),
     AP_GROUPEND
 };
 

libraries/SITL/SITL.h

@@ -25,6 +25,7 @@ struct sitl_fdm {
     double rpm2;            // secondary RPM
     uint8_t rcin_chan_count;
     float  rcin[8];         // RC input 0..1
+    Vector3f bodyMagField;  // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss.
 };
 
 // number of rc output channels
@@ -118,6 +119,10 @@ public:
     AP_Float adsb_radius_m;
     AP_Float adsb_altitude_m;
 
+    // Earth magnetic field anomaly
+    AP_Vector3f mag_anomaly_ned; // NED anomaly vector at ground level (mGauss)
+    AP_Float mag_anomaly_hgt; // height above ground where anomally strength has decayed to 1/8 of the ground level value (m)
+
     void simstate_send(mavlink_channel_t chan);
 
     void Log_Write_SIMSTATE(DataFlash_Class *dataflash);