Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/ins/ins.h"
29 
30 #if USE_GPS
31 // for ins_reset_utm_zone
32 #include "modules/gps/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;
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
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
ins.h
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
ins_reset_utm_zone
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
INS utm zone reset.
Definition: ins.c:68
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
lla_of_utm_f
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
Definition: pprz_geodetic_float.c:344
utm_of_lla_f
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Definition: pprz_geodetic_float.c:308
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
stateSetLocalUtmOrigin_f
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
gps.h
Device independent GPS code (interface)
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
GPS_VALID_POS_UTM_BIT
#define GPS_VALID_POS_UTM_BIT
Definition: gps.h:50
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
GpsState::utm_pos
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over MSL)
Definition: gps.h:93
ltp_def_from_lla_i
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:72
ins_init_origin_i_from_flightplan
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
utm_float_from_gps
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:411
ins_reset_altitude_ref
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins.c:65
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
ins_reset_local_origin
void WEAK ins_reset_local_origin(void)
INS local origin reset.
Definition: ins.c:55
state.h
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
UtmCoor_i::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_int.h:90
LlaCoor_f
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_float.h:54