Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/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 {
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 
stateSetAccelNed_i
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
INS_MODULE_IMU_ID
#define INS_MODULE_IMU_ID
IMU (accel, body_to_imu)
Definition: ins_skeleton.c:54
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
INT32_VECT3_SCALE_2
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
Definition: pprz_algebra_int.h:291
baro_cb
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_skeleton.c:93
InsModuleInt::ltp_accel
struct NedCoor_i ltp_accel
Definition: ins_skeleton.h:47
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
abi.h
ins_module_init
void WEAK ins_module_init(void)
Definition: ins_skeleton.c:149
baro_ev
static abi_event baro_ev
Definition: ins_skeleton.c:66
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ins_skeleton.c:69
GpsState
data structure for GPS information
Definition: gps.h:87
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
nps_bypass_ins
bool nps_bypass_ins
Definition: nps_autopilot_fixedwing.c:54
ins_reset_local_origin
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_skeleton.c:207
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
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_DEN
Definition: pprz_algebra_int.h:71
accel_ev
static abi_event accel_ev
Definition: ins_skeleton.c:67
INT32_VECT3_ZERO
#define INT32_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:289
InsModuleInt::gps
struct GpsState gps
internal copy of last GPS message
Definition: ins_skeleton.h:50
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_MODULE_GPS_ID
#define INS_MODULE_GPS_ID
ABI binding for gps data.
Definition: ins_skeleton.c:62
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
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
InsModuleInt
Ins implementation state (fixed point)
Definition: ins_skeleton.h:40
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
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
INT32_SPEED_OF_CM_S_DEN
#define INT32_SPEED_OF_CM_S_DEN
Definition: pprz_algebra_int.h:76
ins_skeleton.h
InsModuleInt::ltp_speed
struct NedCoor_i ltp_speed
velocity in m/s in BFP with INT32_SPEED_FRAC
Definition: ins_skeleton.h:46
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_skeleton.c:115
InsModuleInt::ltp_initialized
bool ltp_initialized
Definition: ins_skeleton.h:42
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
ins_module_reset_local_origin
void WEAK ins_module_reset_local_origin(void)
Definition: ins_skeleton.c:198
ins_module_wrapper_init
void ins_module_wrapper_init(void)
Definition: ins_skeleton.c:223
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
ins_init_origin_i_from_flightplan
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
stateSetPositionNed_i
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
ins_module
struct InsModuleInt ins_module
global INS state
Definition: ins_skeleton.c:71
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
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_skeleton.c:100
InsModuleInt::ltp_pos
struct NedCoor_i ltp_pos
position in m in BFP with INT32_POS_FRAC
Definition: ins_skeleton.h:45
sim_overwrite_ins
void sim_overwrite_ins(void)
Definition: nps_autopilot_fixedwing.c:247
INS_MODULE_BARO_ID
#define INS_MODULE_BARO_ID
baro
Definition: ins_skeleton.c:47
GPS_VALID_VEL_NED_BIT
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
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
GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
InsModuleInt::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ins_skeleton.h:53
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
gps_ev
static abi_event gps_ev
Definition: ins_skeleton.c:68
InsModuleInt::ltp_def
struct LtpDef_i ltp_def
Definition: ins_skeleton.h:41
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
GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
ins_module_update_gps
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt)
Definition: ins_skeleton.c:157
state.h
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_skeleton.c:140
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
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
ins_module_propagate
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt)
Definition: ins_skeleton.c:185
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
ins_ned_to_state
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition: ins_skeleton.c:76
ins_module_update_baro
void WEAK ins_module_update_baro(float pressure)
Definition: ins_skeleton.c:153