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 new file mode 100755 index 0000000000..5a8df52553 --- /dev/null +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml @@ -0,0 +1,221 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/modules/geo_mag.xml b/conf/modules/geo_mag.xml new file mode 100755 index 0000000000..bcae9b0c48 --- /dev/null +++ b/conf/modules/geo_mag.xml @@ -0,0 +1,20 @@ + + + + + +
+ +
+ + + + + + + + + diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h index d712ae2f5f..cdcc260162 100644 --- a/sw/airborne/math/pprz_algebra_double.h +++ b/sw/airborne/math/pprz_algebra_double.h @@ -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; \ diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c new file mode 100755 index 0000000000..759f44794b --- /dev/null +++ b/sw/airborne/modules/geo_mag/geo_mag.c @@ -0,0 +1,49 @@ +/* + * + * Copyright (C) 2012 Sergey Krukowski . + * + * 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; +} diff --git a/sw/airborne/modules/geo_mag/geo_mag.h b/sw/airborne/modules/geo_mag/geo_mag.h new file mode 100755 index 0000000000..b0a17b56b4 --- /dev/null +++ b/sw/airborne/modules/geo_mag/geo_mag.h @@ -0,0 +1,28 @@ +/* + * + * Copyright (C) 2012 Sergey Krukowski . + * + * 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 diff --git a/sw/airborne/modules/geo_mag/wmm2010.c b/sw/airborne/modules/geo_mag/wmm2010.c new file mode 100755 index 0000000000..e1d83c592d --- /dev/null +++ b/sw/airborne/modules/geo_mag/wmm2010.c @@ -0,0 +1,240 @@ +/* + * + * Copyright (C) 2012 Sergey Krukowski . + * + * 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); +} diff --git a/sw/airborne/modules/geo_mag/wmm2010.h b/sw/airborne/modules/geo_mag/wmm2010.h new file mode 100755 index 0000000000..6448ba690a --- /dev/null +++ b/sw/airborne/modules/geo_mag/wmm2010.h @@ -0,0 +1,43 @@ +/* + * + * Copyright (C) 2012 Sergey Krukowski . + * + * 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 diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index c021a0832a..19c984b973 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -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); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index c9d3581794..82e37e5ae5 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -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.