diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
index 5a8df52553..c89a281729 100755
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
@@ -35,12 +35,13 @@
+
-
-
-
-
+
+
+
+
diff --git a/conf/modules/geo_mag.xml b/conf/modules/geo_mag.xml
index bcae9b0c48..717ebd6fef 100755
--- a/conf/modules/geo_mag.xml
+++ b/conf/modules/geo_mag.xml
@@ -1,11 +1,12 @@
-
-
+
+
+ Geo Mag modell
+ by Sergey Kruskowski
+
+
@@ -14,7 +15,7 @@
-
-
+
+
diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c
index 759f44794b..39ee3a93ee 100755
--- a/sw/airborne/modules/geo_mag/geo_mag.c
+++ b/sw/airborne/modules/geo_mag/geo_mag.c
@@ -13,37 +13,44 @@
#include "autopilot.h"
#include "subsystems/ahrs/ahrs_int_cmpl_quat.h" // in AHRS subsystem ahrs_h is DoubleVect3 variable
-bool_t calc_flag;
+bool_t geo_mag_calc_flag;
struct GeoMagVect geo_mag_vect;
void geo_mag_init(void) {
- calc_flag = FALSE;
+ geo_mag_calc_flag = FALSE;
geo_mag_vect.ready = FALSE;
}
void geo_mag_periodic(void) {
if(gps.fix == GPS_FIX_3D && !geo_mag_vect.ready && !autopilot_motors_on)
- calc_flag = TRUE;
+ geo_mag_calc_flag = TRUE;
}
void geo_mag_event(void) {
- if(calc_flag) {
- double gha[MAXCOEFF]; // Geomag global variables
+ if(geo_mag_calc_flag) {
+ double gha[MAXCOEFF]; // Geomag global variables
int32_t nmax;
- double sdate = GPS_EPOCH_BEGIN + (double)gps.week/WEEKS_IN_YEAR + // Current date in decimal year, for example 2012.68
- (double)gps.tow/1000/SECS_IN_YEAR;
- double latitude = DegOfRad((double)gps.lla_pos.lat / 1e7); // LLA Position latitude in decimal degree
- double longitude = DegOfRad((double)gps.lla_pos.lon / 1e7); // LLA Position longtitude in decimal degree
- double alt = (double)gps.lla_pos.alt / 1e6; // LLA Altitude in km
- nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha); // Calculates additional coeffs
- mag_calc(1, latitude, longitude, alt, nmax, gha, &geo_mag_vect.x, &geo_mag_vect.y, &geo_mag_vect.z, // Calculates absolute magnet fields
- IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
- FLOAT_VECT3_NORMALIZE(geo_mag_vect); // Normalizes mag vector
- DOUBLE_VECT3_COPY(ahrs_h, geo_mag_vect); // Copy to ahrs_h Vector
+ /* Current date in decimal year, for example 2012.68 */
+ double sdate = GPS_EPOCH_BEGIN +
+ (double)gps.week/WEEKS_IN_YEAR +
+ (double)gps.tow/1000/SECS_IN_YEAR;
+
+ /* LLA Position in decimal degrees and altitude in km */
+ double latitude = DegOfRad((double)gps.lla_pos.lat / 1e7);
+ double longitude = DegOfRad((double)gps.lla_pos.lon / 1e7);
+ double alt = (double)gps.lla_pos.alt / 1e6;
+
+ // Calculates additional coeffs
+ nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
+ // Calculates absolute magnet fields
+ mag_calc(1, latitude, longitude, alt, nmax, gha,
+ &geo_mag_vect.x, &geo_mag_vect.y, &geo_mag_vect.z,
+ IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
+ FLOAT_VECT3_NORMALIZE(geo_mag_vect);
+ DOUBLE_VECT3_COPY(ahrs_h, geo_mag_vect);
geo_mag_vect.ready = TRUE;
-
}
- calc_flag = FALSE;
+ geo_mag_calc_flag = FALSE;
}
diff --git a/sw/airborne/modules/geo_mag/wmm2010.c b/sw/airborne/modules/geo_mag/wmm2010.c
index e1d83c592d..7ebbf83e3d 100755
--- a/sw/airborne/modules/geo_mag/wmm2010.c
+++ b/sw/airborne/modules/geo_mag/wmm2010.c
@@ -48,15 +48,15 @@ int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double
factor = date - dte1;
if (nmax1 == nmax2) {
- k = nmax1 * (nmax1 + 2);
- nmax = nmax1;
+ k = nmax1 * (nmax1 + 2);
+ nmax = nmax1;
}
else {
if (nmax1 > nmax2) {
k = nmax2 * (nmax2 + 2);
l = nmax1 * (nmax1 + 2);
- for ( ii = k + 1; ii <= l; ++ii) {
+ for ( ii = k + 1; ii <= l; ++ii) {
gh[ii] = gh1[ii];
}
@@ -66,7 +66,7 @@ int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double
k = nmax1 * (nmax1 + 2);
l = nmax2 * (nmax2 + 2);
- for ( ii = k + 1; ii <= l; ++ii) {
+ for ( ii = k + 1; ii <= l; ++ii) {
gh[ii] = factor * gh2[ii];
}
@@ -99,7 +99,7 @@ int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t n
double sl[14];
double cl[14];
#ifdef GEO_MAG_DOUBLE
- double p[119];
+ double p[119];
double q[119];
#else
float p[119];
@@ -117,7 +117,7 @@ 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; /* 300 ft. from North pole */
}
else {
if ((90.0 + flat) < 0.001) {
@@ -207,7 +207,7 @@ int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t n
aa = rr * gh[l];
if (m == 0) {
- *geo_mag_x = *geo_mag_x + aa * q[k];
+ *geo_mag_x = *geo_mag_x + aa * q[k];
*geo_mag_z = *geo_mag_z - aa * p[k];
l = l + 1;
}
@@ -218,7 +218,7 @@ int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t n
*geo_mag_z = *geo_mag_z - cc * p[k];
if (clat > 0) {
*geo_mag_y = *geo_mag_y + (aa * sl[m] - bb * cl[m]) *
- fm * p[k]/((fn + 1.0) * clat);
+ fm * p[k]/((fn + 1.0) * clat);
}
else {
*geo_mag_y = *geo_mag_y + (aa * sl[m] - bb * cl[m]) * q[k] * slat;
@@ -228,13 +228,13 @@ int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t n
m = m + 1;
}
if (iext != 0) {
- aa = ext2 * cl[1] + ext3 * sl[1];
+ aa = ext2 * cl[1] + ext3 * sl[1];
*geo_mag_x = *geo_mag_x - ext1 * clat + aa * slat;
*geo_mag_y = *geo_mag_y + ext2 * sl[1] - ext3 * cl[1];
*geo_mag_z = *geo_mag_z + ext1 * slat + aa * clat;
}
- aa = *geo_mag_x;
- *geo_mag_x = *geo_mag_x * cd + *geo_mag_z * sd;
- *geo_mag_z = *geo_mag_z * cd - aa * sd;
+ aa = *geo_mag_x;
+ *geo_mag_x = *geo_mag_x * cd + *geo_mag_z * sd;
+ *geo_mag_z = *geo_mag_z * cd - aa * sd;
return(ios);
}
diff --git a/sw/airborne/modules/geo_mag/wmm2010.h b/sw/airborne/modules/geo_mag/wmm2010.h
index 6448ba690a..3c0c623b00 100755
--- a/sw/airborne/modules/geo_mag/wmm2010.h
+++ b/sw/airborne/modules/geo_mag/wmm2010.h
@@ -9,27 +9,29 @@
#ifndef WMM2010_H
#define WMM2010_H
-#define WMM2010_FRAC 12
-#define N_MAX_OF_GH 12
+#define WMM2010_FRAC 2
+#define N_MAX_OF_GH 12
-#define GEO_EPOCH 2010. // Geo mag current observation epoch begin
-#define YR_MIN 2010.
-#define YR_MAX 2015.
-#define NMAX_1 12
-#define NMAX_2 12
+// Geo mag current observation epoch begin
+#define GEO_EPOCH 2010.
+#define YR_MIN 2010.
+#define YR_MAX 2015.
+#define NMAX_1 12
+#define NMAX_2 12
#define IEXT 0
#define EXT_COEFF1 (double)0
#define EXT_COEFF2 (double)0
#define EXT_COEFF3 (double)0
-#define GPS_EPOCH_BEGIN (double)1980.016393442623 // Begin of the GPS epoch
-#define GPS_EPOCH_YEAR 1980
-#define GPS_EPOCH_MONTH 1
-#define GPS_EPOCH_DAY 6
+/// Begin of the GPS epoch
+#define GPS_EPOCH_BEGIN (double)1980.016393442623
+#define GPS_EPOCH_YEAR 1980
+#define GPS_EPOCH_MONTH 1
+#define GPS_EPOCH_DAY 6
-#define WEEKS_IN_YEAR 52.143
-#define SECS_IN_YEAR 31536000
+#define WEEKS_IN_YEAR 52.143
+#define SECS_IN_YEAR 31536000
#define MAXDEG 13
#define MAXCOEFF (MAXDEG*(MAXDEG+2)+1)
@@ -38,6 +40,8 @@ extern const double gh1[];
extern const double gh2[];
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh);
-int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax, double *gh, double *geo_mag_x, double *geo_mag_y, double *geo_mag_z, int16_t iext, double ext1, double ext2, double ext3);
+int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax,
+ double *gh, double *geo_mag_x, double *geo_mag_y, double *geo_mag_z,
+ int16_t iext, double ext1, double ext2, double ext3);
#endif