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.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"
36 
37 #ifndef USE_INS_NAV_INIT
38 #define USE_INS_NAV_INIT TRUE
39 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
40 #endif
41 
42 #if USE_INS_NAV_INIT
43 #include "generated/flight_plan.h"
44 #endif
45 
47  struct LtpDef_i ltp_def;
49 
50  /* output LTP NED */
54 };
55 
57 
58 #if PERIODIC_TELEMETRY
60 
61 static void send_ins(void) {
62  DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice,
66 }
67 
68 static void send_ins_z(void) {
69  static const float fake_baro_z = 0.0;
70  DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice,
71  &fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
72 }
73 
74 static void send_ins_ref(void) {
75  static const float fake_qfe = 0.0;
77  DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice,
80  &ins_impl.ltp_def.hmsl, &fake_qfe);
81  }
82 }
83 #endif
84 
85 void ins_init(void) {
86 
87 #if USE_INS_NAV_INIT
88  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
89  llh_nav0.lat = NAV_LAT0;
90  llh_nav0.lon = NAV_LON0;
91  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
92  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
93 
94  struct EcefCoor_i ecef_nav0;
95  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
96 
97  ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
98  ins_impl.ltp_def.hmsl = NAV_ALT0;
100 
102 #else
104 #endif
105 
109 
110 #if PERIODIC_TELEMETRY
111  register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
112  register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z);
113  register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
114 #endif
115 }
116 
117 void ins_periodic(void) {
120 }
121 
122 
129 }
130 
132  struct LlaCoor_i lla = {
135  gps.lla_pos.alt
136  };
140 }
141 
142 void ins_update_gps(void) {
143  if (gps.fix == GPS_FIX_3D) {
144  if (!ins_impl.ltp_initialized) {
146  }
147 
148  /* simply scale and copy pos/speed from gps */
149  struct NedCoor_i gps_pos_cm_ned;
150  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
151  INT32_VECT3_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned,
154 
155  struct NedCoor_i gps_speed_cm_s_ned;
156  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);
157  INT32_VECT3_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned,
160  }
161 }
struct Ins ins
global INS state
Definition: ins.c:36
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:508
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
int32_t lat
in degrees*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
vector in EarthCenteredEarthFixed coordinates
Periodic telemetry system header (includes downlink utility and generated code).
enum InsStatus status
status of the INS
Definition: ins.h:47
struct NedCoor_i ltp_speed
struct InsGpsPassthrough ins_impl
global INS state
int32_t x
North.
Integrated Navigation System interface.
struct LlaCoor_i lla
Reference point in lla.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
vector in Latitude, Longitude and Altitude
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
void ins_reset_altitude_ref(void)
INS altitude reference reset.
uint8_t fix
status of fix
Definition: gps.h:78
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:712
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
void ins_update_gps(void)
Update INS state with GPS measurements.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
struct NedCoor_i ltp_accel
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
#define FALSE
Definition: imu_chimu.h:141
vector in North East Down coordinates
int32_t z
Down.
definition of the local (flat earth) coordinate system
struct LtpDef_i ltp_def
#define INT32_SPEED_OF_CM_S_DEN
#define INT32_VECT3_ZERO(_v)
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:440
Device independent GPS code (interface)
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.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t y
East.
int32_t x
in centimeters
#define INT32_POS_OF_CM_DEN
#define TRUE
Definition: imu_chimu.h:144
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
API to get/set the generic vehicle states.
#define INT32_SPEED_OF_CM_S_NUM
int32_t lon
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
void ins_init(void)
INS initialization.
struct EcefCoor_i ecef
Reference point in ecef.
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.
void ins_reset_local_origin(void)
INS local origin reset.
#define INT32_POS_OF_CM_NUM
struct NedCoor_i ltp_pos
struct GpsState gps
global GPS state
Definition: gps.c:41
void ins_periodic(void)
INS periodic call.
struct State state
Definition: state.c:36