Added WMM2010 Geo model

2026-05-31 20:38:27 +08:00 · 2012-07-22 11:24:58 -07:00
parent 2e87994d49
commit 640fc38099
9 changed files with 631 additions and 0 deletions
@@ -0,0 +1,221 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!-- this is a quadrotor frame equiped with
+     * Autopilot:   Lisa/M 2.0             http://paparazzi.enac.fr/wiki/Lisa/M_v20
+     * IMU:         Aspirin 2.1            http://paparazzi.enac.fr/wiki/AspirinIMU
+     * Actuators:   PWM motor controllers  http://paparazzi.enac.fr/wiki/Subsystem/actuators#PWM_Supervision
+     * GPS:         Ublox                  http://paparazzi.enac.fr/wiki/Subsystem/gps
+     * RC:          two Spektrum sats      http://paparazzi.enac.fr/wiki/Subsystem/radio_control#Spektrum
+-->
+
+<airframe name="Quadrotor LisaM_2.0 pwm">
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="lisa_m_2.0">
+      <subsystem name="radio_control" type="spektrum">
+        <define name="RADIO_MODE" value="RADIO_AUX1"/>
+        <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/>
+      </subsystem>
+      <!-- MPU600 is configured to output data at 500Hz -->
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
+    </target>
+
+    <target name="nps" board="pc">
+      <subsystem name="fdm" type="jsbsim"/>
+      <subsystem name="radio_control" type="ppm"/>
+    </target>
+
+    <subsystem name="actuators"     type="pwm_supervision">
+      <define name="SERVO_HZ" value="400"/>
+      <define name="USE_SERVOS_7AND8"/>
+    </subsystem>
+
+    <subsystem name="telemetry"     type="transparent"/>
+    <subsystem name="imu"           type="aspirin_v2.1"/>
+    <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization" type="int_quat"/>
+    <subsystem name="ahrs"          type="int_cmpl_quat"/>
+  </firmware>
+
+<!--									 Using Geo Mag module										 -->
+	<modules>
+		<load name="geo_mag.xml"/>
+	</modules>
+
+  <servos>
+    <servo name="FRONT"   no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="BACK"    no="1" min="1000" neutral="1000" max="2000"/>
+    <servo name="RIGHT"   no="2" min="1000" neutral="1000" max="2000"/>
+    <servo name="LEFT"    no="3" min="1000" neutral="1000" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+
+  <command_laws>
+    <!-- command_laws is needed for pwm_supervision -->
+    <!-- but can be empty if no additional servos are used -->
+  </command_laws>
+
+  <section name="SUPERVISION" prefix="SUPERVISION_">
+    <define name="STOP_MOTOR" value="1000"/>
+    <define name="MIN_MOTOR"  value="1100"/>
+    <define name="MAX_MOTOR"  value="1900"/>
+    <define name="TRIM_A" value="0"/>
+    <define name="TRIM_E" value="0"/>
+    <define name="TRIM_R" value="0"/>
+    <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
+    <!-- front/back turning CW, right/left CCW -->
+    <define name="ROLL_COEF"   value="{    0,    0, -256,  256 }"/>
+    <define name="PITCH_COEF"  value="{  256, -256,    0,    0 }"/>
+    <define name="YAW_COEF"    value="{ -256, -256,  256,  256 }"/>
+    <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+    <define name="ACCEL_X_NEUTRAL" value="11"/>
+    <define name="ACCEL_Y_NEUTRAL" value="11"/>
+    <define name="ACCEL_Z_NEUTRAL" value="-25"/>
+
+    <!-- replace this with your own calibration -->
+    <define name="MAG_X_NEUTRAL" value="-152"/>
+    <define name="MAG_Y_NEUTRAL" value="-51"/>
+    <define name="MAG_Z_NEUTRAL" value="10"/>
+    <define name="MAG_X_SENS" value="4.04042714046" integer="16"/>
+    <define name="MAG_Y_SENS" value="3.95350991963" integer="16"/>
+    <define name="MAG_Z_SENS" value="3.83055079257" integer="16"/>
+
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-45." unit="deg"/>
+  </section>
+
+  <section name="AHRS" prefix="AHRS_">
+    <define name="H_X" value="0.3770441"/>
+    <define name="H_Y" value="0.0193986"/>
+    <define name="H_Z" value="0.9259921"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="BARO_SENS" value="22.3" integer="16"/>
+  </section>
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_P" value="10000"/>
+    <define name="SP_MAX_Q" value="10000"/>
+    <define name="SP_MAX_R" value="10000"/>
+    <define name="DEADBAND_P" value="20"/>
+    <define name="DEADBAND_Q" value="20"/>
+    <define name="DEADBAND_R" value="200"/>
+    <define name="REF_TAU" value="4"/>
+
+    <!-- feedback -->
+    <define name="GAIN_P" value="400"/>
+    <define name="GAIN_Q" value="400"/>
+    <define name="GAIN_R" value="350"/>
+
+    <define name="IGAIN_P" value="75"/>
+    <define name="IGAIN_Q" value="75"/>
+    <define name="IGAIN_R" value="50"/>
+
+    <!-- feedforward -->
+    <define name="DDGAIN_P" value="300"/>
+    <define name="DDGAIN_Q" value="300"/>
+    <define name="DDGAIN_R" value="300"/>
+  </section>
+
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
+    <define name="DEADBAND_A"     value="0"/>
+    <define name="DEADBAND_E"     value="0"/>
+    <define name="DEADBAND_R"     value="250"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="1000"/>
+    <define name="PHI_DGAIN"  value="400"/>
+    <define name="PHI_IGAIN"  value="200"/>
+
+    <define name="THETA_PGAIN"  value="1000"/>
+    <define name="THETA_DGAIN"  value="400"/>
+    <define name="THETA_IGAIN"  value="200"/>
+
+    <define name="PSI_PGAIN"  value="500"/>
+    <define name="PSI_DGAIN"  value="300"/>
+    <define name="PSI_IGAIN"  value="10"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value="300"/>
+    <define name="THETA_DDGAIN" value="300"/>
+    <define name="PSI_DDGAIN"   value="300"/>
+  </section>
+
+  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+    <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+    <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+    <define name="MAX_SUM_ERR" value="2000000"/>
+    <define name="HOVER_KP"    value="150"/>
+    <define name="HOVER_KD"    value="80"/>
+    <define name="HOVER_KI"    value="20"/>
+    <!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
+    <define name="RC_CLIMB_COEF" value ="163"/>
+    <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
+    <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
+    <!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive estimation -->
+    <!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/-->
+  </section>
+
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="PGAIN" value="100"/>
+    <define name="DGAIN" value="100"/>
+    <define name="IGAIN" value="0"/>
+  </section>
+
+  <section name="SIMULATOR" prefix="NPS_">
+    <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
+    <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
+    <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
+  </section>
+
+  <section name="AUTOPILOT">
+    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+    <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
+    <define name="MODE_AUTO2"  value="AP_MODE_HOVER_Z_HOLD"/>
+  </section>
+
+  <section name="BAT">
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
+    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
+  </section>
+
+</airframe>
@@ -0,0 +1,20 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<!--
+     Geo Mag modell
+		 by Sergey Krukowski
+     -->
+
+<module name="geo_mag" dir="geo_mag">
+  <header>
+    <file name="geo_mag.h"/>
+  </header>
+  <init fun="geo_mag_init()"/>
+  <periodic fun="geo_mag_periodic()" freq="1"/>
+  <event fun="geo_mag_event()"/><!---->
+  <makefile>
+    <file name="geo_mag.c"/>
+		<file name="wmm2010.c"/>
+		<define name="USE_GEO_MAG"/>
+  </makefile>
+</module>
@@ -92,6 +92,10 @@ struct DoubleRates {
    (_vout).z = rint((_vin).z);			\
  }

