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
ins_gps_passthrough_utm.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2004-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 
29 #include "subsystems/ins.h"
30 
31 #include <inttypes.h>
32 #include <math.h>
33 
34 #include "state.h"
35 #include "subsystems/gps.h"
37 
38 void ins_init(void) {
39  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
41  stateSetPositionUtm_f(&utm0);
42 
44 }
45 
47  struct UtmCoor_f utm;
48 #ifdef GPS_USE_LATLONG
49  /* Recompute UTM coordinates in this zone */
50  struct LlaCoor_f lla;
52  utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
53  utm_of_lla_f(&utm, &lla);
54 #else
55  utm.zone = gps.utm_pos.zone;
56  utm.east = gps.utm_pos.east / 100.0f;
57  utm.north = gps.utm_pos.north / 100.0f;
58 #endif
59  // ground_alt
60  utm.alt = gps.hmsl / 1000.0f;
61  // reset state UTM ref
63 }
64 
66  struct UtmCoor_f utm = state.utm_origin_f;
67  utm.alt = gps.hmsl / 1000.0f;
69 }
70 
71 void ins_update_gps(void) {
72  struct UtmCoor_f utm;
73  utm.east = gps.utm_pos.east / 100.0f;
74  utm.north = gps.utm_pos.north / 100.0f;
75  utm.zone = nav_utm_zone0;
76  utm.alt = gps.hmsl / 1000.0f;
77 
78  // set position
80 
81  struct NedCoor_f ned_vel = {
82  gps.ned_vel.x / 100.0f,
83  gps.ned_vel.y / 100.0f,
84  gps.ned_vel.z / 100.0f
85  };
86  // set velocity
87  stateSetSpeedNed_f(&ned_vel);
88 }
struct Ins ins
global INS state
Definition: ins.c:36
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
void ins_reset_altitude_ref(void)
INS altitude reference reset.
uint8_t zone
UTM zone number.
enum InsStatus status
status of the INS
Definition: ins.h:47
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
Integrated Navigation System interface.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
vector in Latitude, Longitude and Altitude
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:555
int32_t z
Down.
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:754
uint8_t zone
UTM zone number.
int32_t nav_utm_north0
Definition: common_nav.c:43
vector in North East Down coordinates Units: meters
float north
in meters
void ins_reset_local_origin(void)
INS local origin reset.
Device independent GPS code (interface)
position in UTM coordinates Units: meters
void ins_init(void)
INS initialization.
void ins_update_gps(void)
Update INS state with GPS measurements.
int32_t y
East.
int32_t north
in centimeters
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:228
int32_t east
in centimeters
API to get/set the generic vehicle states.
int32_t lon
in degrees*1e7
int32_t nav_utm_east0
Definition: common_nav.c:42
#define LLA_FLOAT_OF_BFP(_o, _i)
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
float east
in meters
float alt
in meters above WGS84 reference ellipsoid
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:459
struct GpsState gps
global GPS state
Definition: gps.c:41
struct State state
Definition: state.c:36
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)