Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_gps_passthrough.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"
37 #include "subsystems/abi.h"
38 
39 #ifndef USE_INS_NAV_INIT
40 #define USE_INS_NAV_INIT TRUE
41 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
42 #endif
43 
44 #if USE_INS_NAV_INIT
45 #include "generated/flight_plan.h"
46 #endif
47 
48 
50  struct LtpDef_i ltp_def;
52 
53  /* output LTP NED */
57 };
58 
60 
61 
65 #ifndef INS_PT_GPS_ID
66 #define INS_PT_GPS_ID GPS_MULTI_ID
67 #endif
68 PRINT_CONFIG_VAR(INS_PT_GPS_ID)
70 
71 static void gps_cb(uint8_t sender_id __attribute__((unused)),
72  uint32_t stamp __attribute__((unused)),
73  struct GpsState *gps_s)
74 {
75  if (gps_s->fix < GPS_FIX_3D) {
76  return;
77  }
78  if (!ins_gp.ltp_initialized) {
80  }
81 
82  /* simply scale and copy pos/speed from gps */
83  struct NedCoor_i gps_pos_cm_ned;
84  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos);
85  INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
88 
89  struct NedCoor_i gps_speed_cm_s_ned;
90  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel);
91  INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
94 }
95 
96 
97 #if PERIODIC_TELEMETRY
99 
100 static void send_ins(struct transport_tx *trans, struct link_device *dev)
101 {
102  pprz_msg_send_INS(trans, dev, AC_ID,
106 }
107 
108 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
109 {
110  static const float fake_baro_z = 0.0;
111  pprz_msg_send_INS_Z(trans, dev, AC_ID,
112  (float *)&fake_baro_z, &ins_gp.ltp_pos.z,
114 }
115 
116 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
117 {
118  static const float fake_qfe = 0.0;
119  if (ins_gp.ltp_initialized) {
120  pprz_msg_send_INS_REF(trans, dev, AC_ID,
123  &ins_gp.ltp_def.hmsl, (float *)&fake_qfe);
124  }
125 }
126 #endif
127 
129 {
130 
131 #if USE_INS_NAV_INIT
132  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
133  llh_nav0.lat = NAV_LAT0;
134  llh_nav0.lon = NAV_LON0;
135  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
136  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
137 
138  struct EcefCoor_i ecef_nav0;
139  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
140 
141  ltp_def_from_ecef_i(&ins_gp.ltp_def, &ecef_nav0);
142  ins_gp.ltp_def.hmsl = NAV_ALT0;
144 
145  ins_gp.ltp_initialized = true;
146 #else
147  ins_gp.ltp_initialized = false;
148 #endif
149 
153 
154 #if PERIODIC_TELEMETRY
158 #endif
159 
160  AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb);
161 }
162 
164 {
169  ins_gp.ltp_initialized = true;
170 }
171 
173 {
174  struct LlaCoor_i lla = {
176  .lon = state.ned_origin_i.lla.lon,
177  .alt = gps.lla_pos.alt
178  };
182 }
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:525
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
int32_t z
in centimeters
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
definition of the local (flat earth) coordinate system
Periodic telemetry system header (includes downlink utility and generated code).
vector in EarthCenteredEarthFixed coordinates
int32_t y
in centimeters
#define INT32_VECT3_ZERO(_v)
Main include for ABI (AirBorneInterface).
#define INS_PT_GPS_ID
ABI binding for gps data.
struct NedCoor_i ltp_speed
Integrated Navigation System interface.
void ins_reset_altitude_ref(void)
INS altitude reference reset.
#define INT32_SPEED_OF_CM_S_DEN
vector in Latitude, Longitude and Altitude
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:757
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
int32_t z
Down.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
struct EcefCoor_i ecef
Reference point in ecef.
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t y
East.
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
data structure for GPS information
Definition: gps.h:81
#define INT32_POS_OF_CM_NUM
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:451
Device independent GPS code (interface)
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:85
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t lon
in degrees*1e7
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
struct NedCoor_i ltp_accel
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
struct NedCoor_i ltp_pos
#define INT32_POS_OF_CM_DEN
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
vector in North East Down coordinates
void ins_gps_passthrough_init(void)
#define INT32_SPEED_OF_CM_S_NUM
static void send_ins(struct transport_tx *trans, struct link_device *dev)
struct InsGpsPassthrough ins_gp
int32_t x
in centimeters
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ins_reset_local_origin(void)
INS local origin reset.
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:86
static abi_event gps_ev
Simply passes GPS through to the state interface.
int32_t lat
in degrees*1e7
struct GpsState gps
global GPS state
Definition: gps.c:75
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
struct State state
Definition: state.c:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
struct LtpDef_i ltp_def