Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2012 The Paparazzi Team
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 
28 #include "subsystems/ins.h"
29 
30 #if USE_GPS
31 // for ins_reset_utm_zone
32 #include "subsystems/gps.h"
33 #include "state.h"
34 #endif
35 
36 #include "generated/flight_plan.h"
37 
38 
40 {
41  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
42  llh_nav0.lat = NAV_LAT0;
43  llh_nav0.lon = NAV_LON0;
44  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
45  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
46 
47  ltp_def_from_lla_i(ltp_def, &llh_nav0);
48  ltp_def->hmsl = NAV_ALT0;
49  stateSetLocalOrigin_i(ltp_def);
50 }
51 
52 
53 // weak functions, used if not explicitly provided by implementation
54 
55 void WEAK ins_reset_local_origin(void)
56 {
57 #if USE_GPS
58  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
59 
60  // reset state UTM ref
62 #endif
63 }
64 
65 void WEAK ins_reset_altitude_ref(void) {}
66 
67 #if USE_GPS
68 void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
69 {
70  struct LlaCoor_f lla0;
71  lla_of_utm_f(&lla0, utm);
72  if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
73  utm->zone = gps.utm_pos.zone;
74  }
75  else {
76  utm->zone = 0; // recompute zone from lla
77  }
78  utm_of_lla_f(utm, &lla0);
79 
81 }
82 #else
83 void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm __attribute__((unused))) {}
84 #endif
definition of the local (flat earth) coordinate system
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
INS utm zone reset.
Definition: ins.c:68
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:82
Integrated Navigation System interface.
position in UTM coordinates Units: meters
vector in Latitude, Longitude and Altitude
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over MSL)
Definition: gps.h:87
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins.c:65
uint8_t zone
UTM zone number.
vector in Latitude, Longitude and Altitude
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
Device independent GPS code (interface)
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
int32_t lon
in degrees*1e7
void ins_init_origin_i_from_flightplan(struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
Definition: ins.c:39
uint8_t zone
UTM zone number.
void WEAK ins_reset_local_origin(void)
INS local origin reset.
Definition: ins.c:55
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
Definition: gps.c:393
API to get/set the generic vehicle states.
int32_t lat
in degrees*1e7
#define GPS_VALID_POS_UTM_BIT
Definition: gps.h:50
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
struct GpsState gps
global GPS state
Definition: gps.c:75
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)