Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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  * Copyright (C) 2015 OpenUAS <info@openuas.org>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
32 #include "subsystems/gps.h"
33 #include "subsystems/abi.h"
34 
35 //FIXME: should not be in this spot
36 //for kill_throttle check
37 #include "autopilot.h"
38 
39 #ifndef GEO_MAG_SENDER_ID
40 #define GEO_MAG_SENDER_ID 1
41 #endif
42 
43 struct GeoMag geo_mag;
44 
45 void geo_mag_init(void)
46 {
47  geo_mag.calc_once = false;
48  geo_mag.ready = false;
49 }
50 
51 void geo_mag_periodic(void)
52 {
53  //FIXME: kill_throttle has no place in a geomag module
55  geo_mag.calc_once = true;
56  }
57 }
58 
59 void geo_mag_event(void)
60 {
61  if (geo_mag.calc_once) {
62  double gha[MAXCOEFF]; // Geomag global variables
63  int32_t nmax;
64 
65  /* Current date in decimal year, for example 2015.68 */
66  double sdate = GPS_EPOCH_BEGIN +
67  (double)gps.week / WEEKS_IN_YEAR +
68  (double)gps.tow / 1000 / SECS_IN_YEAR;
69 
70  /* LLA Position in decimal degrees and altitude in km */
71  double latitude = (double)gps.lla_pos.lat / 1e7;
72  double longitude = (double)gps.lla_pos.lon / 1e7;
73  double alt = (double)gps.lla_pos.alt / 1e6;
74 
75  // Calculates additional coeffs
76  nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
77  // Calculates absolute magnet fields
78  mag_calc(1, latitude, longitude, alt, nmax, gha,
81 
82  // send as normalized float vector via ABI
83  struct FloatVect3 h = { .x = geo_mag.vect.x,
84  .y = geo_mag.vect.y,
85  .z = geo_mag.vect.z };
87  AbiSendMsgGEO_MAG(GEO_MAG_SENDER_ID, &h);
88 
89  geo_mag.ready = true;
90  }
91  geo_mag.calc_once = false;
92 }
#define NMAX_1
struct DoubleVect3 vect
Definition: geo_mag.h:36
WMM2015 Geomagnetic field model.
#define MAXCOEFF
struct GeoMag geo_mag
Definition: geo_mag.c:43
Definition: geo_mag.h:35
#define NMAX_2
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:225
uint16_t week
GPS week.
Definition: gps.h:100
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
Main include for ABI (AirBorneInterface).
#define SECS_IN_YEAR
bool calc_once
Definition: geo_mag.h:37
bool ready
Definition: geo_mag.h:38
int32_t alt
in millimeters above WGS84 reference ellipsoid
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
uint32_t tow
GPS time of week in ms.
Definition: gps.h:101
Device independent GPS code (interface)
#define IEXT
int32_t lon
in degrees*1e7
signed long int32_t
Definition: types.h:19
#define WEEKS_IN_YEAR
Core autopilot interface common to all firmwares.
#define EXT_COEFF2
void geo_mag_init(void)
Definition: geo_mag.c:45
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 GEO_EPOCH
#define EXT_COEFF1
Calculation of the Geomagnetic field vector from current location.
Paparazzi double precision floating point algebra.
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:86
#define EXT_COEFF3
void geo_mag_event(void)
Definition: geo_mag.c:59
int32_t lat
in degrees*1e7
#define GPS_EPOCH_BEGIN
Begin of the GPS epoch.
#define GEO_MAG_SENDER_ID
Definition: geo_mag.c:40
struct GpsState gps
global GPS state
Definition: gps.c:75
void geo_mag_periodic(void)
Definition: geo_mag.c:51
#define GpsFixValid()
Definition: gps.h:43