/* update body magnetic field from position and rotation */ void Aircraft::update_mag_field_bf() { // get the magnetic field intensity and orientation float intensity; float declination; float inclination; get_mag_field_ef(location.lat*1e-7f,location.lng*1e-7f,intensity,declination,inclination); // create a field vector and rotate to the required orientation Vector3f mag_ef(1e3f * intensity, 0, 0); Matrix3f R; R.from_euler(0, -ToRad(inclination), ToRad(declination)); mag_ef = R * mag_ef; // calculate frame height 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; }
/* calculate magnetic field intensity and orientation */ float AP_Declination::get_declination(float latitude_deg, float longitude_deg) { float declination_deg=0, inclination_deg=0, intensity_gauss=0; get_mag_field_ef(latitude_deg, longitude_deg, intensity_gauss, declination_deg, inclination_deg); return declination_deg; }