Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
geo_mag.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
31 #include "subsystems/gps.h"
32 #include "autopilot.h"
33 
34 // in int_cmpl_quat implementation, mag_h is an Int32Vect3 with INT32_MAG_FRAC
36 
39 
40 void geo_mag_init(void) {
43 }
44 
45 void geo_mag_periodic(void) {
48 }
49 
50 void geo_mag_event(void) {
51 
52  if(geo_mag_calc_flag) {
53  double gha[MAXCOEFF]; // Geomag global variables
54  int32_t nmax;
55 
56  /* Current date in decimal year, for example 2012.68 */
57  double sdate = GPS_EPOCH_BEGIN +
58  (double)gps.week/WEEKS_IN_YEAR +
59  (double)gps.tow/1000/SECS_IN_YEAR;
60 
61  /* LLA Position in decimal degrees and altitude in km */
62  double latitude = DegOfRad((double)gps.lla_pos.lat / 1e7);
63  double longitude = DegOfRad((double)gps.lla_pos.lon / 1e7);
64  double alt = (double)gps.lla_pos.alt / 1e6;
65 
66  // Calculates additional coeffs
67  nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
68  // Calculates absolute magnet fields
69  mag_calc(1, latitude, longitude, alt, nmax, gha,
73 
74  // convert to MAG_BFP and copy to ahrs
76 
78  }
80 }
bool_t geo_mag_calc_flag
Definition: geo_mag.c:37
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
int32_t lat
in radians*1e7
#define GEO_EPOCH
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)
#define NMAX_1
#define MAXCOEFF
double z
Definition: geo_mag.h:36
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_FIX_3D
Definition: gps.h:42
bool_t ready
Definition: geo_mag.h:37
int16_t week
GPS week.
Definition: gps.h:78
#define WEEKS_IN_YEAR
#define EXT_COEFF2
#define FALSE
Definition: imu_chimu.h:141
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
bool_t autopilot_motors_on
Definition: autopilot.c:38
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
double y
Definition: geo_mag.h:35
#define MAG_BFP_OF_REAL(_af)
Device independent GPS code (interface)
double x
Definition: geo_mag.h:34
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct AhrsFloatCmplRmat ahrs_impl
#define EXT_COEFF1
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
#define EXT_COEFF3
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:99
struct GeoMagVect geo_mag_vect
Definition: geo_mag.c:38
void geo_mag_init(void)
Definition: geo_mag.c:40
int32_t lon
in radians*1e7
Calculation of the Geomagnetic field vector based on current GPS fix.
Paparazzi double precision floating point algebra.
#define FLOAT_VECT3_NORMALIZE(_v)
#define NMAX_2
#define GPS_EPOCH_BEGIN
Begin of the GPS epoch.
void geo_mag_event(void)
Definition: geo_mag.c:50
#define IEXT
struct GpsState gps
global GPS state
Definition: gps.c:31
#define SECS_IN_YEAR
void geo_mag_periodic(void)
Definition: geo_mag.c:45