Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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 #include "subsystems/ahrs.h"
35 
38 
39 void geo_mag_init(void) {
42 }
43 
44 void geo_mag_periodic(void) {
47 }
48 
49 void geo_mag_event(void) {
50 
51  if (geo_mag_calc_flag) {
52  double gha[MAXCOEFF]; // Geomag global variables
53  int32_t nmax;
54 
55  /* Current date in decimal year, for example 2012.68 */
56  double sdate = GPS_EPOCH_BEGIN +
57  (double)gps.week/WEEKS_IN_YEAR +
58  (double)gps.tow/1000/SECS_IN_YEAR;
59 
60  /* LLA Position in decimal degrees and altitude in km */
61  double latitude = (double)gps.lla_pos.lat / 1e7;
62  double longitude = (double)gps.lla_pos.lon / 1e7;
63  double alt = (double)gps.lla_pos.alt / 1e6;
64 
65  // Calculates additional coeffs
66  nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
67  // Calculates absolute magnet fields
68  mag_calc(1, latitude, longitude, alt, nmax, gha,
72 
73  // copy to ahrs
74 #ifdef AHRS_FLOAT
76 #else
77  // convert to MAG_BFP and copy to ahrs
79 #endif
80 
82  }
84 }
bool_t geo_mag_calc_flag
Definition: geo_mag.c:36
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
int32_t lat
in degrees*1e7
Attitude and Heading Reference System interface.
#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:78
#define GPS_FIX_3D
Definition: gps.h:43
bool_t ready
Definition: geo_mag.h:37
int16_t week
GPS week.
Definition: gps.h:79
#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)
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
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
bool_t kill_throttle
Definition: autopilot.c:41
#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:107
struct GeoMagVect geo_mag_vect
Definition: geo_mag.c:37
void geo_mag_init(void)
Definition: geo_mag.c:39
int32_t lon
in degrees*1e7
Calculation of the Geomagnetic field vector from current GPS fix.
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:129
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:49
#define IEXT
struct AhrsARDrone ahrs_impl
Definition: ahrs_ardrone2.c:45
struct GpsState gps
global GPS state
Definition: gps.c:41
#define SECS_IN_YEAR
void geo_mag_periodic(void)
Definition: geo_mag.c:44