Paparazzi UAS  v7.0_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 
69 
71 
72 void ins_module_wrapper_init(void);
73 
75 static void ins_ned_to_state(void)
76 {
80 
81 #if defined SITL && USE_NPS
82  if (nps_bypass_ins) {
84  }
85 #endif
86 }
87 
88 /***********************************************************
89  * ABI callback functions
90  **********************************************************/
91 
92 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
93 {
94  /* call module implementation */
95  ins_module_update_baro(pressure);
97 }
98 
99 static void accel_cb(uint8_t sender_id __attribute__((unused)),
100  uint32_t stamp, struct Int32Vect3 *accel)
101 {
102  /* timestamp in usec when last callback was received */
103  static uint32_t last_stamp = 0;
104 
105  if (last_stamp > 0) {
106  float dt = (float)(stamp - last_stamp) * 1e-6;
107  /* call module implementation */
108  ins_module_propagate(accel, dt);
110  }
111  last_stamp = stamp;
112 }
113 
114 static void gps_cb(uint8_t sender_id __attribute__((unused)),
115  uint32_t stamp, struct GpsState *gps_s)
116 {
117  /* timestamp in usec when last callback was received */
118  static uint32_t last_stamp = 0;
119 
120  if (last_stamp > 0) {
121  float dt = (float)(stamp - last_stamp) * 1e-6;
122 
123  /* copy GPS state */
124  ins_module.gps = *gps_s;
125 
128  }
129 
130  if (gps_s->fix >= GPS_FIX_3D) {
131  /* call module implementation */
132  ins_module_update_gps(gps_s, dt);
134  }
135  }
136  last_stamp = stamp;
137 }
138 
139 /*********************************************************************
140  * weak functions that are used if not implemented in a module
141  ********************************************************************/
142 
143 void WEAK ins_module_init(void)
144 {
145 }
146 
147 void WEAK ins_module_update_baro(float pressure __attribute__((unused)))
148 {
149 }
150 
151 void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((unused)))
152 {
153  /* copy velocity from GPS */
154  if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
155  /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
158  }
159  else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
160  /* convert ECEF to NED */
161  struct NedCoor_i gps_speed_cm_s_ned;
162  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_module.ltp_def, &gps_s->ecef_vel);
163  /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
164  INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_speed_cm_s_ned,
166  }
167 
168  /* copy position from GPS */
169  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
170  /* convert ECEF to NED */
171  struct NedCoor_i gps_pos_cm_ned;
172  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_module.ltp_def, &gps_s->ecef_pos);
173  /* convert pos from cm to m in BFP with INT32_POS_FRAC */
174  INT32_VECT3_SCALE_2(ins_module.ltp_pos, gps_pos_cm_ned,
176  }
177 }
178 
179 void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused)))
180 {
181  /* untilt accels */
182  stateSetAccelBody_i(accel);
183  struct Int32Vect3 accel_meas_ltp;
184  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
185 
186  VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
187 }
188 
190 {
191 }
192 
193 
194 /***********************************************************
195  * wrapper functions
196  **********************************************************/
197 
199 {
200  if (ins_module.gps.fix >= GPS_FIX_3D) {
206  } else {
207  ins_module.ltp_initialized = false;
208  }
209 
211 }
212 
213 
215 {
216 #if USE_INS_NAV_INIT
219 #else
220  ins_module.ltp_initialized = false;
221 #endif
222 
226 
227  ins_module_init();
228 
229  // Bind to ABI messages
230  AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
231  AbiBindMsgIMU_ACCEL(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
232  AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
233 }
234 
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:90
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:49
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:50
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:89
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:93
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:46
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:94
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:86
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
uint8_t fix
status of fix
Definition: gps.h:105
data structure for GPS information
Definition: gps.h:85
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define INT32_POS_OF_CM_DEN
#define INT32_SPEED_OF_CM_S_NUM
#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)
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
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.
vector in North East Down coordinates
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
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
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt)
Definition: ins_skeleton.c:179
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_skeleton.c:198
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_skeleton.c:114
#define INS_MODULE_GPS_ID
ABI binding for gps data.
Definition: ins_skeleton.c:62
void WEAK ins_module_reset_local_origin(void)
Definition: ins_skeleton.c:189
void ins_module_wrapper_init(void)
Definition: ins_skeleton.c:214
static abi_event accel_ev
Definition: ins_skeleton.c:67
void WEAK ins_module_update_baro(float pressure)
Definition: ins_skeleton.c:147
static abi_event baro_ev
Definition: ins_skeleton.c:66
#define INS_MODULE_BARO_ID
baro
Definition: ins_skeleton.c:47
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition: ins_skeleton.c:75
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_skeleton.c:92
void WEAK ins_module_init(void)
Definition: ins_skeleton.c:143
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_skeleton.c:99
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt)
Definition: ins_skeleton.c:151
#define INS_MODULE_IMU_ID
IMU (accel)
Definition: ins_skeleton.c:54
struct InsModuleInt ins_module
global INS state
Definition: ins_skeleton.c:70
static abi_event gps_ev
Definition: ins_skeleton.c:68
Paparazzi specific wrapper to run simple module based INS.
struct NedCoor_i ltp_speed
velocity in m/s in BFP with INT32_SPEED_FRAC
Definition: ins_skeleton.h:46
struct NedCoor_i ltp_pos
position in m in BFP with INT32_POS_FRAC
Definition: ins_skeleton.h:45
struct LtpDef_i ltp_def
Definition: ins_skeleton.h:41
struct GpsState gps
internal copy of last GPS message
Definition: ins_skeleton.h:50
struct NedCoor_i ltp_accel
Definition: ins_skeleton.h:47
bool ltp_initialized
Definition: ins_skeleton.h:42
Ins implementation state (fixed point)
Definition: ins_skeleton.h:40
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
bool nps_bypass_ins
void sim_overwrite_ins(void)
API to get/set the generic vehicle states.
Architecture independent timing functions.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98