src/utils/declination.py

   1 #!/usr/bin/env python
   2
   3 # Ported from https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_Declination/generate
   4 # Run this script with python3!
   5 # To install the igrf module, use python3 -m pip install --user igrf12
   6 # Note that it requires a fortran compiler
   7
   8 '''
   9 generate field tables from IGRF12
  10 '''
  11
  12 import collections
  13 import datetime
  14 import pathlib
  15 import sys
  16
  17 import igrf12
  18 import numpy as np
  19
  20 SAMPLING_RES = 10
  21 SAMPLING_MIN_LAT = -90
  22 SAMPLING_MAX_LAT = 90
  23 SAMPLING_MIN_LON = -180
  24 SAMPLING_MAX_LON = 180
  25
  26 # This is used for flash constrained environments. We limit
  27 # the latitude range to [-60, 60], so the values fit in an int8_t
  28 SAMPLING_COMPACT_MIN_LAT = -60
  29 SAMPLING_COMPACT_MAX_LAT = 60
  30
  31 PREPROCESSOR_SYMBOL = 'NAV_AUTO_MAG_DECLINATION_PRECISE'
  32
  33 Query = collections.namedtuple('Query', ['date', 'res', 'min_lat', 'max_lat', 'min_lon', 'max_lon'])
  34 Result = collections.namedtuple('Result', ['query', 'lats', 'lons', 'declination', 'inclination', 'intensity'])
  35
  36 def write_table(f, name, table, compact):
  37     '''write one table'''
  38
  39     if compact:
  40         format_entry = lambda x: '%d' % round(x)
  41         table_type = 'int8_t'
  42     else:
  43         table_type = 'float'
  44         format_entry = lambda x: '%.5ff' % x
  45
  46     num_lat = len(table)
  47     num_lon = len(table[0])
  48
  49     f.write("static const %s %s[%u][%u] = {\n" %
  50                 (table_type, name, num_lat, num_lon))
  51     for i in range(num_lat):
  52         f.write("    {")
  53         for j in range(num_lon):
  54             f.write(format_entry(table[i][j]))
  55             if j != num_lon - 1:
  56                 f.write(",")
  57         f.write("}")
  58         if i != num_lat - 1:
  59             f.write(",")
  60         f.write("\n")
  61     f.write("};\n\n")
  62
  63 def declination_tables(query):
  64     lats = np.arange(query.min_lat, query.max_lat + query.res, query.res)
  65     lons = np.arange(query.min_lon, query.max_lon + query.res, query.res)
  66
  67     num_lat = lats.size
  68     num_lon = lons.size
  69
  70     intensity = np.empty((num_lat, num_lon))
  71     inclination = np.empty((num_lat, num_lon))
  72     declination = np.empty((num_lat, num_lon))
  73
  74     for i, lat in enumerate(lats):
  75         for j, lon in enumerate(lons):
  76             mag = igrf12.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
  77             intensity[i][j] = mag.total / 1e5
  78             inclination[i][j] = mag.incl
  79             declination[i][j] = mag.decl
  80
  81     return Result(query=query, lats=lats, lons=lons,
  82         declination=declination, inclination=inclination, intensity=intensity)
  83
  84 def generate_constants(f, query):
  85     f.write('#define SAMPLING_RES\t\t%.5ff\n' % query.res)
  86     f.write('#define SAMPLING_MIN_LON\t%.5ff\n' % query.min_lon)
  87     f.write('#define SAMPLING_MAX_LON\t%.5ff\n' % query.max_lon)
  88     f.write('#define SAMPLING_MIN_LAT\t%.5ff\n' % query.min_lat)
  89     f.write('#define SAMPLING_MAX_LAT\t%.5ff\n' % query.max_lat)
  90     f.write('\n')
  91
  92 def generate_tables(f, query, compact):
  93     result = declination_tables(query)
  94     write_table(f, 'declination_table', result.declination, compact)
  95
  96     # We're not using these tables for now
  97     #if not compact:
  98     #    write_table(f, 'inclination_table', result.inclination, False)
  99     #    write_table(f, 'intensity_table', result.intensity, False)
 100
 101 def generate_code(f, date):
 102
 103     compact_query = Query(date=date, res=SAMPLING_RES,
 104         min_lat=SAMPLING_COMPACT_MIN_LAT, max_lat=SAMPLING_COMPACT_MAX_LAT,
 105         min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON)
 106
 107     precise_query = Query(date=date, res=SAMPLING_RES,
 108         min_lat=SAMPLING_MIN_LAT, max_lat=SAMPLING_MAX_LAT,
 109         min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON)
 110
 111     f.write('/* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */\n\n\n')
 112     f.write('/* Updated on %s */\n\n\n' % date)
 113     f.write('#include <stdint.h>\n\n')
 114
 115
 116     f.write('\n\n#if defined(%s)\n' % PREPROCESSOR_SYMBOL)
 117     generate_constants(f, precise_query)
 118     generate_tables(f, precise_query, False)
 119     # We're not using these tables for now
 120     # write_table(f, 'inclination_table', inclination_table)
 121     # write_table(f, 'intensity_table', intensity_table)
 122     f.write('#else /* !%s */\n' % PREPROCESSOR_SYMBOL)
 123     generate_constants(f, compact_query)
 124     generate_tables(f, compact_query, True)
 125     f.write('#endif\n')
 126
 127 if __name__ == '__main__':
 128
 129     output = pathlib.PurePath(__file__).parent / '..' / 'main'  / 'navigation' / 'navigation_declination_gen.c'
 130     date = datetime.datetime.now()
 131
 132     with open(output, 'w') as f:
 133         generate_code(f, date)