Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

lib/world_magnetic_model: fetch_noaa_table.py refactor and scaling improvements #23152

Merged
merged 2 commits into from
Jun 4, 2024
Merged

lib/world_magnetic_model: fetch_noaa_table.py refactor and scaling improvements #23152

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ccbae90
lib/world_magnetic_model: fetch_noaa_table.py refactor and scaling im…
dagar May 16, 2024
8dc431c
simplify world_magnetic_model interface (degrees in, degrees out)
dagar May 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 4 additions & 7 deletions src/examples/fake_magnetometer/FakeMagnetometer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,15 +66,12 @@ void FakeMagnetometer::Run()
if (_vehicle_gps_position_sub.copy(&gps)) {
if (gps.eph < 1000) {

const double lat = gps.latitude_deg;
const double lon = gps.longitude_deg;

// magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = get_mag_declination_radians(lat, lon);
const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
const float declination_rad = math::radians(get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg));
const float inclination_rad = math::radians(get_mag_inclination_degrees(gps.latitude_deg, gps.longitude_deg));
const float field_strength_gauss = get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg);

_mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);
_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);

_mag_earth_available = true;
}
Expand Down
236 changes: 79 additions & 157 deletions src/lib/world_magnetic_model/fetch_noaa_table.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
############################################################################
#
# Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
# Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
Expand Down Expand Up @@ -33,6 +33,7 @@
############################################################################

import math
import numpy
import json
import statistics
import sys
Expand All @@ -49,7 +50,7 @@ def constrain(n, nmin, nmax):

header = """/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
Expand Down Expand Up @@ -80,6 +81,9 @@ def constrain(n, nmin, nmax):
*
****************************************************************************/
"""

key=sys.argv[1] # NOAA key (https://www.ngdc.noaa.gov/geomag/CalcSurvey.shtml)

print(header)

print('#include <stdint.h>\n')
Expand All @@ -97,188 +101,106 @@ def constrain(n, nmin, nmax):
print('static constexpr int LON_DIM = {}'.format(LON_DIM) + ';')
print('\n')

print('// *INDENT-OFF*')

# Declination
params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'd', 'resultFormat': 'json'})
key=sys.argv[1] # NOAA key (https://www.ngdc.noaa.gov/geomag/CalcSurvey.shtml)
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
data = json.loads(f.read())
print("// Magnetic declination data in radians * 10^-4")
print('// Model: {},'.format(data['model']))
print('// Version: {},'.format(data['version']))
print('// Date: {},'.format(data['result'][0]['date']))
print('static constexpr const int16_t declination_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {")
print('\t// LONGITUDE: ', end='')
for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
print('{0:6d},'.format(l), end='')
print('')

declination_min = float('inf')
declination_max = float('-inf')
declination_min_lat_lon = ()
declination_max_lat_lon = ()

for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES):
params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'd', 'resultFormat': 'json'})
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
data = json.loads(f.read())

latitude_blackout_zone = False

print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
for p in data['result']:
declination_rad = math.radians(p['declination'])

warning = False
print('// *INDENT-OFF*\n\n\n')

try:
if p['warning']:
warning = True
latitude_blackout_zone = True
#print("WARNING black out zone!", p)

except:
pass

# declination in radians * 10^-4
declination_int = constrain(int(round(declination_rad * 10000)), 32767, -32768)
print('{0:6d},'.format(declination_int), end='')
# build the world magnetic model dictionary
world_magnitude_model = {} # lat/lon dictionary with grid result

if (declination_rad > declination_max):
declination_max = declination_rad
declination_max_lat_lon = (p['latitude'], p['longitude'])

if (declination_rad < declination_min):
declination_min = declination_rad
declination_min_lat_lon = (p['latitude'], p['longitude'])

if latitude_blackout_zone:
print(' }, // WARNING! black out zone')
else:
print(' },')

print("};")

print('static constexpr float WMM_DECLINATION_MIN_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(declination_min, declination_min_lat_lon[0], declination_min_lat_lon[1]))
print('static constexpr float WMM_DECLINATION_MAX_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(declination_max, declination_max_lat_lon[0], declination_max_lat_lon[1]))
print("\n")


# Inclination
params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'i', 'resultFormat': 'json'})
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
params = urllib.parse.urlencode({'lat1': 0, 'lon1': 0, 'resultFormat': 'json'})
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfwmm?key=%s&%s" % (key, params))
data = json.loads(f.read())
print("// Magnetic inclination data in radians * 10^-4")
print('// Model: {},'.format(data['model']))
print('// Version: {},'.format(data['version']))
print('// Date: {},'.format(data['result'][0]['date']))
print('static constexpr const int16_t inclination_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {")
print('\t// LONGITUDE: ', end='')
for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
print('{0:6d},'.format(l), end='')
print('')

inclination_min = float('inf')
inclination_max = float('-inf')
inclination_min_lat_lon = ()
inclination_max_lat_lon = ()
world_magnitude_model_units = data['units']

for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES):
params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'i', 'resultFormat': 'json'})
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
data = json.loads(f.read())
world_magnitude_model[latitude] = {}

latitude_blackout_zone = False
for longitude in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
params = urllib.parse.urlencode({'lat1': latitude, 'lon1': longitude, 'lon2': SAMPLING_MAX_LON, 'resultFormat': 'json'})
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfwmm?key=%s&%s" % (key, params))
data = json.loads(f.read())
#print(json.dumps(data, indent = 4)) # debugging

print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
for p in data['result']:
inclination_rad = math.radians(p['inclination'])
world_magnitude_model[latitude][longitude] = data['result'][0]
#print(world_magnitude_model[latitude][longitude])

warning = False

try:
if p['warning']:
warning = True
latitude_blackout_zone = True
#print("WARNING black out zone!", p)
def print_wmm_table(key_name):