+#define DOUBLE_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
+
+#define DOUBLE_VECT3_COPY(_a, _b) VECT3_COPY(_a, _b)
+
 #define DOUBLE_VECT3_SUM(_c,_a,_b) {            \
    (_c).x = (_a).x + (_b).x;			\
    (_c).y = (_a).y + (_b).y;			\
@@ -0,0 +1,49 @@
+/*
+ *
+ * Copyright (C) 2012  Sergey Krukowski <softsr@yahoo.de>.
+ *
+ * This module based on the WMM2010 modell (http://www.ngdc.noaa.gov/geomag/models.shtml).
+ *
+ */
+
+#include "modules/geo_mag/geo_mag.h"
+#include "modules/geo_mag/wmm2010.h"
+#include "math/pprz_algebra_double.h"
+#include "subsystems/gps.h"
+#include "autopilot.h"
+#include "subsystems/ahrs/ahrs_int_cmpl_quat.h" // in AHRS subsystem ahrs_h is DoubleVect3 variable
+
+bool_t calc_flag;
+struct GeoMagVect geo_mag_vect;
+
+void geo_mag_init(void) {
+  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;
+}
+
+void geo_mag_event(void) {
+
+  if(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
+    geo_mag_vect.ready = TRUE;
+
+  }
+  calc_flag = FALSE;
+}
@@ -0,0 +1,28 @@
+/*
+ *
+ * Copyright (C) 2012  Sergey Krukowski <softsr@yahoo.de>.
+ *
+ * This module based on the WMM2010 modell (http://www.ngdc.noaa.gov/geomag/models.shtml).
+ *
+ */
+
+#ifndef GEO_MAG_H
+#define GEO_MAG_H
+
+#include "std.h"
+#include "wmm2010.h"
+
+struct GeoMagVect {
+  double x;
+  double y;
+  double z;
+  bool_t ready;
+};
+
+extern void geo_mag_init(void);
+extern void geo_mag_periodic(void);
+extern void geo_mag_event(void);
+
+extern struct GeoMagVect geo_mag_vect;
+
+#endif
@@ -0,0 +1,240 @@
+/*
+ *
+ * Copyright (C) 2012  Sergey Krukowski <softsr@yahoo.de>.
+ *
+ * This module based on the WMM2010 modell (http://www.ngdc.noaa.gov/geomag/models.shtml).
+ *
+ */
+
+#include "std.h"
+#include "wmm2010.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
+};
+
+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
+};
+
+int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh) {
+
+  int16_t   nmax;
+  int16_t   k, l;
+  int16_t   ii;
+  double factor;
+
+  factor = date - dte1;
+  if (nmax1 == nmax2) {
+      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) {
+        gh[ii] = gh1[ii];
+      }
+
+      nmax = nmax1;
+    }
+    else {
+      k = nmax1 * (nmax1 + 2);
+      l = nmax2 * (nmax2 + 2);
+
+		  for ( ii = k + 1; ii <= l; ++ii) {
+        gh[ii] = factor * gh2[ii];
+      }
+
+      nmax = nmax2;
+    }
+  }
+
+  for ( ii = 1; ii <= k; ++ii) {
+    gh[ii] = gh1[ii] + factor * gh2[ii];
+  }
+
+  return(nmax);
+}
+
+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) {
+
+  double earths_radius = 6371.2;
+  double dtr = 0.01745329;
+  double slat;
+  double clat;
+  double ratio;
+  double aa, bb, cc, dd;
+  double sd;
+  double cd;
+  double r;
+  double a2;
+  double b2;
+  double rr = 0;
+  double fm,fn = 0;
+  double sl[14];
+  double cl[14];
+#ifdef GEO_MAG_DOUBLE
+	double p[119];
+  double q[119];
+#else
+  float p[119];
+  float q[119];
+#endif
+  int ii,j,k,l,m,n;
+  int npq;
+  int ios;
+  double argument;
+  double power;
+  a2 = 40680631.59;            /* WGS84 */
+  b2 = 40408299.98;            /* WGS84 */
+  ios = 0;
+  r = elev;
+  argument = flat * dtr;
+  slat = sinf( argument );
+  if ((90.0 - flat) < 0.001) {
+      aa = 89.999;            /*  300 ft. from North pole  */
+  }
+  else {
+    if ((90.0 + flat) < 0.001) {
+      aa = -89.999;        /*  300 ft. from South pole  */
+    }
+    else {
+      aa = flat;
+    }
+  }
+  argument = aa * dtr;
+  clat = cosf( argument );
+  argument = flon * dtr;
+  sl[1] = sinf( argument );
+  cl[1] = cosf( argument );
+
+  *geo_mag_x = 0;
+  *geo_mag_y = 0;
+  *geo_mag_z = 0;
+
+  sd = 0.0;
+  cd = 1.0;
+  l = 1;
+  n = 0;
+  m = 1;
+  npq = (nmax * (nmax + 3)) / 2;
+
+  if (igdgc == 1) {
+    aa = a2 * clat * clat;
+    bb = b2 * slat * slat;
+    cc = aa + bb;
+    argument = cc;
+    dd = sqrt( argument );
+    argument = elev * (elev + 2.0 * dd) + (a2 * aa + b2 * bb) / cc;
+    r = sqrt( argument );
+    cd = (elev + dd) / r;
+    sd = (a2 - b2) / dd * slat * clat / r;
+    aa = slat;
+    slat = slat * cd - clat * sd;
+    clat = clat * cd + aa * sd;
+  }
+
+  ratio = earths_radius / r;
+  argument = 3.0;
+  aa = sqrt( argument );
+  p[1] = 2.0 * slat;
+  p[2] = 2.0 * clat;
+  p[3] = 4.5 * slat * slat - 1.5;
+  p[4] = 3.0 * aa * clat * slat;
+  q[1] = -clat;
+  q[2] = slat;
+  q[3] = -3.0 * clat * slat;
+  q[4] = aa * (slat * slat - clat * clat);
+
+  for ( k = 1; k <= npq; ++k) {
+    if (n < m) {
+      m = 0;
+      n = n + 1;
+      argument = ratio;
+      power =  n + 2;
+      rr = pow(argument,power);
+      fn = n;
+    }
+    fm = m;
+    if (k >= 5) {
+      if (m == n) {
+        argument = (1.0 - 0.5/fm);
+        aa = sqrt( argument );
+        j = k - n - 1;
+        p[k] = (1.0 + 1.0/fm) * aa * clat * p[j];
+        q[k] = aa * (clat * q[j] + slat/fm * p[j]);
+        sl[m] = sl[m-1] * cl[1] + cl[m-1] * sl[1];
+        cl[m] = cl[m-1] * cl[1] - sl[m-1] * sl[1];
+      }
+      else {
+        argument = fn*fn - fm*fm;
+        aa = sqrt( argument );
+        argument = ((fn - 1.0)*(fn-1.0)) - (fm * fm);
+        bb = sqrt( argument )/aa;
+        cc = (2.0 * fn - 1.0)/aa;
+        ii = k - n;
+        j = k - 2 * n + 1;
+        p[k] = (fn + 1.0) * (cc * slat/fn * p[ii] - bb/(fn - 1.0) * p[j]);
+        q[k] = cc * (slat * q[ii] - clat/fn * p[ii]) - bb * q[j];
+      }
+    }
+
+    aa = rr * gh[l];
+
+    if (m == 0) {
+			*geo_mag_x = *geo_mag_x + aa * q[k];
+      *geo_mag_z = *geo_mag_z - aa * p[k];
+      l = l + 1;
+    }
+    else {
+      bb = rr * gh[l+1];
+      cc = aa * cl[m] + bb * sl[m];
+      *geo_mag_x = *geo_mag_x + cc * q[k];
+      *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);
+      }
+      else {
+        *geo_mag_y = *geo_mag_y + (aa * sl[m] - bb * cl[m]) * q[k] * slat;
+      }
+      l = l + 2;
+    }
+    m = m + 1;
+  }
+  if (iext != 0) {
+		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;
+  return(ios);
+}
@@ -0,0 +1,43 @@
+/*
+ *
+ * Copyright (C) 2012  Sergey Krukowski <softsr@yahoo.de>.
+ *
+ * This module based on the WMM2010 modell (http://www.ngdc.noaa.gov/geomag/models.shtml).
+ *
+ */
+
+#ifndef WMM2010_H
+#define WMM2010_H
+
+#define WMM2010_FRAC 	12
+#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
+
+#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
+
+#define WEEKS_IN_YEAR 			52.143
+#define SECS_IN_YEAR 				31536000
+
+#define MAXDEG 13
+#define MAXCOEFF (MAXDEG*(MAXDEG+2)+1)
+
+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);
+
+#endif
@@ -32,6 +32,9 @@
 #endif
 #include "math/pprz_trig_int.h"
 #include "math/pprz_algebra_int.h"
