/*
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;
}
