Browse Source

AP_Declination: 2to3, IGRF API update

use built-in IGRF total, incl, decl computations
mission-4.1.18
scivision 7 years ago committed by Andrew Tridgell
parent
commit
67a503d5d8
  1. 138
      libraries/AP_Declination/generate/generate.py

138
libraries/AP_Declination/generate/generate.py

@ -6,93 +6,81 @@ generate field tables from IGRF12
import pyigrf12 as igrf import pyigrf12 as igrf
import numpy as np import numpy as np
import datetime import datetime
import os.path from pathlib import Path
import sys
from math import sqrt, pi, atan2, asin if not Path("AP_Declination.h").is_file():
raise OSError("Please run this tool from the AP_Declination directory")
if not os.path.isfile("AP_Declination.h"):
print("Please run this tool from the AP_Declination directory")
sys.exit(1) def write_table(f,name, table):
'''write one table'''
f.write("const float AP_Declination::%s[%u][%u] = {\n" %
def field_to_angles(x, y, z): (name, NUM_LAT, NUM_LON))
intensity = sqrt(x**2+y**2+z**2) for i in range(NUM_LAT):
r2d = 180.0 / pi f.write(" {")
declination = r2d * atan2(y, x) for j in range(NUM_LON):
inclination = r2d * asin(z / intensity) f.write("%.5f" % table[i][j])
return [intensity, inclination, declination] if j != NUM_LON-1:
f.write(",")
f.write("}")
isv = 0 if i != NUM_LAT-1:
date = datetime.datetime.now() f.write(",")
itype = 1 f.write("\n")
alt = 0.0 f.write("};\n\n")
SAMPLING_RES = 10
SAMPLING_MIN_LAT = -90
SAMPLING_MAX_LAT = 90
SAMPLING_MIN_LON = -180 if __name__ == '__main__':
SAMPLING_MAX_LON = 180
date = datetime.datetime.now()
NUM_LAT = 1 + (SAMPLING_MAX_LAT - SAMPLING_MIN_LAT) / SAMPLING_RES # date = datetime.date(2018,2,20)
NUM_LON = 1 + (SAMPLING_MAX_LON - SAMPLING_MIN_LON) / SAMPLING_RES
SAMPLING_RES = 10
intensity_table = np.empty((NUM_LAT, NUM_LON)) SAMPLING_MIN_LAT = -90
inclination_table = np.empty((NUM_LAT, NUM_LON)) SAMPLING_MAX_LAT = 90
declination_table = np.empty((NUM_LAT, NUM_LON)) SAMPLING_MIN_LON = -180
SAMPLING_MAX_LON = 180
for i in range(NUM_LAT):
for j in range(NUM_LON): lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES)
lat = SAMPLING_MIN_LAT + i*SAMPLING_RES lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES)
lon = SAMPLING_MIN_LON + j*SAMPLING_RES
x, y, z, f, d = igrf.gridigrf12(date, isv, itype, alt, lat, lon) NUM_LAT = lats.size
intensity, I, D = field_to_angles(x, y, z) NUM_LON = lons.size
intensity_table[i][j] = intensity * 0.00001
inclination_table[i][j] = I intensity_table = np.empty((NUM_LAT, NUM_LON))
declination_table[i][j] = D inclination_table = np.empty((NUM_LAT, NUM_LON))
declination_table = np.empty((NUM_LAT, NUM_LON))
tfile = open("tables.cpp", 'w')
tfile.write('''// this is an auto-generated file from the IGRF tables. Do not edit for i,lat in enumerate(lats):
for j,lon in enumerate(lons):
mag = igrf.igrf(date=date, glat=lat, glon=lon, alt=0., isv=0, itype=1)
intensity_table[i][j] = mag.total/1e5
inclination_table[i][j] = mag.incl
declination_table[i][j] = mag.decl
with open("tables.cpp", 'w') as f:
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"
''') ''')
tfile.write(''' f.write('''const float AP_Declination::SAMPLING_RES = %u;
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;
const float AP_Declination::SAMPLING_MAX_LON = %u; const float AP_Declination::SAMPLING_MAX_LON = %u;
''' % (SAMPLING_RES, ''' % (SAMPLING_RES,
SAMPLING_MIN_LAT, SAMPLING_MIN_LAT,
SAMPLING_MAX_LAT, SAMPLING_MAX_LAT,
SAMPLING_MIN_LON, SAMPLING_MIN_LON,
SAMPLING_MAX_LON)) SAMPLING_MAX_LON))
def write_table(name, table):
'''write one table'''
tfile.write("const float AP_Declination::%s[%u][%u] = {\n" %
(name, NUM_LAT, NUM_LON))
for i in range(NUM_LAT):
tfile.write(" {")
for j in range(NUM_LON):
tfile.write("%.5f" % table[i][j])
if j != NUM_LON-1:
tfile.write(",")
tfile.write("}")
if i != NUM_LAT-1:
tfile.write(",")
tfile.write("\n")
tfile.write("};\n\n")
write_table('declination_table', declination_table)
write_table('inclination_table', inclination_table)
write_table('intensity_table', intensity_table)
write_table(f,'declination_table', declination_table)
write_table(f,'inclination_table', inclination_table)
write_table(f,'intensity_table', intensity_table)
tfile.close()

Loading…
Cancel
Save