Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
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
36PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
37#endif
38
39/*
40 * ABI bindings
41 */
43#ifndef INS_MODULE_BARO_ID
44#define INS_MODULE_BARO_ID AIR_DATA_SENDER_ID
45#endif
47
48
49#ifndef INS_MODULE_IMU_ID
50#define INS_MODULE_IMU_ID ABI_BROADCAST
51#endif
53
54
57#ifndef INS_MODULE_GPS_ID
58#define INS_MODULE_GPS_ID GPS_MULTI_ID
59#endif
61
66
68
70
72static void ins_ned_to_state(void)
73{
74 stateSetPositionNed_i(MODULE_INS_SKELETON_ID, &ins_module.ltp_pos);
75 stateSetSpeedNed_i(MODULE_INS_SKELETON_ID, &ins_module.ltp_speed);
76 stateSetAccelNed_i(MODULE_INS_SKELETON_ID, &ins_module.ltp_accel);
77
78#if defined SITL && USE_NPS
79 if (nps_bypass_ins) {
81 }
82#endif
83}
84
85/***********************************************************
86 * ABI callback functions
87 **********************************************************/
88
89static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
90{
91 if (flag == INS_RESET_REF) {
92 if (ins_module.gps.fix >= GPS_FIX_3D) {
93 struct EcefCoor_i ecef_pos = ecef_int_from_gps(&ins_module.gps);
94 ltp_def_from_ecef_i(&ins_module.ltp_def, &ecef_pos);
95 ins_module.ltp_def.lla.alt = ins_module.gps.lla_pos.alt;
96 ins_module.ltp_def.hmsl = ins_module.gps.hmsl;
97 ins_module.ltp_initialized = true;
98 stateSetLocalOrigin_i(MODULE_INS_SKELETON_ID, &ins_module.ltp_def);
99 } else {
100 ins_module.ltp_initialized = false;
101 }
102
104 }
105}
106
107static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
108{
109 /* call module implementation */
110 ins_module_update_baro(pressure);
112}
113
114static void accel_cb(uint8_t sender_id __attribute__((unused)),
115 uint32_t stamp, struct Int32Vect3 *accel)
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 /* call module implementation */
123 ins_module_propagate(accel, dt);
125 }
126 last_stamp = stamp;
127}
128
129static void gps_cb(uint8_t sender_id __attribute__((unused)),
130 uint32_t stamp, struct GpsState *gps_s)
131{
132 /* timestamp in usec when last callback was received */
133 static uint32_t last_stamp = 0;
134
135 if (last_stamp > 0) {
136 float dt = (float)(stamp - last_stamp) * 1e-6;
137
138 /* copy GPS state */
139 ins_module.gps = *gps_s;
140
141 if (!ins_module.ltp_initialized) {
142 reset_cb(MODULE_INS_SKELETON_ID, INS_RESET_REF);
143 }
144
145 if (gps_s->fix >= GPS_FIX_3D) {
146 /* call module implementation */
147 ins_module_update_gps(gps_s, dt);
149 }
150 }
151 last_stamp = stamp;
152}
153
154/*********************************************************************
155 * weak functions that are used if not implemented in a module
156 ********************************************************************/
157
158void WEAK ins_module_init(void)
159{
160}
161
162void WEAK ins_module_update_baro(float pressure __attribute__((unused)))
163{
164}
165
166void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((unused)))
167{
168 /* copy velocity from GPS */
169 if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
170 /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
171 INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_s->ned_vel,
173 }
174 else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
175 /* convert ECEF to NED */
176 struct NedCoor_i gps_speed_cm_s_ned;
177 ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_module.ltp_def, &gps_s->ecef_vel);
178 /* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
179 INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_speed_cm_s_ned,
181 }
182
183 /* copy position from GPS */
184 if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT) ||
185 bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
186 /* convert ECEF to NED */
187 struct NedCoor_i gps_pos_cm_ned;
188 struct EcefCoor_i gps_ecef_pos = ecef_int_from_gps(gps_s);
189 ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_module.ltp_def, &gps_ecef_pos);
190 /* convert pos from cm to m in BFP with INT32_POS_FRAC */
191 INT32_VECT3_SCALE_2(ins_module.ltp_pos, gps_pos_cm_ned,
193 }
194}
195
196void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused)))
197{
198 /* untilt accels */
199 stateSetAccelBody_i(MODULE_INS_SKELETON_ID, accel);
200 struct Int32Vect3 accel_meas_ltp;
201 int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
202
203 VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
204}
205
207{
208}
209
210
212{
213#if USE_INS_NAV_INIT
214 ins_init_origin_i_from_flightplan(MODULE_INS_SKELETON_ID, &ins_module.ltp_def);
215 ins_module.ltp_initialized = true;
216#else
217 ins_module.ltp_initialized = false;
218#endif
219
221 INT32_VECT3_ZERO(ins_module.ltp_speed);
222 INT32_VECT3_ZERO(ins_module.ltp_accel);
223
225
226 // Bind to ABI messages
227 AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
228 AbiBindMsgIMU_ACCEL(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
229 AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
230 AbiBindMsgINS_RESET(ABI_BROADCAST, &reset_ev, reset_cb);
231}
232
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event gps_ev
#define UNUSED(x)
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Definition gps.c:507
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition gps.h:98
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
struct NedCoor_i ned_vel
speed NED in cm/s
Definition gps.h:99
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition gps.h:91
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
uint8_t fix
status of fix
Definition gps.h:110
data structure for GPS information
Definition gps.h:90
#define VECT3_COPY(_a, _b)
#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)
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 EarthCenteredEarthFixed coordinates
vector in North East Down coordinates
static void stateSetAccelNed_i(uint16_t id, struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition state.h:1146
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1186
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition state.h:1301
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:530
static void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition state.h:649
static void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition state.h:903
void ins_init_origin_i_from_flightplan(uint16_t id, struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
Definition ins.c:33
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static void reset_cb(uint8_t sender_id, uint8_t flag)
static abi_event reset_ev
abi_event baro_ev
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition ins_int.c:538
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
#define INS_MODULE_GPS_ID
ABI binding for gps data.
void WEAK ins_module_reset_local_origin(void)
void ins_module_wrapper_init(void)
void WEAK ins_module_update_baro(float pressure)
static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
#define INS_MODULE_BARO_ID
baro
static void ins_ned_to_state(void)
copy position and speed to state interface
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
void WEAK ins_module_init(void)
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt)
#define INS_MODULE_IMU_ID
IMU (accel).
struct InsModuleInt ins_module
global INS state
Paparazzi specific wrapper to run simple module based INS.
Ins implementation state (fixed point).
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
bool nps_bypass_ins
void sim_overwrite_ins(void)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
API to get/set the generic vehicle states.
Architecture independent timing functions.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.