30 #include "message_pragmas.h"
34 #ifndef USE_INS_NAV_INIT
35 #define USE_INS_NAV_INIT TRUE
43 #ifndef INS_MODULE_BARO_ID
45 #define INS_MODULE_BARO_ID BARO_BOARD_SENDER_ID
47 #define INS_MODULE_BARO_ID ABI_BROADCAST
53 #ifndef INS_MODULE_IMU_ID
54 #define INS_MODULE_IMU_ID ABI_BROADCAST
61 #ifndef INS_MODULE_GPS_ID
62 #define INS_MODULE_GPS_ID GPS_MULTI_ID
181 #if defined SITL && USE_NPS
205 if (last_stamp > 0) {
206 float dt = (float)(stamp - last_stamp) * 1e-6;
220 if (last_stamp > 0) {
221 float dt = (float)(stamp - last_stamp) * 1e-6;
void WEAK ins_module_init(void)
#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).
Event structure to store callbacks in a linked list.
void WEAK ins_module_update_baro(float pressure)
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
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
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
#define VECT3_COPY(_a, _b)
#define INT32_VECT3_ZERO(_v)
Main include for ABI (AirBorneInterface).
#define INS_MODULE_IMU_ID
IMU (accel, body_to_imu)
static abi_event accel_ev
static abi_event body_to_imu_ev
Ins implementation state (fixed point)
struct NedCoor_i ltp_accel
#define INT32_SPEED_OF_CM_S_DEN
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
#define GPS_FIX_3D
3D GPS fix
void ins_reset_local_origin(void)
INS local origin reset.
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
#define INS_MODULE_GPS_ID
ABI binding for gps data.
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
void WEAK ins_module_reset_local_origin(void)
void ins_module_wrapper_init(void)
int32_t hmsl
height above mean sea level (MSL) in mm
struct NedCoor_i ltp_speed
velocity in m/s in BFP with INT32_SPEED_FRAC
Architecture independent timing functions.
data structure for GPS information
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
#define INT32_POS_OF_CM_NUM
struct InsModuleInt ins_module
global INS state
static void baro_cb(uint8_t sender_id, float pressure)
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
struct EcefCoor_i ecef_pos
position in ECEF in cm
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct NedCoor_i ltp_pos
position in m in BFP with INT32_POS_FRAC
struct GpsState gps
internal copy of last GPS message
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
#define INT32_POS_OF_CM_DEN
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
#define GPS_VALID_POS_ECEF_BIT
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
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt)
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
#define GPS_VALID_VEL_ECEF_BIT
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
static void ins_ned_to_state(void)
copy position and speed to state interface
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt)
struct NedCoor_i ned_vel
speed NED in cm/s
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.