except:
pass
value_min = float('inf')
value_min_lat_lon = ()

# inclination in radians * 10^-4
inclination_int = constrain(int(round(inclination_rad * 10000)), 32767, -32768)
print('{0:6d},'.format(inclination_int), end='')
value_max = float('-inf')
value_max_lat_lon = ()

if (inclination_rad > inclination_max):
inclination_max = inclination_rad
inclination_max_lat_lon = (p['latitude'], p['longitude'])
for latitude, lat_row in world_magnitude_model.items():
#print(latitude, lat_row)
for longitude, result in lat_row.items():
#print(result)

if (inclination_rad < inclination_min):
inclination_min = inclination_rad
inclination_min_lat_lon = (p['latitude'], p['longitude'])
value = float(result[key_name])

if latitude_blackout_zone:
print(' }, // WARNING! black out zone')
else:
print(' },')
if (value > value_max):
value_max = value
value_max_lat_lon = (latitude, longitude)

print("};")
if (value < value_min):
value_min = value
value_min_lat_lon = (latitude, longitude)

print('static constexpr float WMM_INCLINATION_MIN_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(inclination_min, inclination_min_lat_lon[0], inclination_min_lat_lon[1]))
print('static constexpr float WMM_INCLINATION_MAX_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(inclination_max, inclination_max_lat_lon[0], inclination_max_lat_lon[1]))
print("\n")
# scale the values to fit into int16_t
value_scale_max = abs(numpy.iinfo(numpy.int16).max) / abs(value_max)
value_scale_min = abs(numpy.iinfo(numpy.int16).min) / abs(value_min)
value_scale = min(value_scale_max, value_scale_min)

# total intensity
params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'i', 'resultFormat': 'json'})
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
data = json.loads(f.read())
print("// Magnetic strength data in milli-Gauss * 10")
print('// Model: {},'.format(data['model']))
print('// Version: {},'.format(data['version']))
print('// Date: {},'.format(data['result'][0]['date']))
print('static constexpr const int16_t strength_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {")
print('\t// LONGITUDE: ', end='')
for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
print('{0:5d},'.format(l), end='')
print('')
units_str = world_magnitude_model_units[key_name].split(' ')[0]

strength_min = float('inf')
strength_max = 0
strength_min_lat_lon = ()
strength_max_lat_lon = ()
strength_sum = 0
strength_sum_count = 0
# print the table
print('// Magnetic {} data in {:.4g} {}'.format(key_name, 1.0 / value_scale, units_str))
print('// Model: {},'.format(data['model']))
print('// Version: {},'.format(data['version']))
print('// Date: {},'.format(data['result'][0]['date']))
print('static constexpr const int16_t {}_table[{}][{}]'.format(key_name, LAT_DIM, LON_DIM) + " {")
print('\t// LONGITUDE: ', end='')
for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES):
print('{0:6d},'.format(l), end='')
print('')

strength_list = []
for latitude, lat_row in world_magnitude_model.items():
print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
latitude_blackout_zone = False

for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES):
params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'f', 'resultFormat': 'json'})
f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params))
data = json.loads(f.read())
for longitude, result in lat_row.items():

value = float(result[key_name])

# value scaled to fit into int16_t
value_int = int(round(value * value_scale))
print('{0:6d},'.format(value_int), end='')

print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='')
for p in data['result']:
# blackout warning at this latitude
try:
if result['warning']:
latitude_blackout_zone = True

strength_gauss = p['totalintensity'] * 1e-3
strength_list.append(strength_gauss)
except:
pass

totalintensity_int = int(round(p['totalintensity']/10))
print('{0:5d},'.format(totalintensity_int), end='')
if latitude_blackout_zone:
print(' }, // WARNING! black out zone')
else:
print(' },')

if (strength_gauss > strength_max):
strength_max = strength_gauss
strength_max_lat_lon = (p['latitude'], p['longitude'])
print("};")

if (strength_gauss < strength_min):
strength_min = strength_gauss
strength_min_lat_lon = (p['latitude'], p['longitude'])
print('static constexpr float WMM_{}_SCALE_TO_{} = {:.9g}f;'.format(key_name.upper(), units_str.upper(), 1.0 / value_scale))
print('static constexpr float WMM_{}_MIN_{} = {:.1f}f; // latitude: {:.0f}, longitude: {:.0f}'.format(key_name.upper(), units_str.upper(), value_min, value_min_lat_lon[0], value_min_lat_lon[1]))
print('static constexpr float WMM_{}_MAX_{} = {:.1f}f; // latitude: {:.0f}, longitude: {:.0f}'.format(key_name.upper(), units_str.upper(), value_max, value_max_lat_lon[0], value_max_lat_lon[1]))
print("\n")

print(' },')
print("};")

print('static constexpr float WMM_STRENGTH_MIN_GS = {:.1f}; // latitude: {:.0f}, longitude: {:.0f}'.format(strength_min, strength_min_lat_lon[0], strength_min_lat_lon[1]))
print('static constexpr float WMM_STRENGTH_MAX_GS = {:.1f}; // latitude: {:.0f}, longitude: {:.0f}'.format(strength_max, strength_max_lat_lon[0], strength_max_lat_lon[1]))
print('static constexpr float WMM_STRENGTH_MEAN_GS = {:.1f};'.format(statistics.mean(strength_list)))
print('static constexpr float WMM_STRENGTH_MEDIAN_GS = {:.1f};'.format(statistics.median(strength_list)))
print("\n")
print_wmm_table('declination')
print_wmm_table('inclination')
print_wmm_table('totalintensity')
2 changes: 1 addition & 1 deletion src/lib/world_magnetic_model/generate_gtest.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@

header = """/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
Expand Down
Loading
Loading