Paparazzi UAS  v5.15_devel-88-gb3ad7fe
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 
75 /*********************************************************************
76  * weak functions that are used if not implemented in a module
77  ********************************************************************/
78 
79 void WEAK ins_module_init(void)
80 {
81 }
82 
83 void WEAK ins_module_update_baro(float pressure __attribute__((unused)))
84 {
85 }
86 
87 void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((unused)))
88 {
89  /* copy velocity from GPS */
90  if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
91  /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
94  }
95  else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
96  /* convert ECEF to NED */
97  struct NedCoor_i gps_speed_cm_s_ned;
98  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_module.ltp_def, &gps_s->ecef_vel);
99  /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
100  INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_speed_cm_s_ned,
102  }
103 
104  /* copy position from GPS */
105  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
106  /* convert ECEF to NED */
107  struct NedCoor_i gps_pos_cm_ned;
108  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_module.ltp_def, &gps_s->ecef_pos);
109  /* convert pos from cm to m in BFP with INT32_POS_FRAC */
110  INT32_VECT3_SCALE_2(ins_module.ltp_pos, gps_pos_cm_ned,
112  }
113 }
114 
115 void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused)))
116 {
117  /* untilt accels */
118  struct Int32Vect3 accel_meas_body;
119  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_module.body_to_imu);
120  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
121  stateSetAccelBody_i(&accel_meas_body);
122  struct Int32Vect3 accel_meas_ltp;
123  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
124 
125  VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
126 }
127 
129 {
130 }
131 
132 
133 /***********************************************************
134  * wrapper functions
135  **********************************************************/
136 
138 {
139  if (ins_module.gps.fix >= GPS_FIX_3D) {
145  } else {
146  ins_module.ltp_initialized = false;
147  }
148 
150 }
151 
152 
154 {
155 #if USE_INS_NAV_INIT
158 #else
159  ins_module.ltp_initialized = false;
160 #endif
161 
165 
166  ins_module_init();
167 
168  // Bind to ABI messages
169  AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
170  AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
171  AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
172  AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
173 }
174 
176 static void ins_ned_to_state(void)
177 {
181 
182 #if defined SITL && USE_NPS
183  if (nps_bypass_ins) {
185  }
186 #endif
187 }
188 
189 /***********************************************************
190  * ABI callback functions
191  **********************************************************/
192 
193 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
194 {
195  /* call module implementation */
196  ins_module_update_baro(pressure);
198 }
199 
200 static void accel_cb(uint8_t sender_id __attribute__((unused)),
201  uint32_t stamp, struct Int32Vect3 *accel)
202 {
203  /* timestamp in usec when last callback was received */
204  static uint32_t last_stamp = 0;
205 
206  if (last_stamp > 0) {
207  float dt = (float)(stamp - last_stamp) * 1e-6;
208  /* call module implementation */
209  ins_module_propagate(accel, dt);
211  }
212  last_stamp = stamp;
213 }
214 
215 static void gps_cb(uint8_t sender_id __attribute__((unused)),
216  uint32_t stamp, struct GpsState *gps_s)
217 {
218  /* timestamp in usec when last callback was received */
219  static uint32_t last_stamp = 0;
220 
221  if (last_stamp > 0) {
222  float dt = (float)(stamp - last_stamp) * 1e-6;
223 
224  /* copy GPS state */
225  ins_module.gps = *gps_s;
226 
229  }
230 
231  if (gps_s->fix >= GPS_FIX_3D) {
232  /* call module implementation */
233  ins_module_update_gps(gps_s, dt);
235  }
236  }
237  last_stamp = stamp;
238 }
239 
240 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
241  struct FloatQuat *q_b2i_f)
242 {
244 }
bool nps_bypass_ins
void WEAK ins_module_init(void)
Definition: ins_skeleton.c:79
#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:83
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_skeleton.c:240
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:137
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:128
void ins_module_wrapper_init(void)
Definition: ins_skeleton.c:153
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:215
#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:115
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_skeleton.c:200
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:176
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt)
Definition: ins_skeleton.c:87
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:193
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