Paparazzi UAS  v5.18.0_stable-1-g6993852-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_skeleton.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Felix Ruess <felix.ruess@gmail.com>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "subsystems/abi.h"
29 #include "mcu_periph/sys_time.h"
30 #include "message_pragmas.h"
31 
32 #include "state.h"
33 
34 #ifndef USE_INS_NAV_INIT
35 #define USE_INS_NAV_INIT TRUE
36 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
37 #endif
38 
39 /*
40  * ABI bindings
41  */
43 #ifndef INS_MODULE_BARO_ID
44 #if USE_BARO_BOARD
45 #define INS_MODULE_BARO_ID BARO_BOARD_SENDER_ID
46 #else
47 #define INS_MODULE_BARO_ID ABI_BROADCAST
48 #endif
49 #endif
50 PRINT_CONFIG_VAR(INS_MODULE_BARO_ID)
51 
52 
53 #ifndef INS_MODULE_IMU_ID
54 #define INS_MODULE_IMU_ID ABI_BROADCAST
55 #endif
56 PRINT_CONFIG_VAR(INS_MODULE_IMU_ID)
57 
58 
61 #ifndef INS_MODULE_GPS_ID
62 #define INS_MODULE_GPS_ID GPS_MULTI_ID
63 #endif
64 PRINT_CONFIG_VAR(INS_MODULE_GPS_ID)
65 
70 
72 
73 void ins_module_wrapper_init(void);
74 
76 static void ins_ned_to_state(void)
77 {
78  stateSetPositionNed_i(&ins_module.ltp_pos);
79  stateSetSpeedNed_i(&ins_module.ltp_speed);
80  stateSetAccelNed_i(&ins_module.ltp_accel);
81 
82 #if defined SITL && USE_NPS
83  if (nps_bypass_ins) {
85  }
86 #endif
87 }
88 
89 /***********************************************************
90  * ABI callback functions
91  **********************************************************/
92 
93 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
94 {
95  /* call module implementation */
96  ins_module_update_baro(pressure);
98 }
99 
100 static void accel_cb(uint8_t sender_id __attribute__((unused)),
101  uint32_t stamp, struct Int32Vect3 *accel)
102 {
103  /* timestamp in usec when last callback was received */
104  static uint32_t last_stamp = 0;
105 
106  if (last_stamp > 0) {
107  float dt = (float)(stamp - last_stamp) * 1e-6;
108  /* call module implementation */
109  ins_module_propagate(accel, dt);
111  }
112  last_stamp = stamp;
113 }
114 
115 static void gps_cb(uint8_t sender_id __attribute__((unused)),
116  uint32_t stamp, struct GpsState *gps_s)
117 {
118  /* timestamp in usec when last callback was received */
119  static uint32_t last_stamp = 0;
120 
121  if (last_stamp > 0) {
122  float dt = (float)(stamp - last_stamp) * 1e-6;
123 
124  /* copy GPS state */
125  ins_module.gps = *gps_s;
126 
129  }
130 
131  if (gps_s->fix >= GPS_FIX_3D) {
132  /* call module implementation */
133  ins_module_update_gps(gps_s, dt);
135  }
136  }
137  last_stamp = stamp;
138 }
139 
140 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
141  struct FloatQuat *q_b2i_f)
142 {
144 }
145 /*********************************************************************
146  * weak functions that are used if not implemented in a module
147  ********************************************************************/
148 
149 void WEAK ins_module_init(void)
150 {
151 }
152 
153 void WEAK ins_module_update_baro(float pressure __attribute__((unused)))
154 {
155 }
156 
157 void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((unused)))
158 {
159  /* copy velocity from GPS */
160  if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
161  /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
164  }
165  else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
166  /* convert ECEF to NED */
167  struct NedCoor_i gps_speed_cm_s_ned;
168  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_module.ltp_def, &gps_s->ecef_vel);
169  /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
170  INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_speed_cm_s_ned,
172  }
173 
174  /* copy position from GPS */
175  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
176  /* convert ECEF to NED */
177  struct NedCoor_i gps_pos_cm_ned;
178  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_module.ltp_def, &gps_s->ecef_pos);
179  /* convert pos from cm to m in BFP with INT32_POS_FRAC */
180  INT32_VECT3_SCALE_2(ins_module.ltp_pos, gps_pos_cm_ned,
182  }
183 }
184 
185 void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused)))
186 {
187  /* untilt accels */
188  struct Int32Vect3 accel_meas_body;
189  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_module.body_to_imu);
190  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
191  stateSetAccelBody_i(&accel_meas_body);
192  struct Int32Vect3 accel_meas_ltp;
193  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
194 
195  VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
196 }
197 
199 {
200 }
201 
202 
203 /***********************************************************
204  * wrapper functions
205  **********************************************************/
206 
208 {
209  if (ins_module.gps.fix >= GPS_FIX_3D) {
215  } else {
216  ins_module.ltp_initialized = false;
217  }
218 
220 }
221 
222 
224 {
225 #if USE_INS_NAV_INIT
228 #else
229  ins_module.ltp_initialized = false;
230 #endif
231 
235 
236  ins_module_init();
237 
238  // Bind to ABI messages
239  AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
240  AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
241  AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
242  AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
243 }
244 
bool nps_bypass_ins
void WEAK ins_module_init(void)
Definition: ins_skeleton.c:149
#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:531
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void WEAK ins_module_update_baro(float pressure)
Definition: ins_skeleton.c:153
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_skeleton.c:140
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define INT32_VECT3_ZERO(_v)
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
Main include for ABI (AirBorneInterface).
#define INS_MODULE_IMU_ID
IMU (accel, body_to_imu)
Definition: ins_skeleton.c:54
static abi_event accel_ev
Definition: ins_skeleton.c:67
static abi_event body_to_imu_ev
Definition: ins_skeleton.c:69
Ins implementation state (fixed point)
Definition: ins_skeleton.h:40
struct NedCoor_i ltp_accel
Definition: ins_skeleton.h:47
#define INT32_SPEED_OF_CM_S_DEN
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_skeleton.c:207
static abi_event baro_ev
Definition: ins_skeleton.c:66
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.
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
Roation quaternion.
#define INS_MODULE_GPS_ID
ABI binding for gps data.
Definition: ins_skeleton.c:62
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
void WEAK ins_module_reset_local_origin(void)
Definition: ins_skeleton.c:198
void ins_module_wrapper_init(void)
Definition: ins_skeleton.c:223
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
struct NedCoor_i ltp_speed
velocity in m/s in BFP with INT32_SPEED_FRAC
Definition: ins_skeleton.h:46
Architecture independent timing functions.
bool ltp_initialized
Definition: ins_skeleton.h:42
data structure for GPS information
Definition: gps.h:87
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_skeleton.c:115
#define INT32_POS_OF_CM_NUM
struct InsModuleInt ins_module
global INS state
Definition: ins_skeleton.c:71
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct NedCoor_i ltp_pos
position in m in BFP with INT32_POS_FRAC
Definition: ins_skeleton.h:45
struct GpsState gps
internal copy of last GPS message
Definition: ins_skeleton.h:50
struct LlaCoor_i lla
Reference point in lla.
void ins_init_origin_i_from_flightplan(struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
Definition: ins.c:39
#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
#define INT32_SPEED_OF_CM_S_NUM
#define INS_MODULE_BARO_ID
baro
Definition: ins_skeleton.c:47
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ins_skeleton.h:53
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt)
Definition: ins_skeleton.c:185
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_skeleton.c:100
rotation matrix
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
uint8_t fix
status of fix
Definition: gps.h:107
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition: ins_skeleton.c:76
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt)
Definition: ins_skeleton.c:157
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_skeleton.c:93
static abi_event gps_ev
Definition: ins_skeleton.c:68
void sim_overwrite_ins(void)
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
Paparazzi specific wrapper to run simple module based INS.
struct LtpDef_i ltp_def
Definition: ins_skeleton.h:41