mavextra: removed cache from earth field calculations

the cache was not winning much time and made external usage more
complex

mavextra.py

@@ -1326,6 +1326,7 @@ def interpolate_table(table, latitude_deg, longitude_deg):
     value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min
     return value
 
+earth_declination = None
 
 '''
 calculate magnetic field intensity and orientation, interpolating in tables
@@ -1347,17 +1348,14 @@ def get_mag_field_ef(latitude_deg, longitude_deg):
     declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg)
     inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg)
 
+    global earth_declination
+    earth_declination = declination_deg
+
     return [declination_deg, inclination_deg, intensity_gauss]
-
-earth_field = None
-earth_declination = None
+
 
 def expected_earth_field_lat_lon(lat, lon):
     '''return expected magnetic field for a location'''
-    global earth_field
-    if earth_field is not None:
-        return earth_field
-
     field_var = get_mag_field_ef(lat, lon)
     mag_ef = Vector3(field_var[2]*1000.0, 0.0, 0.0)
     R = Matrix3()
@@ -1366,13 +1364,8 @@ def expected_earth_field_lat_lon(lat, lon):
     earth_field = mag_ef
     return earth_field
 
-def expected_earth_field(GPS):
-    '''return expected magnetic field for a location'''
-    global earth_field
-    global earth_declination
-    if earth_field is not None:
-        return earth_field
-
+def get_lat_lon_status(GPS):
+    '''get lat, lon and status from a GPS message'''
     if hasattr(GPS,'fix_type'):
         gps_status = GPS.fix_type
         lat = GPS.lat*1.0e-7
@@ -1381,23 +1374,20 @@ def expected_earth_field(GPS):
         gps_status = GPS.Status
         lat = GPS.Lat
         lon = GPS.Lng
+    return (lat, lon, gps_status)
+
+def expected_earth_field(GPS):
+    '''return expected magnetic field for a location'''
+    (lat, lon, gps_status) = get_lat_lon_status(GPS)
 
     if gps_status < 3:
         return Vector3(0,0,0)
-    field_var = get_mag_field_ef(lat, lon)
-    earth_declination = field_var[0]
-    mag_ef = Vector3(field_var[2]*1000.0, 0.0, 0.0)
-    R = Matrix3()
-    R.from_euler(0.0, -radians(field_var[1]), radians(field_var[0]))
-    mag_ef = R * mag_ef
-    earth_field = mag_ef
-    return earth_field
+    return expected_earth_field_lat_lon(lat, lon)
+
 
 def expected_mag(GPS,ATT,roll_adjust=0,pitch_adjust=0,yaw_adjust=0):
     '''return expected magnetic field for a location and attitude'''
-    global earth_field
-
-    expected_earth_field(GPS)
+    earth_field = expected_earth_field(GPS)
     if earth_field is None:
         return Vector3(0,0,0)
 
@@ -1419,9 +1409,7 @@ def expected_mag(GPS,ATT,roll_adjust=0,pitch_adjust=0,yaw_adjust=0):
 
 def expected_mag_latlon(lat,lon,ATT,roll_adjust=0,pitch_adjust=0,yaw_adjust=0):
     '''return expected magnetic field for a location and attitude'''
-    global earth_field
-
-    expected_earth_field_lat_lon(lat,lon)
+    earth_field = expected_earth_field_lat_lon(lat,lon)
     if earth_field is None:
         return Vector3(0,0,0)
 
@@ -1454,8 +1442,11 @@ def mag_yaw(GPS,ATT,MAG):
     headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y
     headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z)
 
-    global earth_declination
-    heading = degrees(atan2(-headY,headX)) + earth_declination
+    # we need the declination too
+    (lat, lon, gps_status) = get_lat_lon_status(GPS)
+    field = get_mag_field_ef(lat, lon)
+    declination = field[0]
+    heading = degrees(atan2(-headY,headX)) + declination
     if heading < 0:
         heading += 360
     return heading
@@ -1478,8 +1469,7 @@ def expected_mag_yaw(GPS,ATT,MAG,roll_adjust=0,pitch_adjust=0,yaw_adjust=0):
 
 def earth_field_error(GPS,NKF2):
     '''return vector error in earth field estimate'''
-    global earth_field
-    expected_earth_field(GPS)
+    earth_field = expected_earth_field(GPS)
     if earth_field is None:
         return Vector3(0,0,0)
     ef = Vector3(NKF2.MN,NKF2.ME,NKF2.MD)
@@ -1569,7 +1559,6 @@ def reset_state_data():
     global last_delta
     global first_fix
     global dcm_state
-    global earth_field
     global last_sum
     global last_integral
     average_data.clear()
@@ -1580,7 +1569,6 @@ def reset_state_data():
     last_integral.clear()
     first_fix = None
     dcm_state = None
-    earth_field = None
 
 # terrain functions, using MAVProxy elevation module
 EleModel = None