mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-26 16:30:07 +08:00
[modules] add ins_skeleton (#1575)
INS module skeleton. This module is meant to be used with an additional module which implements any of the following - ins_module_init() - ins_module_propagate(struct Int32Vect3 *accel, float dt) - ins_module_update_gps(struct GpsState *gps_s, float dt) - ins_module_update_baro(float pressure) - ins_module_reset_local_origin(void) In each of those functions the updated estimate has to be written to the ins_module struct, which is then used to update the state interface.
This commit is contained in:
Felix Ruess
@@ -0,0 +1,32 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="ins_skeleton" dir="ins">
|
||||
<doc>
|
||||
<description>
|
||||
INS module skeleton.
|
||||
This module is meant to be used with an additional module which implements any of the following
|
||||
- ins_module_init()
|
||||
- ins_module_propagate(struct Int32Vect3 *accel, float dt)
|
||||
- ins_module_update_gps(struct GpsState *gps_s, float dt)
|
||||
- ins_module_update_baro(float pressure)
|
||||
- ins_module_reset_local_origin(void)
|
||||
|
||||
In each of those functions the updated estimate has to be written to the ins_module struct,
|
||||
which is then used to update the state interface.
|
||||
</description>
|
||||
<define name="INS_MODULE_IMU_ID" value="ABI_BROADCAST" description="ABI sender id of IMU messages to subscribe to"/>
|
||||
<define name="INS_MODULE_BARO_ID" value="ABI_BROADCAST" description="ABI sender id of BARO_ABS messages to subscribe to"/>
|
||||
<define name="INS_MODULE_GPS_ID" value="GPS_MULTI_ID" description="ABI sender id of GPS messages to subscribe to"/>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="ins_skeleton.h"/>
|
||||
</header>
|
||||
<makefile target="ap|nps">
|
||||
<raw>
|
||||
ap.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_skeleton.h\"
|
||||
nps.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_skeleton.h\"
|
||||
</raw>
|
||||
<file name="ins.c" dir="subsystems"/>
|
||||
<file name="ins_skeleton.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,251 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ins/ins_skeleton.c
|
||||
*
|
||||
* Paparazzi specific wrapper to run simple module based INS.
|
||||
*/
|
||||
|
||||
#include "modules/ins/ins_skeleton.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "mcu_periph/sys_time.h"
|
||||
#include "message_pragmas.h"
|
||||
|
||||
#include "state.h"
|
||||
|
||||
#ifndef USE_INS_NAV_INIT
|
||||
#define USE_INS_NAV_INIT TRUE
|
||||
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
|
||||
#endif
|
||||
|
||||
/*
|
||||
* ABI bindings
|
||||
*/
|
||||
/** baro */
|
||||
#ifndef INS_MODULE_BARO_ID
|
||||
#if USE_BARO_BOARD
|
||||
#define INS_MODULE_BARO_ID BARO_BOARD_SENDER_ID
|
||||
#else
|
||||
#define INS_MODULE_BARO_ID ABI_BROADCAST
|
||||
#endif
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(INS_MODULE_BARO_ID)
|
||||
|
||||
/** IMU (accel, body_to_imu) */
|
||||
#ifndef INS_MODULE_IMU_ID
|
||||
#define INS_MODULE_IMU_ID ABI_BROADCAST
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(INS_MODULE_IMU_ID)
|
||||
|
||||
/** ABI binding for gps data.
|
||||
* Used for GPS ABI messages.
|
||||
*/
|
||||
#ifndef INS_MODULE_GPS_ID
|
||||
#define INS_MODULE_GPS_ID GPS_MULTI_ID
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(INS_MODULE_GPS_ID)
|
||||
|
||||
static abi_event baro_ev;
|
||||
static abi_event accel_ev;
|
||||
static abi_event gps_ev;
|
||||
static abi_event body_to_imu_ev;
|
||||
|
||||
struct InsModuleInt ins_module;
|
||||
|
||||
void ins_module_wrapper_init(void);
|
||||
|
||||
/*********************************************************************
|
||||
* weak functions that are used if not implemented in a module
|
||||
********************************************************************/
|
||||
|
||||
void WEAK ins_module_init(void)
|
||||
{
|
||||
}
|
||||
|
||||
void WEAK ins_module_update_baro(float pressure __attribute__((unused)))
|
||||
{
|
||||
}
|
||||
|
||||
void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((unused)))
|
||||
{
|
||||
/* copy velocity from GPS */
|
||||
if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
|
||||
/* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
|
||||
INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_s->ned_vel,
|
||||
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
|
||||
}
|
||||
else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
|
||||
/* convert ECEF to NED */
|
||||
struct NedCoor_i gps_speed_cm_s_ned;
|
||||
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_module.ltp_def, &gps_s->ecef_vel);
|
||||
/* convert speed from cm/s to m/s in BFP with INT32_SPEED_FRAC */
|
||||
INT32_VECT3_SCALE_2(ins_module.ltp_speed, gps_speed_cm_s_ned,
|
||||
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
|
||||
}
|
||||
|
||||
/* copy position from GPS */
|
||||
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
|
||||
/* convert ECEF to NED */
|
||||
struct NedCoor_i gps_pos_cm_ned;
|
||||
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_module.ltp_def, &gps_s->ecef_pos);
|
||||
/* convert pos from cm to m in BFP with INT32_POS_FRAC */
|
||||
INT32_VECT3_SCALE_2(ins_module.ltp_pos, gps_pos_cm_ned,
|
||||
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
}
|
||||
}
|
||||
|
||||
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused)))
|
||||
{
|
||||
/* untilt accels */
|
||||
struct Int32Vect3 accel_meas_body;
|
||||
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_module.body_to_imu);
|
||||
int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
|
||||
struct Int32Vect3 accel_meas_ltp;
|
||||
int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
|
||||
|
||||
VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
|
||||
}
|
||||
|
||||
void WEAK ins_module_reset_local_origin(void)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/***********************************************************
|
||||
* wrapper functions
|
||||
**********************************************************/
|
||||
|
||||
void ins_reset_local_origin(void)
|
||||
{
|
||||
if (ins_module.gps.fix >= GPS_FIX_3D) {
|
||||
ltp_def_from_ecef_i(&ins_module.ltp_def, &ins_module.gps.ecef_pos);
|
||||
ins_module.ltp_def.lla.alt = ins_module.gps.lla_pos.alt;
|
||||
ins_module.ltp_def.hmsl = ins_module.gps.hmsl;
|
||||
ins_module.ltp_initialized = true;
|
||||
stateSetLocalOrigin_i(&ins_module.ltp_def);
|
||||
} else {
|
||||
ins_module.ltp_initialized = false;
|
||||
}
|
||||
|
||||
ins_module_reset_local_origin();
|
||||
}
|
||||
|
||||
|
||||
void ins_module_wrapper_init(void)
|
||||
{
|
||||
#if USE_INS_NAV_INIT
|
||||
ins_init_origin_i_from_flightplan(&ins_module.ltp_def);
|
||||
ins_module.ltp_initialized = true;
|
||||
#else
|
||||
ins_module.ltp_initialized = false;
|
||||
#endif
|
||||
|
||||
INT32_VECT3_ZERO(ins_module.ltp_pos);
|
||||
INT32_VECT3_ZERO(ins_module.ltp_speed);
|
||||
INT32_VECT3_ZERO(ins_module.ltp_accel);
|
||||
|
||||
ins_module_init();
|
||||
}
|
||||
|
||||
/** copy position and speed to state interface */
|
||||
static void ins_ned_to_state(void)
|
||||
{
|
||||
stateSetPositionNed_i(&ins_module.ltp_pos);
|
||||
stateSetSpeedNed_i(&ins_module.ltp_speed);
|
||||
stateSetAccelNed_i(&ins_module.ltp_accel);
|
||||
|
||||
#if defined SITL && USE_NPS
|
||||
if (nps_bypass_ins) {
|
||||
sim_overwrite_ins();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/***********************************************************
|
||||
* ABI callback functions
|
||||
**********************************************************/
|
||||
|
||||
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
|
||||
{
|
||||
/* call module implementation */
|
||||
ins_module_update_baro(pressure);
|
||||
ins_ned_to_state();
|
||||
}
|
||||
|
||||
static void accel_cb(uint8_t sender_id __attribute__((unused)),
|
||||
uint32_t stamp, struct Int32Vect3 *accel)
|
||||
{
|
||||
/* timestamp in usec when last callback was received */
|
||||
static uint32_t last_stamp = 0;
|
||||
|
||||
if (last_stamp > 0) {
|
||||
float dt = (float)(stamp - last_stamp) * 1e-6;
|
||||
/* call module implementation */
|
||||
ins_module_propagate(accel, dt);
|
||||
ins_ned_to_state();
|
||||
}
|
||||
last_stamp = stamp;
|
||||
}
|
||||
|
||||
static void gps_cb(uint8_t sender_id __attribute__((unused)),
|
||||
uint32_t stamp, struct GpsState *gps_s)
|
||||
{
|
||||
/* timestamp in usec when last callback was received */
|
||||
static uint32_t last_stamp = 0;
|
||||
|
||||
if (last_stamp > 0) {
|
||||
float dt = (float)(stamp - last_stamp) * 1e-6;
|
||||
|
||||
/* copy GPS state */
|
||||
ins_module.gps = *gps_s;
|
||||
|
||||
if (!ins_module.ltp_initialized) {
|
||||
ins_reset_local_origin();
|
||||
}
|
||||
|
||||
if (gps_s->fix >= GPS_FIX_3D) {
|
||||
/* call module implementation */
|
||||
ins_module_update_gps(gps_s, dt);
|
||||
ins_ned_to_state();
|
||||
}
|
||||
}
|
||||
last_stamp = stamp;
|
||||
}
|
||||
|
||||
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
|
||||
struct FloatQuat *q_b2i_f)
|
||||
{
|
||||
orientationSetQuat_f(&ins_module.body_to_imu, q_b2i_f);
|
||||
}
|
||||
|
||||
void ins_module_register(void)
|
||||
{
|
||||
ins_register_impl(ins_module_wrapper_init);
|
||||
|
||||
// Bind to ABI messages
|
||||
AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
|
||||
AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
|
||||
AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
|
||||
AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#endif
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ins/ins_skeleton.h
|
||||
*
|
||||
* Paparazzi specific wrapper to run simple module based INS.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef INS_SKELETON_H
|
||||
#define INS_SKELETON_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic_int.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "math/pprz_orientation_conversion.h"
|
||||
|
||||
#include "subsystems/ins.h"
|
||||
#include "subsystems/gps.h"
|
||||
|
||||
/** Ins implementation state (fixed point) */
|
||||
struct InsModuleInt {
|
||||
struct LtpDef_i ltp_def;
|
||||
bool ltp_initialized;
|
||||
|
||||
/* output LTP NED */
|
||||
struct NedCoor_i ltp_pos; ///< position in m in BFP with #INT32_POS_FRAC
|
||||
struct NedCoor_i ltp_speed; ///< velocity in m/s in BFP with #INT32_SPEED_FRAC
|
||||
struct NedCoor_i ltp_accel;
|
||||
|
||||
/** internal copy of last GPS message */
|
||||
struct GpsState gps;
|
||||
|
||||
/** body_to_imu rotation */
|
||||
struct OrientationReps body_to_imu;
|
||||
};
|
||||
|
||||
/** global INS state */
|
||||
extern struct InsModuleInt ins_module;
|
||||
|
||||
#ifndef DefaultInsImpl
|
||||
#define DefaultInsImpl ins_module
|
||||
#endif
|
||||
|
||||
extern void ins_module_register(void);
|
||||
|
||||
/* these functions can/should be implemented in your module */
|
||||
extern void ins_module_init(void);
|
||||
extern void ins_module_propagate(struct Int32Vect3 *accel, float dt);
|
||||
extern void ins_module_update_gps(struct GpsState *gps_s, float dt);
|
||||
extern void ins_module_update_baro(float pressure);
|
||||
extern void ins_module_reset_local_origin(void);
|
||||
|
||||
#endif /* INS_SKELETON_H */
|
||||
Reference in New Issue
Block a user