Paparazzi UAS  v5.15_devel-99-g2ff7410
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 
30 #include "subsystems/ins.h"
31 
32 #include <inttypes.h>
33 #include <math.h>
34 
35 #include "state.h"
36 #include "subsystems/gps.h"
38 
39 
40 #include "subsystems/abi.h"
44 #ifndef INS_PT_GPS_ID
45 #define INS_PT_GPS_ID GPS_MULTI_ID
46 #endif
47 PRINT_CONFIG_VAR(INS_PT_GPS_ID)
49 
50 static void gps_cb(uint8_t sender_id __attribute__((unused)),
51  uint32_t stamp __attribute__((unused)),
52  struct GpsState *gps_s)
53 {
54  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
55 
56  // set position
58 
59  struct NedCoor_f ned_vel = {
60  gps_s->ned_vel.x / 100.0f,
61  gps_s->ned_vel.y / 100.0f,
62  gps_s->ned_vel.z / 100.0f
63  };
64  // set velocity
65  stateSetSpeedNed_f(&ned_vel);
66 }
67 
68 
70 {
71  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
73  stateSetPositionUtm_f(&utm0);
74 
75  AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb);
76 }
77 
79 {
80  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
81 
82  // reset state UTM ref
84 }
85 
87 {
88  struct UtmCoor_f utm = state.utm_origin_f;
89  utm.alt = gps.hmsl / 1000.0f;
91 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void ins_reset_altitude_ref(void)
INS altitude reference reset.
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
void ins_gps_passthrough_init(void)
Main include for ABI (AirBorneInterface).
uint8_t nav_utm_zone0
Definition: common_nav.c:44
Integrated Navigation System interface.
position in UTM coordinates Units: meters
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
int32_t nav_utm_north0
Definition: common_nav.c:43
vector in North East Down coordinates Units: meters
data structure for GPS information
Definition: gps.h:87
void ins_reset_local_origin(void)
INS local origin reset.
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
#define INS_PT_GPS_ID
ABI binding for gps data.
unsigned char uint8_t
Definition: types.h:14
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:389
API to get/set the generic vehicle states.
int32_t nav_utm_east0
Definition: common_nav.c:42
static abi_event gps_ev
Simply passes GPS through to the state interface.
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:69
struct State state
Definition: state.c:36
float x
in meters