Paparazzi UAS  v5.10_stable-5-g83a0da5-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 
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  struct Int32Vect3 accel_meas_ltp;
122  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
123 
124  VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
125 }
126 
128 {
129 }
130 
131 
132 /***********************************************************
133  * wrapper functions
134  **********************************************************/
135 
137 {
138  if (ins_module.gps.fix >= GPS_FIX_3D) {
144  } else {
145  ins_module.ltp_initialized = false;
146  }
147 
149 }
150 
151 
153 {
154 #if USE_INS_NAV_INIT
157 #else
158  ins_module.ltp_initialized = false;
159 #endif
160 
164 
165  ins_module_init();
166 
167  // Bind to ABI messages
168  AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
169  AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
170  AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
171  AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
172 }
173 
175 static void ins_ned_to_state(void)
176 {
180 
181 #if defined SITL && USE_NPS
182  if (nps_bypass_ins) {
184  }
185 #endif
186 }
187 
188 /***********************************************************
189  * ABI callback functions
190  **********************************************************/
191 
192 static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
193 {
194  /* call module implementation */
195  ins_module_update_baro(pressure);
197 }
198 
199 static void accel_cb(uint8_t sender_id __attribute__((unused)),
200  uint32_t stamp, struct Int32Vect3 *accel)
201 {
202  /* timestamp in usec when last callback was received */
203  static uint32_t last_stamp = 0;
204 
205  if (last_stamp > 0) {
206  float dt = (float)(stamp - last_stamp) * 1e-6;
207  /* call module implementation */
208  ins_module_propagate(accel, dt);
210  }
211  last_stamp = stamp;
212 }
213 
214 static void gps_cb(uint8_t sender_id __attribute__((unused)),
215  uint32_t stamp, struct GpsState *gps_s)
216 {
217  /* timestamp in usec when last callback was received */
218  static uint32_t last_stamp = 0;
219 
220  if (last_stamp > 0) {
221  float dt = (float)(stamp - last_stamp) * 1e-6;
222 
223  /* copy GPS state */
224  ins_module.gps = *gps_s;
225 
228  }
229 
230  if (gps_s->fix >= GPS_FIX_3D) {
231  /* call module implementation */
232  ins_module_update_gps(gps_s, dt);
234  }
235  }
236  last_stamp = stamp;
237 }
238 
239 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
240  struct FloatQuat *q_b2i_f)
241 {
243 }
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:525
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:239
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:1101
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:82
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
#define INT32_VECT3_ZERO(_v)
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
float dt
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:757
#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:136
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:968
void WEAK ins_module_reset_local_origin(void)
Definition: ins_skeleton.c:127
void ins_module_wrapper_init(void)
Definition: ins_skeleton.c:152
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
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:81
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_skeleton.c:214
#define INT32_POS_OF_CM_NUM
struct InsModuleInt ins_module
global INS state
Definition: ins_skeleton.c:71
static void baro_cb(uint8_t sender_id, float pressure)
Definition: ins_skeleton.c:192
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:451
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:85
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:199
rotation matrix
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:89
#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:86
uint8_t fix
status of fix
Definition: gps.h:99
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition: ins_skeleton.c:175
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:90
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