Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Update Geo Mag module and math Data to WMM 2015 data

Browse Source 
closes #1270
This commit is contained in:
Open UAS
2015-07-28 17:17:52 +02:00
committed by Felix Ruess
parent 0b7a6ebc95
commit 00c72cc66b
8 changed files with 73 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile
+1 -1
View File
@@ -65,7 +65,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_ivy_fixedwing.c \
$(NPSDIR)/nps_flightgear.c \
nps.srcs += math/pprz_geodetic_wmm2010.c
nps.srcs += math/pprz_geodetic_wmm2015.c
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
nps.srcs += subsystems/datalink/ivy_transport.c
conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+1 -1
View File
@@ -63,7 +63,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_ivy_mission_commands.c
# for geo mag calculation
nps.srcs += math/pprz_geodetic_wmm2010.c
nps.srcs += math/pprz_geodetic_wmm2015.c
include $(CFG_SHARED)/telemetry_ivy.makefile
nps.srcs += $(SRC_FIRMWARE)/rotorcraft_telemetry.c
conf/modules/geo_mag.xml
+13 -4
View File
@@ -4,8 +4,14 @@
<doc>
<description>
GeoMagnetic field vector.
Calculation of the normalized geomagnetic field vector (saved to ahrs_impl.mag_h) at startup using GPS fix.
Based on the WMM2010 model (http://www.ngdc.noaa.gov/geomag/models.shtml).
Calculation of the normalized geomagnetic field vector (saved to ahrs_impl.mag_h) at startup as soon as a valid location is aquired.
Method uses is based on the the ngdc noaa DODWMM model and data (http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml).
To obtain the a magnetic heading to be used by the autopilot, the program must know the magnetic declination at all the points en route, at the indicated flight altitude.
It's very important to keep declination into account because a flight in some geographical areas without considering declination, could lead to unacceptable autopilot navigation errors.
The magnetic declination, that is the difference between true heading and magnetic heading, varies in time and space according to undefined rules, whose long-term progress can be considered substantially random.
To determine the magnetic declination for a point through mathematical calculations is thus indispensable to resort to a model, which represents the progress of the earth magnetic field all over the surface of the globe. The WMM is based on earth magnetic field
measuring at an high number of sites on the whole globe and on its mathematical representation through a series of characteristic values listed in a file (WMM.COF) which has a five-year validity. The autopilot used data derived from this file to make the complex
calculation of declination. Every 5 years (2015, 2020) an updated geomagnetic model is released and datatables in the code must be updated accordingly for more accurate flight.
</description>
</doc>
<header>
@@ -14,11 +20,14 @@
<init fun="geo_mag_init()"/>
<periodic fun="geo_mag_periodic()" freq="1"/>
<event fun="geo_mag_event()"/>
<makefile target="ap|nps">
<makefile target="ap|nps|sim">
<file name="geo_mag.c"/>
<file name="pprz_geodetic_wmm2010.c" dir="math"/>
<file name="pprz_geodetic_wmm2015.c" dir="math"/>
</makefile>
<makefile target="nps">
<define name="NPS_CALC_GEO_MAG"/>
</makefile>
<makefile target="sim">
<define name="SIM_CALC_GEO_MAG"/>
</makefile>
</module>
sw/airborne/math/pprz_geodetic_wmm2010.c → sw/airborne/math/pprz_geodetic_wmm2015.c
+33 -30
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
* Copyright (C) 2015 OpenUAS info@openuas.org>
*
* This file is part of paparazzi.
*
@@ -19,43 +20,45 @@
*/
/**
* @file pprz_geodetic_wmm2010.c
* @brief WMM2010 Geomagnetic field model.
* @file pprz_geodetic_wmm2015.c
* @brief WMM2015 Geomagnetic field model.
*
* Based on the WMM2010 model (http://www.ngdc.noaa.gov/geomag/models.shtml)
* Based on the WMM2015 model (http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml)
*/
#include "std.h"
#include "math/pprz_geodetic_wmm2010.h"
#include "math/pprz_geodetic_wmm2015.h"
const double gh1[MAXCOEFF] = {
0.0, -29496.6, -1586.3, 4944.4,
-2396.6, 3026.1, -2707.7, 1668.6, -576.1,
1340.1, -2326.2, -160.2, 1231.9, 251.9, 634, -536.6,
912.6, 808.9, 286.4, 166.7, -211.2, -357.1, 164.3, 89.4, -309.1,
-230.9, 357.2, 44.6, 200.3, 188.9, -141.1, -118.2, -163.0, 0.0, -7.8, 100.9,
72.8, 68.6, -20.8, 76.0, 44.1, -141.4, 61.5, -22.8, -66.3, 13.2, 3.1, -77.9, 55.0,
80.5, -75.1, -57.9, -4.7, -21.1, 45.3, 6.5, 13.9, 24.9, 10.4, 7.0, 1.7, -27.7, 4.9, -3.3,
24.4, 8.1, 11.0, -14.5, -20.0, -5.6, 11.9, -19.3, -17.4, 11.5, 16.7, 10.9, 7.0, -14.1, -10.8, -3.7, 1.7,
5.4, 9.4, -20.5, 3.4, 11.5, -5.2, 12.8, 3.1, -7.2, -12.4, -7.4, -0.7, 8.0, 8.4, 2.1, -8.5, -6.1, -10.1, 7.0,
-2.0, -6.3, 2.8, 0.9, -0.1, -1.1, 4.7, -0.2, 4.4, 2.5, -7.2, -0.3, -1.0, 2.2, -3.9, 3.1, -2.0, -1.0, -2.0, -2.8, -8.3,
3.0, -1.5, 0.2, -2.1, 1.7, 1.7, -0.6, -0.5, -1.8, 0.5, 0.9, -0.8, -0.4, 0.4, -2.5, 1.8, -1.3, 0.1, -2.1, 0.7, -1.9, 3.8, -1.8,
-2.2, -0.2, -0.9, 0.3, 0.3, 1.0, 2.1, -0.6, -2.5, 0.9, 0.5, -0.1, 0.6, 0.5, 0.0, -0.4, 0.1, -0.4, 0.3, 0.2, -0.9, -0.8, -0.2, 0.0, 0.9
//WMM 2015 data
0.0, -29438.5, -1501.1, 4796.2,
-2445.3, 3012.5, -2845.6, 1676.6, -642.0,
1351.1, -2352.3, -115.3, 1225.6, 245.0, 581.9, -538.3,
907.2, 813.7, 283.4, 120.3, -188.6, -335.0, 180.9, 70.3, -329.5,
-232.6, 360.1, 47.4, 192.4, 196.9, -141.0, -119.4, -157.4, 16.1, 4.3, 100.1,
69.5, 67.4, -20.7, 72.8, 33.2, -129.8, 58.8, -29.0, -66.5, 13.2, 7.3, -70.9, 62.5,
81.6, -76.1, -54.1, -6.8, -19.4, 51.9, 5.6, 15.0, 24.4, 9.3, 3.3, -2.8, -27.5, 6.7, -2.3,
24.0, 8.6, 10.2, -16.9, -18.1, -3.2, 13.2, -20.6, -14.6, 13.3, 16.2, 11.7, 5.7, -16.0, -9.1, -2.0, 2.2,
5.4, 8.8, -21.6, 3.1, 10.8, -3.1, 11.7, 0.6, -6.8, -13.3, -6.9, -0.1, 7.8, 8.7, 1.0, -9.1, -3.9, -10.5, 8.5,
-1.9, -6.5, 3.3, 0.2, -0.3, 0.6, 4.6, -0.6, 4.4, 1.7, -7.9, -0.7, -0.6, 2.1, -4.1, 2.3, -2.8, -1.8, -1.1, -3.6, -8.7,
3.1, -1.5, -0.1, -2.3, 2.1, 2.1, -0.7, -0.9, -1.1, 0.6, 0.7, -0.7, -0.2, 0.2, -2.1, 1.7, -1.5, -0.2, -2.5, 0.4, -2.0, 3.5, -2.3,
-2.0, -0.3, -1.0, 0.4, 0.5, 1.3, 1.8, -0.9, -2.2, 0.9, 0.3, 0.1, 0.7, 0.5, -0.1, -0.4, 0.3, -0.4, 0.2, 0.2, -0.9, -0.9, -0.2, 0.0, 0.7
};
const double gh2[MAXCOEFF] = {
0.0, 11.6, 16.5, -25.9,
-12.1, -4.4, -22.5, 1.9, -11.8,
0.4, -4.1, 7.3, -2.9, -3.9, -7.7, -2.6,
-1.8, 2.3, 1.1, -8.7, 2.7, 4.6, 3.9, -2.1, -0.8,
-1.0, 0.6, 0.4, -1.8, 1.8, -1.0, 1.2, 0.9, 4.0, 1.0, -0.6,
-0.2, -0.2, -0.2, -0.1, -2.1, 2.0, -0.4, -1.7, -0.6, -0.3, 0.5, 1.7, 0.9,
0.1, -0.1, 0.7, -0.6, 0.3, 1.3, -0.1, 0.4, -0.1, 0.3, -0.8, -0.7, -0.3, 0.6, 0.3,
-0.1, 0.1, -0.1, -0.6, 0.2, 0.2, 0.4, -0.2, 0.4, 0.3, 0.1, 0.3, -0.1, -0.6, 0.4, 0.2, 0.3,
0.0, -0.1, 0.0, 0.0, -0.2, 0.3, 0.0, -0.4, -0.1, -0.3, 0.1, 0.1, 0.0, -0.1, -0.2, -0.4, 0.3, -0.2, 0.2,
0.0, 0.0, 0.1, -0.1, -0.1, 0.2, 0.0, 0.0, -0.1, -0.1, -0.1, -0.2, 0.0, 0.0, -0.1, -0.1, -0.2, -0.2, 0.0, -0.2, -0.1,
0.0, 0.0, 0.0, 0.0, 0.1, 0.1, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, -0.1, 0.0, -0.1, -0.1, 0.0, 0.0, -0.1,
0.0, 0.0, 0.0, 0.1, 0.0, 0.1, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.0, 0.1, 0.0
//WMM 2015 data
0.0, 10.7, 17.9, -26.8,
-8.6, 0.0, -3.3, -27.1, 2.4, -13.3,
3.1, -6.2, 8.4, -0.4, -0.4, -10.4, 2.3,
-0.4, 0.8, -0.6, -9.2, 5.3, 4.0, 3.0, -4.2, -5.3,
-0.2, 0.1, 0.4, -1.4, 1.6, 0.0, -1.1, 1.3, 3.3, 3.8, 0.1,
-0.5, -0.2, 0.0, -0.6, -2.2, 2.4, -0.7, -1.1, 0.1, 0.3, 1.0, 1.5, 1.3,
0.2, -0.2, 0.7, -0.4, 0.5, 1.3, -0.2, 0.2, -0.1, -0.4, -0.7, -0.9, 0.1, 0.3, 0.1,
0.0, 0.1, -0.3, -0.5, 0.3, 0.5, 0.3, -0.2, 0.6, 0.4, -0.1, 0.2, -0.2, -0.4, 0.3, 0.3, 0.0,
0.0, -0.1, -0.2, -0.1, -0.1, 0.4, -0.2, -0.5, 0.1, -0.2, 0.1, 0.1, 0.0, 0.0, -0.2, -0.2, 0.4, -0.1, 0.3,
0.0, 0.0, 0.1, -0.1, -0.1, 0.3, 0.0, -0.1, 0.0, -0.1, -0.2, -0.1, 0.1, 0.0, -0.1, -0.2, -0.2, -0.1, 0.1, -0.2, -0.1,
0.0, 0.0, 0.0, -0.1, 0.1, 0.1, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, -0.1, -0.1, 0.0, -0.1, -0.1,
0.1, 0.0, 0.0, 0.0, 0.0, 0.1, -0.1, -0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
};
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
@@ -137,10 +140,10 @@ int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t n
argument = flat * dtr;
slat = sinf(argument);
if ((90.0 - flat) < 0.001) {
aa = 89.999; /* 300 ft. from North pole */
aa = 89.999; /* in meters from North pole */
} else {
if ((90.0 + flat) < 0.001) {
aa = -89.999; /* 300 ft. from South pole */
aa = -89.999; /* in meters from South pole */
} else {
aa = flat;
}
sw/airborne/math/pprz_geodetic_wmm2010.h → sw/airborne/math/pprz_geodetic_wmm2015.h
+13 -11
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
* Copyright (C) 2015 OpenUAS <info@openuas.org>
*
* This file is part of paparazzi.
*
@@ -19,10 +20,10 @@
*/
/**
* @file pprz_geodetic_wmm2010.h
* @brief WMM2010 Geomagnetic field model.
* @file pprz_geodetic_wmm2015.h
* @brief WMM2015 Geomagnetic field model.
*
* Based on the WMM2010 model (http://www.ngdc.noaa.gov/geomag/models.shtml)
* Based on the WMM2015 model (http://www.ngdc.noaa.gov/geomag/models.shtml)
*
* @addtogroup math_geodetic
* @{
@@ -30,20 +31,20 @@
* @{
*/
#ifndef WMM2010_H
#define WMM2010_H
#ifndef WMM2015_H
#define WMM2015_H
#ifdef __cplusplus
extern "C" {
#endif
#define WMM2010_FRAC 2
#define WMM2015_FRAC 2
#define N_MAX_OF_GH 12
// Geo mag current observation epoch begin
#define GEO_EPOCH 2010.
#define YR_MIN 2010.
#define YR_MAX 2015.
#define GEO_EPOCH 2015.
#define YR_MIN 2015.
#define YR_MAX 2020.
#define NMAX_1 12
#define NMAX_2 12
@@ -58,7 +59,8 @@ extern "C" {
#define GPS_EPOCH_MONTH 1
#define GPS_EPOCH_DAY 6
#define WEEKS_IN_YEAR 52.143
#define WEEKS_IN_YEAR 52.14285
#define SECS_IN_YEAR 31536000
#define MAXDEG 13
@@ -76,6 +78,6 @@ int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t n
} /* extern "C" */
#endif
#endif /* WMM2010_H */
#endif /* WMM2015_H */
/** @}*/
/** @}*/
sw/airborne/modules/geo_mag/geo_mag.c
+8 -5
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
* Copyright (C) 2015 OpenUAS <info@openuas.org>
*
* This file is part of paparazzi.
*
@@ -21,17 +22,18 @@
/**
* @file modules/geo_mag/geo_mag.c
* @brief Calculation of the Geomagnetic field vector from current GPS fix.
* This module is based on the WMM2010 model (http://www.ngdc.noaa.gov/geomag/models.shtml).
* @brief Calculation of the Geomagnetic field vector from current location.
* This module is based on the WMM2015 model (http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml).
*/
#include "modules/geo_mag/geo_mag.h"
#include "math/pprz_geodetic_wmm2010.h"
#include "math/pprz_geodetic_wmm2015.h"
#include "math/pprz_algebra_double.h"
#include "subsystems/gps.h"
#include "subsystems/abi.h"
// for kill_throttle check
//FIXME: should not be in this spot
//for kill_throttle check
#include "autopilot.h"
#ifndef GEO_MAG_SENDER_ID
@@ -49,6 +51,7 @@ void geo_mag_init(void)
void geo_mag_periodic(void)
{
//FIXME: kill_throttle has no place in a geomag module
if (!geo_mag.ready && gps.fix == GPS_FIX_3D && kill_throttle) {
geo_mag_calc_flag = TRUE;
}
@@ -61,7 +64,7 @@ void geo_mag_event(void)
double gha[MAXCOEFF]; // Geomag global variables
int32_t nmax;
/* Current date in decimal year, for example 2012.68 */
/* Current date in decimal year, for example 2015.68 */
double sdate = GPS_EPOCH_BEGIN +
(double)gps.week / WEEKS_IN_YEAR +
(double)gps.tow / 1000 / SECS_IN_YEAR;
sw/airborne/modules/geo_mag/geo_mag.h
+3 -2
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
* Copyright (C) 2015 OpenUAS <info@openuas.org>
*
* This file is part of paparazzi.
*
@@ -21,8 +22,8 @@
/**
* @file modules/geo_mag/geo_mag.h
* @brief Calculation of the Geomagnetic field vector from current GPS fix.
* This module is based on the WMM2010 model (http://www.ngdc.noaa.gov/geomag/models.shtml).
* @brief Calculation of the Geomagnetic field vector from current location.
* This module is based on the WMM2015 model (http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml).
*/
#ifndef GEO_MAG_H
sw/simulator/nps/nps_fdm_jsbsim.cpp
+1 -1
View File
@@ -51,7 +51,7 @@
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_wmm2010.h"
#include "math/pprz_geodetic_wmm2015.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"