Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
geo_mag.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Sergey Krukowski <softsr@yahoo.de>
3  * Copyright (C) 2020 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 }
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
GEO_MAG_SENDER_ID
#define GEO_MAG_SENDER_ID
Definition: geo_mag.c:40
geo_mag_event
void geo_mag_event(void)
Definition: geo_mag.c:59
DoubleVect3::z
double z
Definition: pprz_algebra_double.h:49
pprz_geodetic_wmm2020.h
WMM2020 Geomagnetic field model.
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
abi.h
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
NMAX_1
#define NMAX_1
Definition: pprz_geodetic_wmm2020.h:48
GeoMag::vect
struct DoubleVect3 vect
Definition: geo_mag.h:36
MAXCOEFF
#define MAXCOEFF
Definition: pprz_geodetic_wmm2020.h:67
geo_mag
struct GeoMag geo_mag
Definition: geo_mag.c:43
extrapsh
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
Definition: pprz_geodetic_wmm2020.c:64
NMAX_2
#define NMAX_2
Definition: pprz_geodetic_wmm2020.h:49
float_vect3_normalize
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
Definition: pprz_algebra_float.h:177
FloatVect3
Definition: pprz_algebra_float.h:54
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
GeoMag
Definition: geo_mag.h:35
gps.h
Device independent GPS code (interface)
DoubleVect3::x
double x
Definition: pprz_algebra_double.h:47
autopilot_throttle_killed
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:230
SECS_IN_YEAR
#define SECS_IN_YEAR
Definition: pprz_geodetic_wmm2020.h:64
GpsState::week
uint16_t week
GPS week.
Definition: gps.h:108
WEEKS_IN_YEAR
#define WEEKS_IN_YEAR
Definition: pprz_geodetic_wmm2020.h:63
geo_mag_init
void geo_mag_init(void)
Definition: geo_mag.c:45
EXT_COEFF2
#define EXT_COEFF2
Definition: pprz_geodetic_wmm2020.h:53
autopilot.h
IEXT
#define IEXT
Definition: pprz_geodetic_wmm2020.h:51
GpsFixValid
#define GpsFixValid()
Definition: gps.h:43
pprz_algebra_double.h
Paparazzi double precision floating point algebra.
GPS_EPOCH_BEGIN
#define GPS_EPOCH_BEGIN
Begin of the GPS epoch.
Definition: pprz_geodetic_wmm2020.h:57
EXT_COEFF1
#define EXT_COEFF1
Definition: pprz_geodetic_wmm2020.h:52
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
int32_t
signed long int32_t
Definition: types.h:19
GeoMag::calc_once
bool calc_once
Definition: geo_mag.h:37
geo_mag_periodic
void geo_mag_periodic(void)
Definition: geo_mag.c:51
geo_mag.h
Calculation of the Geomagnetic field vector from current location. This module is based on the world ...
mag_calc
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)
Definition: pprz_geodetic_wmm2020.c:105
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
GeoMag::ready
bool ready
Definition: geo_mag.h:38
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
GEO_EPOCH
#define GEO_EPOCH
Definition: pprz_geodetic_wmm2020.h:45
EXT_COEFF3
#define EXT_COEFF3
Definition: pprz_geodetic_wmm2020.h:54