From 79bbd3ce9f8f23113a1420c4cf4755ad433bf67d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 20 Dec 2019 11:34:13 +1100 Subject: [PATCH] AP_Declination: update generate script added ability to display max error --- libraries/AP_Declination/generate/generate.py | 164 ++++++++++++++---- 1 file changed, 135 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Declination/generate/generate.py b/libraries/AP_Declination/generate/generate.py index 6e1d2c080f..44aee0d1d4 100755 --- a/libraries/AP_Declination/generate/generate.py +++ b/libraries/AP_Declination/generate/generate.py @@ -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 numpy as np import datetime 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(): 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\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() - # date = datetime.date(2018,2,20) + intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg) + declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg) + inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg) - SAMPLING_RES = 10 - SAMPLING_MIN_LAT = -90 - SAMPLING_MAX_LAT = 90 - SAMPLING_MIN_LON = -180 - SAMPLING_MAX_LON = 180 + return [declination_deg, inclination_deg, intensity_gauss] - 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) +def field_to_Vector3(mag): + '''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 - NUM_LON = lons.size +def test_error(lat, lon): + '''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)) - inclination_table = np.empty((NUM_LAT, NUM_LON)) - declination_table = np.empty((NUM_LAT, NUM_LON)) +def test_max_error(lat, lon): + '''check for maximum error from lat,lon over SAMPLING_RES range''' + 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 j,lon in enumerate(lons): - mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1) - intensity_table[i][j] = mag.total/1e5 - inclination_table[i][j] = mag.incl - declination_table[i][j] = mag.decl +for i,lat in enumerate(lats): + for j,lon in enumerate(lons): + mag = get_igrf(lat, lon) + declination_table[i][j] = mag[0] + inclination_table[i][j] = mag[1] + intensity_table[i][j] = mag[2] - with open("tables.cpp", 'w') as f: - f.write('''// this is an auto-generated file from the IGRF tables. Do not edit +with open(args.filename, '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 #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_MAX_LAT = %u; const float AP_Declination::SAMPLING_MIN_LON = %u; @@ -80,7 +177,16 @@ const float AP_Declination::SAMPLING_MAX_LON = %u; SAMPLING_MAX_LON)) - write_table(f,'declination_table', declination_table) - write_table(f,'inclination_table', inclination_table) - write_table(f,'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) +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)