Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[ins] wrapper for ins float_invariant

Browse Source
This commit is contained in:
Felix Ruess
2015-03-03 13:23:08 +01:00
parent 221788281a
commit 6a1144ac39
6 changed files with 183 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile
+2 -1
View File
@@ -2,7 +2,7 @@
# attitude and speed estimation for fixedwings via invariant filter
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\"
INS_CFLAGS += -DUSE_AHRS_ALIGNER
INS_CFLAGS += -DUSE_AHRS
INS_CFLAGS += -DINS_UPDATE_FW_ESTIMATOR
@@ -11,6 +11,7 @@ INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant_wrapper.c
ifneq ($(AHRS_ALIGNER_LED),none)
conf/firmwares/subsystems/rotorcraft/ins_float_invariant.makefile
+2 -2
View File
@@ -4,7 +4,7 @@
USE_MAGNETOMETER ?= 1
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\"
INS_CFLAGS += -DUSE_AHRS_ALIGNER
INS_CFLAGS += -DUSE_AHRS
# for geo mag
@@ -18,7 +18,7 @@ INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant_wrapper.c
ifneq ($(AHRS_ALIGNER_LED),none)
INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
sw/airborne/subsystems/ins/ins_float_invariant.c
+3 -108
View File
@@ -16,18 +16,14 @@
* 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 subsystems/ins/ins_float_invariant.c
* @author Jean-Philippe Condomines <jp.condomines@gmail.com>
*
* INS using invariant filter.
*
* Only for fixedwing currenctly
*
*/
#include "subsystems/ins/ins_float_invariant.h"
@@ -50,29 +46,8 @@
#include "math/pprz_rk_float.h"
#include "math/pprz_isa.h"
#include "subsystems/abi.h"
#include "state.h"
#include "led.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
{
float foo = 0.;
if (state.ned_initialized_i) {
pprz_msg_send_INS_REF(trans, dev, AC_ID,
&state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y, &state.ned_origin_i.ecef.z,
&state.ned_origin_i.lla.lat, &state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt,
&state.ned_origin_i.hmsl, &foo);
}
}
#endif
#if LOG_INVARIANT_FILTER
#include "sdLog.h"
@@ -169,38 +144,6 @@ static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
/* barometer */
bool_t ins_baro_initialized;
// Baro event on ABI
#ifndef INS_BARO_ID
#if USE_BARO_BOARD
#define INS_BARO_ID BARO_BOARD_SENDER_ID
#else
#define INS_BARO_ID ABI_BROADCAST
#endif
#endif
PRINT_CONFIG_VAR(INS_BARO_ID)
abi_event baro_ev;
static void baro_cb(uint8_t sender_id, float pressure);
/* magnetometer */
#ifndef INS_MAG_ID
#define INS_MAG_ID ABI_BROADCAST
#endif
static abi_event mag_ev;
static void mag_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
struct Int32Vect3 *mag);
static abi_event aligner_ev;
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag);
#ifndef INS_IMU_ID
#define INS_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static void gyro_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, struct Int32Rates *gyro);
/* gps */
bool_t ins_gps_fix_once;
@@ -510,7 +453,7 @@ void ins_float_inv_update_gps(void)
}
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
void ins_float_invariant_update_baro(float pressure)
{
static float ins_qfe = 101325.0f;
static float alpha = 10.0f;
@@ -551,7 +494,7 @@ void ins_float_invariant_update_mag(struct Int32Vect3* mag)
static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
static int32_t last_mx = 0;
if (last_mx == imu.mag.x) {
if (last_mx == mag->x) {
mag_frozen_count--;
if (mag_frozen_count == 0) {
// if mag is dead, better set measurements to zero
@@ -568,7 +511,7 @@ void ins_float_invariant_update_mag(struct Int32Vect3* mag)
// reset counter
mag_frozen_count = MAG_FROZEN_COUNT;
}
last_mx = imu.mag.x;
last_mx = mag->x;
}
@@ -743,51 +686,3 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
VECT3_ADD(v2, v1);
QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
}
static void gyro_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, struct Int32Rates *gyro)
{
PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
/* 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;
ins_float_invariant_propagate(gyro, &imu.accel, dt);
}
last_stamp = stamp;
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ins_float_inv.is_aligned) {
ins_float_invariant_update_mag(mag);
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
if (!ins_float_inv.is_aligned) {
ins_float_invariant_align(lp_gyro, lp_accel, lp_mag);
}
}
void ins_float_inv_register(void)
{
ins_register_impl(ins_float_inv_init, ins_float_inv_update_gps);
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_MAG_INT32(INS_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_GYRO_INT32(INS_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}
sw/airborne/subsystems/ins/ins_float_invariant.h
+6 -9
View File
@@ -16,11 +16,11 @@
* 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 subsystems/ins/ins_float_invariant.h
* INS using invariant filter.
* For more information, please send an email to "jp.condomines@gmail.com"
*/
@@ -117,22 +117,19 @@ struct InsFloatInv {
extern struct InsFloatInv ins_float_inv;
#define DefaultAhrsImpl ins_float_inv
#define DefaultInsImpl ins_float_inv
extern void ins_float_inv_register(void);
extern void ins_float_inv_init(void);
extern void ins_float_inv_update_gps(void);
extern void ins_float_invariant_propagate(struct Int32Rates* gyro,
struct Int32Vect3* accel, float dt);
/** called on IMU_LOWPASSED ABI message */
extern void ins_float_invariant_align(struct Int32Rates *lp_gyro,
struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag);
/** called on IMU_MAG_INT32 ABI messages */
extern void ins_float_invariant_update_mag(struct Int32Vect3* mag);
extern void ins_float_invariant_update_baro(float pressure);
#endif /* INS_FLOAT_INVARIANT_H */
sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c
+133
View File
@@ -0,0 +1,133 @@
/*
* Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger
* 2015 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 subsystems/ins/ins_float_invariant_wrapper.c
*
* Paparazzi specific wrapper to run INVARIANT filter.
*/
#include "subsystems/ins/ins_float_invariant_wrapper.h"
#include "subsystems/abi.h"
#include "subsystems/imu.h"
#include "message_pragmas.h"
#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
#include "state.h"
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
{
float foo = 0.;
if (state.ned_initialized_i) {
pprz_msg_send_INS_REF(trans, dev, AC_ID,
&state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y,
&state.ned_origin_i.ecef.z, &state.ned_origin_i.lla.lat,
&state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt,
&state.ned_origin_i.hmsl, &foo);
}
}
#endif
/*
* ABI bindings
*/
/** baro */
#ifndef INS_FINV_BARO_ID
#if USE_BARO_BOARD
#define INS_FINV_BARO_ID BARO_BOARD_SENDER_ID
#else
#define INS_FINV_BARO_ID ABI_BROADCAST
#endif
#endif
PRINT_CONFIG_VAR(INS_FINV_BARO_ID)
/** IMU (gyro, accel) */
#ifndef INS_FINV_IMU_ID
#define INS_FINV_IMU_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(INS_FINV_IMU_ID)
/** magnetometer */
#ifndef INS_FINV_MAG_ID
#define INS_FINV_MAG_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(INS_FINV_MAG_ID)
static abi_event baro_ev;
static abi_event mag_ev;
static abi_event gyro_ev;
static abi_event aligner_ev;
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
{
ins_float_invariant_update_baro(pressure);
}
static void gyro_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, struct Int32Rates *gyro)
{
PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
/* 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;
ins_float_invariant_propagate(gyro, &imu.accel, dt);
}
last_stamp = stamp;
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ins_float_inv.is_aligned) {
ins_float_invariant_update_mag(mag);
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
if (!ins_float_inv.is_aligned) {
ins_float_invariant_align(lp_gyro, lp_accel, lp_mag);
}
}
void ins_float_inv_register(void)
{
ins_register_impl(ins_float_inv_init, ins_float_inv_update_gps);
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}
sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h
+37
View File
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2015 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 subsystems/ins/ins_float_invariant_wrapper.h
*
* Paparazzi specific wrapper to run INVARIANT filter.
*/
#ifndef INS_FLOAT_INVARIANT_WRAPPER_H
#define INS_FLOAT_INVARIANT_WRAPPER_H
#include "subsystems/ins/ins_float_invariant.h"
#define DefaultAhrsImpl ins_float_inv
#define DefaultInsImpl ins_float_inv
extern void ins_float_inv_register(void);
#endif /* INS_FLOAT_INVARIANT_WRAPPER_H */