Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Declination: update generate script 

added ability to display max error
This commit is contained in:
Andrew Tridgell 2019-12-20 11:34:13 +11:00
parent 956c3f29a3
commit 79bbd3ce9f
1 changed files with 135 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
libraries/AP_Declination/generate/generate.py
View File

@ -1,12 +1,22 @@
#!/usr/bin/env python #!/usr/bin/env python3
''' '''
generate field tables from IGRF12 generate field tables from IGRF12. Note that this requires python3
''' '''
import igrf12 as igrf import igrf12 as igrf
import numpy as np import numpy as np
import datetime import datetime
from pathlib import Path from pathlib import Path
from pymavlink.rotmat import Vector3, Matrix3
import math
import argparse
parser = argparse.ArgumentParser(description='generate mag tables')
parser.add_argument('--sampling-res', type=int, default=10, help='sampling resolution, degrees')
parser.add_argument('--check-error', action='store_true', help='check max error')
parser.add_argument('--filename', type=str, default='tables.cpp', help='tables file')
args = parser.parse_args()
if not Path("AP_Declination.h").is_file(): if not Path("AP_Declination.h").is_file():
raise OSError("Please run this tool from the AP_Declination directory") raise OSError("Please run this tool from the AP_Declination directory")
@ -28,46 +38,133 @@ def write_table(f,name, table):
f.write("\n") f.write("\n")
f.write("};\n\n") f.write("};\n\n")
date = datetime.datetime.now()
SAMPLING_RES = args.sampling_res
SAMPLING_MIN_LAT = -90
SAMPLING_MAX_LAT = 90
SAMPLING_MIN_LON = -180
SAMPLING_MAX_LON = 180
lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES)
lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES)
NUM_LAT = lats.size
NUM_LON = lons.size
intensity_table = np.empty((NUM_LAT, NUM_LON))
inclination_table = np.empty((NUM_LAT, NUM_LON))
declination_table = np.empty((NUM_LAT, NUM_LON))
max_error = 0
max_error_pos = None
max_error_field = None
def get_igrf(lat, lon):
'''return field as [declination_deg, inclination_deg, intensity_gauss]'''
mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
intensity = float(mag.total/1e5)
inclination = float(mag.incl)
declination = float(mag.decl)
return [declination, inclination, intensity]
def interpolate_table(table, latitude_deg, longitude_deg):
'''interpolate inside a table for a given lat/lon in degrees'''
# round down to nearest sampling resolution
min_lat = int(math.floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES)
min_lon = int(math.floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES)
# find index of nearest low sampling point
min_lat_index = int(math.floor(-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES)
min_lon_index = int(math.floor(-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES)
# calculate intensity
data_sw = table[min_lat_index][min_lon_index]
data_se = table[min_lat_index][min_lon_index + 1]
data_ne = table[min_lat_index + 1][min_lon_index + 1]
data_nw = table[min_lat_index + 1][min_lon_index]
# perform bilinear interpolation on the four grid corners
data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw
data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw
value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min
return value
'''
calculate magnetic field intensity and orientation, interpolating in tables
if __name__ == '__main__': returns array [declination_deg, inclination_deg, intensity] or None
'''
def interpolate_field(latitude_deg, longitude_deg):
# limit to table bounds
if latitude_deg < SAMPLING_MIN_LAT:
return None
if latitude_deg >= SAMPLING_MAX_LAT:
return None
if longitude_deg < SAMPLING_MIN_LON:
return None
if longitude_deg >= SAMPLING_MAX_LON:
return None
date = datetime.datetime.now() intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg)
# date = datetime.date(2018,2,20) declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg)
inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg)
SAMPLING_RES = 10 return [declination_deg, inclination_deg, intensity_gauss]
SAMPLING_MIN_LAT = -90
SAMPLING_MAX_LAT = 90
SAMPLING_MIN_LON = -180
SAMPLING_MAX_LON = 180
lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES) def field_to_Vector3(mag):
lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES) '''return mGauss field from dec, inc and intensity'''
R = Matrix3()
mag_ef = Vector3(mag[2]*1000.0, 0.0, 0.0)
R.from_euler(0.0, -math.radians(mag[1]), math.radians(mag[0]))
return R * mag_ef
NUM_LAT = lats.size def test_error(lat, lon):
NUM_LON = lons.size '''check for error from lat,lon'''
global max_error, max_error_pos, max_error_field
mag1 = get_igrf(lat, lon)
mag2 = interpolate_field(lat, lon)
ef1 = field_to_Vector3(mag1)
ef2 = field_to_Vector3(mag2)
err = (ef1 - ef2).length()
if err > max_error or err > 100:
print(lat, lon, err, ef1, ef2)
max_error = err
max_error_pos = (lat, lon)
max_error_field = ef1 - ef2
intensity_table = np.empty((NUM_LAT, NUM_LON)) def test_max_error(lat, lon):
inclination_table = np.empty((NUM_LAT, NUM_LON)) '''check for maximum error from lat,lon over SAMPLING_RES range'''
declination_table = np.empty((NUM_LAT, NUM_LON)) steps = 3
delta = SAMPLING_RES/steps
for i in range(steps):
for j in range(steps):
lat2 = lat + i * delta
lon2 = lon + j * delta
if lat2 >= SAMPLING_MAX_LAT or lon2 >= SAMPLING_MAX_LON:
continue
if lat2 <= SAMPLING_MIN_LAT or lon2 <= SAMPLING_MIN_LON:
continue
test_error(lat2, lon2)
for i,lat in enumerate(lats): for i,lat in enumerate(lats):
for j,lon in enumerate(lons): for j,lon in enumerate(lons):
mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1) mag = get_igrf(lat, lon)
intensity_table[i][j] = mag.total/1e5 declination_table[i][j] = mag[0]
inclination_table[i][j] = mag.incl inclination_table[i][j] = mag[1]
declination_table[i][j] = mag.decl intensity_table[i][j] = mag[2]
with open("tables.cpp", 'w') as f: with open(args.filename, 'w') as f:
f.write('''// this is an auto-generated file from the IGRF tables. Do not edit f.write('''// this is an auto-generated file from the IGRF tables. Do not edit
// To re-generate run generate/generate.py // To re-generate run generate/generate.py
#include "AP_Declination.h" #include "AP_Declination.h"
''') ''')
f.write('''const float AP_Declination::SAMPLING_RES = %u; f.write('''const float AP_Declination::SAMPLING_RES = %u;
const float AP_Declination::SAMPLING_MIN_LAT = %u; const float AP_Declination::SAMPLING_MIN_LAT = %u;
const float AP_Declination::SAMPLING_MAX_LAT = %u; const float AP_Declination::SAMPLING_MAX_LAT = %u;
const float AP_Declination::SAMPLING_MIN_LON = %u; const float AP_Declination::SAMPLING_MIN_LON = %u;
@ -80,7 +177,16 @@ const float AP_Declination::SAMPLING_MAX_LON = %u;
SAMPLING_MAX_LON)) SAMPLING_MAX_LON))
write_table(f,'declination_table', declination_table) write_table(f,'declination_table', declination_table)
write_table(f,'inclination_table', inclination_table) write_table(f,'inclination_table', inclination_table)
write_table(f,'intensity_table', intensity_table) write_table(f,'intensity_table', intensity_table)
if args.check_error:
print("Checking for maximum error")
for lat in range(-60,60,1):
for lon in range(-180,180,1):
test_max_error(lat, lon)
print("Generated with max error %.2f %s at (%.2f,%.2f)" % (
max_error, max_error_field, max_error_pos[0], max_error_pos[1]))
print("Table generated in %s" % args.filename)