Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
63 #ifndef INS_PT_IMU_ID
64 #define INS_PT_IMU_ID ABI_BROADCAST
65 #endif
67 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
68 
70 static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
72 
73 
77 #ifndef INS_PT_GPS_ID
78 #define INS_PT_GPS_ID GPS_MULTI_ID
79 #endif
80 PRINT_CONFIG_VAR(INS_PT_GPS_ID)
82 
83 static void gps_cb(uint8_t sender_id __attribute__((unused)),
84  uint32_t stamp __attribute__((unused)),
85  struct GpsState *gps_s)
86 {
87  if (gps_s->fix < GPS_FIX_3D) {
88  return;
89  }
90  if (!ins_gp.ltp_initialized) {
92  }
93 
94  /* simply scale and copy pos/speed from gps */
95  struct NedCoor_i gps_pos_cm_ned;
96  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos);
97  INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
100 
101  struct NedCoor_i gps_speed_cm_s_ned;
102  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel);
103  INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
106 }
107 
108 
109 #if PERIODIC_TELEMETRY
111 
112 static void send_ins(struct transport_tx *trans, struct link_device *dev)
113 {
114  pprz_msg_send_INS(trans, dev, AC_ID,
118 }
119 
120 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
121 {
122  static float fake_baro_z = 0.0;
123  pprz_msg_send_INS_Z(trans, dev, AC_ID,
124  (float *)&fake_baro_z, &ins_gp.ltp_pos.z,
126 }
127 
128 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
129 {
130  static float fake_qfe = 0.0;
131  if (ins_gp.ltp_initialized) {
132  pprz_msg_send_INS_REF(trans, dev, AC_ID,
135  &ins_gp.ltp_def.hmsl, (float *)&fake_qfe);
136  }
137 }
138 #endif
139 
141 {
142 
143 #if USE_INS_NAV_INIT
144  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
145  llh_nav0.lat = NAV_LAT0;
146  llh_nav0.lon = NAV_LON0;
147  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
148  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
149 
150  struct EcefCoor_i ecef_nav0;
151  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
152 
153  ltp_def_from_ecef_i(&ins_gp.ltp_def, &ecef_nav0);
154  ins_gp.ltp_def.hmsl = NAV_ALT0;
156 
157  ins_gp.ltp_initialized = true;
158 #else
159  ins_gp.ltp_initialized = false;
160 #endif
161 
165 
166 #if PERIODIC_TELEMETRY
170 #endif
171 
172  AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb);
173  AbiBindMsgIMU_ACCEL_INT32(INS_PT_IMU_ID, &accel_ev, accel_cb);
174  AbiBindMsgBODY_TO_IMU_QUAT(INS_PT_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
175 }
176 
178 {
183  ins_gp.ltp_initialized = true;
184 }
185 
187 {
188  struct LlaCoor_i lla = {
190  .lon = state.ned_origin_i.lla.lon,
191  .alt = gps.lla_pos.alt
192  };
196 }
197 
198 static void accel_cb(uint8_t sender_id __attribute__((unused)),
199  uint32_t stamp __attribute__((unused)),
200  struct Int32Vect3 *accel)
201 {
202  // untilt accel and remove gravity
203  struct Int32Vect3 accel_body, accel_ned;
204  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
205  int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
206  stateSetAccelBody_i(&accel_body);
207  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
208  int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
209  accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
210  stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
211  VECT3_COPY(ins_gp.ltp_accel, accel_ned);
212 }
213 
214 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
215  struct FloatQuat *q_b2i_f)
216 {
218 }
219 
stateSetAccelNed_i
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
gps_ev
static abi_event gps_ev
Definition: ins_gps_passthrough.c:81
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
InsGpsPassthrough::ltp_def
struct LtpDef_i ltp_def
Definition: ins_gps_passthrough.c:50
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_gps_passthrough.c:128
OrientationReps
Definition: pprz_orientation_conversion.h:79
ins.h
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
INS_PT_GPS_ID
#define INS_PT_GPS_ID
ABI binding for gps data.
Definition: ins_gps_passthrough.c:78
INT32_VECT3_SCALE_2
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
Definition: pprz_algebra_int.h:290
InsGpsPassthrough::ltp_speed
struct NedCoor_i ltp_speed
Definition: ins_gps_passthrough.c:55
ltp_def_from_ecef_i
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
Definition: pprz_geodetic_int.c:60
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
abi.h
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
GpsState
data structure for GPS information
Definition: gps.h:87
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
uint32_t
unsigned long uint32_t
Definition: types.h:18
ecef_of_lla_i
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
Definition: pprz_geodetic_int.c:392
accel_ev
static abi_event accel_ev
Definition: ins_gps_passthrough.c:66
stateSetAccelBody_i
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
INT32_POS_OF_CM_NUM
#define INT32_POS_OF_CM_NUM
Definition: pprz_algebra_int.h:70
ins_reset_altitude_ref
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_gps_passthrough.c:186
INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_DEN
Definition: pprz_algebra_int.h:71
InsGpsPassthrough
Definition: ins_gps_passthrough.c:49
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
body_to_imu
static struct OrientationReps body_to_imu
Definition: ins_gps_passthrough.c:71
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
INT32_VECT3_ZERO
#define INT32_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:288
telemetry.h
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
INS_PT_IMU_ID
#define INS_PT_IMU_ID
ABI bindings on ACCEL data.
Definition: ins_gps_passthrough.c:64
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
gps.h
Device independent GPS code (interface)
int32_rmat_transp_vmult
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_int.c:117
send_ins_z
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_gps_passthrough.c:120
InsGpsPassthrough::ltp_initialized
bool ltp_initialized
Definition: ins_gps_passthrough.c:51
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
EcefCoor_i
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_int.h:50
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
INT32_SPEED_OF_CM_S_DEN
#define INT32_SPEED_OF_CM_S_DEN
Definition: pprz_algebra_int.h:76
Int32Vect3
Definition: pprz_algebra_int.h:88
ACCEL_BFP_OF_REAL
#define ACCEL_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:220
uint8_t
unsigned char uint8_t
Definition: types.h:14
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
InsGpsPassthrough::ltp_pos
struct NedCoor_i ltp_pos
Definition: ins_gps_passthrough.c:54
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_gps_passthrough.c:214
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
ned_of_ecef_vect_i
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.
Definition: pprz_geodetic_int.c:186
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
stateSetPositionNed_i
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ins_gps_passthrough.c:69
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_gps_passthrough.c:83
InsGpsPassthrough::ltp_accel
struct NedCoor_i ltp_accel
Definition: ins_gps_passthrough.c:56
ins_gps_passthrough_init
void ins_gps_passthrough_init(void)
Definition: ins_gps_passthrough.c:140
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_gps_passthrough.c:198
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
orientationGetRMat_i
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Definition: pprz_orientation_conversion.h:207
ins_reset_local_origin
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_gps_passthrough.c:177
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
ned_of_ecef_point_i
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.
Definition: pprz_geodetic_int.c:116
ins_gps_passthrough.h
state.h
send_ins
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_gps_passthrough.c:112
state
struct State state
Definition: state.c:36
ins_gp
struct InsGpsPassthrough ins_gp
Definition: ins_gps_passthrough.c:59
inttypes.h
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
stateSetSpeedNed_i
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
INT32_SPEED_OF_CM_S_NUM
#define INT32_SPEED_OF_CM_S_NUM
Definition: pprz_algebra_int.h:75
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140