+#ifdef USE_GEO_MAG
+#include "math/pprz_algebra_double.h"
+#endif

 #include "generated/airframe.h"

@@ -68,6 +71,10 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;

 static inline void set_body_state_from_quat(void);

+#ifdef USE_GEO_MAG
+struct DoubleVect3 ahrs_h;
+#endif
+
 void ahrs_init(void) {

  ahrs.status = AHRS_UNINIT;
@@ -94,6 +101,10 @@ void ahrs_init(void) {
  ahrs_impl.use_gravity_heuristic = FALSE;
 #endif

+#ifdef USE_GEO_MAG
+  VECT3_ASSIGN(ahrs_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
+#endif
+
 }

 void ahrs_align(void) {
@@ -256,9 +267,15 @@ static inline void ahrs_update_mag_full(void) {

  struct Int32RMat ltp_to_imu_rmat;
  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
+#ifndef USE_GEO_MAG
  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
                                          MAG_BFP_OF_REAL(AHRS_H_Y),
                                          MAG_BFP_OF_REAL(AHRS_H_Z)};
+#else
+  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(ahrs_h.x),
+                                          MAG_BFP_OF_REAL(ahrs_h.y),
+                                          MAG_BFP_OF_REAL(ahrs_h.z)};
+#endif
  struct Int32Vect3 expected_imu;
  INT32_RMAT_VMULT(expected_imu, ltp_to_imu_rmat, expected_ltp);

@@ -284,8 +301,14 @@ static inline void ahrs_update_mag_2d(void) {

  struct Int32RMat ltp_to_imu_rmat;
  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
+#ifndef USE_GEO_MAG
  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
                                          MAG_BFP_OF_REAL(AHRS_H_Y)};
+#else
+  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(ahrs_h.x),
+                                          MAG_BFP_OF_REAL(ahrs_h.y)};
+#endif
+
  struct Int32Vect3 measured_ltp;
  INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag);

@@ -42,6 +42,9 @@ struct AhrsIntCmpl {
 };

 extern struct AhrsIntCmpl ahrs_impl;
+#ifdef USE_GEO_MAG
+extern struct DoubleVect3 ahrs_h;
+#endif


 /** Update yaw based on a heading measurement.