Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
30#include "modules/ins/ins.h"
31
32#include <inttypes.h>
33#include <math.h>
34
35#include "state.h"
36#include "modules/gps/gps.h"
37#include "modules/core/abi.h"
38
39#ifndef USE_INS_NAV_INIT
40#define USE_INS_NAV_INIT TRUE
41PRINT_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
52
53 /* output LTP NED */
57};
58
60
63#ifndef INS_PT_IMU_ID
64#define INS_PT_IMU_ID ABI_BROADCAST
65#endif
67static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
68
69
71static void reset_cb(uint8_t sender_id, uint8_t flag);
72
76#ifndef INS_PT_GPS_ID
77#define INS_PT_GPS_ID GPS_MULTI_ID
78#endif
81
82static void gps_cb(uint8_t sender_id __attribute__((unused)),
83 uint32_t stamp __attribute__((unused)),
84 struct GpsState *gps_s)
85{
86 if (gps_s->fix < GPS_FIX_3D) {
87 return;
88 }
89 if (!ins_gp.ltp_initialized) {
90 reset_cb(MODULE_INS_GPS_PASSTHROUGH_ID, INS_RESET_REF);
91 }
92
93 /* simply scale and copy pos/speed from gps */
94 struct NedCoor_i gps_pos_cm_ned;
95 struct EcefCoor_i ecef_pos = ecef_int_from_gps(gps_s);
96 ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &ecef_pos);
97 INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
99 stateSetPositionNed_i(MODULE_INS_GPS_PASSTHROUGH_ID, &ins_gp.ltp_pos);
100
101 struct NedCoor_i gps_speed_cm_s_ned;
102 struct EcefCoor_i ecef_vel = ecef_vel_int_from_gps(gps_s);
103 ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &ecef_vel);
104 INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
106 stateSetSpeedNed_i(MODULE_INS_GPS_PASSTHROUGH_ID, &ins_gp.ltp_speed);
107}
108
109
110#if PERIODIC_TELEMETRY
112
113static void send_ins(struct transport_tx *trans, struct link_device *dev)
114{
115 pprz_msg_send_INS(trans, dev, AC_ID,
116 &ins_gp.ltp_pos.x, &ins_gp.ltp_pos.y, &ins_gp.ltp_pos.z,
117 &ins_gp.ltp_speed.x, &ins_gp.ltp_speed.y, &ins_gp.ltp_speed.z,
118 &ins_gp.ltp_accel.x, &ins_gp.ltp_accel.y, &ins_gp.ltp_accel.z);
119}
120
121static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
122{
123 static float fake_baro_z = 0.0;
124 pprz_msg_send_INS_Z(trans, dev, AC_ID,
125 (float *)&fake_baro_z, &ins_gp.ltp_pos.z,
126 &ins_gp.ltp_speed.z, &ins_gp.ltp_accel.z);
127}
128
129static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
130{
131 static float fake_qfe = 0.0;
132 if (ins_gp.ltp_initialized) {
133 pprz_msg_send_INS_REF(trans, dev, AC_ID,
134 &ins_gp.ltp_def.ecef.x, &ins_gp.ltp_def.ecef.y, &ins_gp.ltp_def.ecef.z,
135 &ins_gp.ltp_def.lla.lat, &ins_gp.ltp_def.lla.lon, &ins_gp.ltp_def.lla.alt,
136 &ins_gp.ltp_def.hmsl, (float *)&fake_qfe);
137 }
138}
139#endif
140
142{
143
144#if USE_INS_NAV_INIT
145 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
146 llh_nav0.lat = NAV_LAT0;
147 llh_nav0.lon = NAV_LON0;
148 /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
149 llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
150
151 struct EcefCoor_i ecef_nav0;
152 ecef_of_lla_i(&ecef_nav0, &llh_nav0);
153
154 ltp_def_from_ecef_i(&ins_gp.ltp_def, &ecef_nav0);
155 ins_gp.ltp_def.hmsl = NAV_ALT0;
156 stateSetLocalOrigin_i(MODULE_INS_GPS_PASSTHROUGH_ID, &ins_gp.ltp_def);
157
158 ins_gp.ltp_initialized = true;
159#else
160 ins_gp.ltp_initialized = false;
161#endif
162
163 INT32_VECT3_ZERO(ins_gp.ltp_pos);
164 INT32_VECT3_ZERO(ins_gp.ltp_speed);
165 INT32_VECT3_ZERO(ins_gp.ltp_accel);
166
167#if PERIODIC_TELEMETRY
171#endif
172
173 AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb);
174 AbiBindMsgIMU_ACCEL(INS_PT_IMU_ID, &accel_ev, accel_cb);
175 AbiBindMsgINS_RESET(ABI_BROADCAST, &reset_ev, reset_cb);
176}
177
178static void reset_ref(void)
179{
180 struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
181 struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
182 ltp_def_from_ecef_i(&ins_gp.ltp_def, &ecef_pos);
183 ins_gp.ltp_def.lla.alt = lla_pos.alt;
184 ins_gp.ltp_def.hmsl = gps.hmsl;
185 stateSetLocalOrigin_i(MODULE_INS_GPS_PASSTHROUGH_ID, &ins_gp.ltp_def);
186 ins_gp.ltp_initialized = true;
187}
188
189static void reset_vertical_ref(void)
190{
191 struct LlaCoor_i lla = {
192 .lat = stateGetLlaOrigin_i().lat,
193 .lon = stateGetLlaOrigin_i().lon,
194 .alt = gps.lla_pos.alt
195 };
196 ltp_def_from_lla_i(&ins_gp.ltp_def, &lla);
197 ins_gp.ltp_def.hmsl = gps.hmsl;
198 stateSetLocalOrigin_i(MODULE_INS_GPS_PASSTHROUGH_ID, &ins_gp.ltp_def);
199}
200
201static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
202{
203 switch (flag) {
204 case INS_RESET_REF:
205 reset_ref();
206 break;
209 break;
210 default:
211 // unsupported cases
212 break;
213 }
214}
215
216static void accel_cb(uint8_t sender_id __attribute__((unused)),
217 uint32_t stamp __attribute__((unused)),
218 struct Int32Vect3 *accel)
219{
220 // untilt accel and remove gravity
221 struct Int32Vect3 accel_ned;
222 stateSetAccelBody_i(MODULE_INS_GPS_PASSTHROUGH_ID, accel);
223 struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
224 int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, accel);
225 accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
226 VECT3_COPY(ins_gp.ltp_accel, accel_ned);
227 stateSetAccelNed_i(MODULE_INS_GPS_PASSTHROUGH_ID, &ins_gp.ltp_accel);
228}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event gps_ev
#define UNUSED(x)
if(GpsFixValid() &&e_identification_started)
static struct uart_periph * dev
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct GpsState gps
global GPS state
Definition gps.c:74
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (integer) Converted on the fly if not available.
Definition gps.c:478
struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (integer) Converted on the fly if not available.
Definition gps.c:536
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Definition gps.c:507
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
data structure for GPS information
Definition gps.h:90
#define VECT3_COPY(_a, _b)
#define INT32_POS_OF_CM_DEN
#define INT32_SPEED_OF_CM_S_NUM
#define ACCEL_BFP_OF_REAL(_af)
#define INT32_SPEED_OF_CM_S_DEN
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_ZERO(_v)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
rotation matrix
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
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.
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 ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
static void stateSetAccelNed_i(uint16_t id, struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition state.h:1146
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1186
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition state.h:1301
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:530
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
static void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition state.h:649
static void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition state.h:903
Integrated Navigation System interface.
#define INS_RESET_VERTICAL_REF
Definition ins.h:37
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static void reset_cb(uint8_t sender_id, uint8_t flag)
static abi_event reset_ev
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:563
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:608
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:589
static void reset_vertical_ref(void)
struct NedCoor_i ltp_speed
static void reset_cb(uint8_t sender_id, uint8_t flag)
static void send_ins(struct transport_tx *trans, struct link_device *dev)
struct InsGpsPassthrough ins_gp
struct NedCoor_i ltp_accel
static void reset_ref(void)
void ins_gps_passthrough_init(void)
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
#define INS_PT_IMU_ID
ABI bindings on ACCEL data.
struct LtpDef_i ltp_def
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
#define INS_PT_GPS_ID
ABI binding for gps data.
struct NedCoor_i ltp_pos
Simply passes GPS through to the state interface.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.