diff --git a/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile b/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile index 5193234437..00e333dfbb 100644 --- a/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile @@ -18,9 +18,10 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\" AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index 4a0283f107..cab17d8206 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -31,9 +31,7 @@ #include "subsystems/ahrs/ahrs_float_mlkf.h" #include "subsystems/ahrs/ahrs_float_utils.h" -#include /* for FLT_MIN */ #include /* for memcpy */ -#include /* for M_PI */ #include "state.h" @@ -42,8 +40,6 @@ #include "math/pprz_simple_matrix.h" #include "generated/airframe.h" -#include "subsystems/abi.h" - //#include #ifndef AHRS_MAG_NOISE_X @@ -60,85 +56,6 @@ static inline void set_body_state_from_quat(void); struct AhrsMlkf ahrs_mlkf; -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" - -static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_GEO_MAG(trans, dev, AC_ID, - &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z); -} -#endif - - -/** ABI binding for IMU data. - * Used for gyro, accel and mag ABI messages. - */ -#ifndef AHRS_MLKF_IMU_ID -#define AHRS_MLKF_IMU_ID ABI_BROADCAST -#endif -static abi_event gyro_ev; -static abi_event accel_ev; -static abi_event mag_ev; - -static abi_event aligner_ev; - -static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, - const struct Int32Rates* gyro) -{ -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.") - /* timestamp in usec when last callback was received */ - static uint32_t last_stamp = 0; - - if (last_stamp > 0 && ahrs_mlkf.status == AHRS_MLKF_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); - ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt); - } -#endif -} - -static void accel_cb(uint8_t sender_id __attribute__((unused)), - const uint32_t* stamp __attribute__((unused)), - const struct Int32Vect3* accel) -{ - if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { - ahrs_mlkf_update_accel((struct Int32Vect3*)accel); - } -} - -static void mag_cb(uint8_t sender_id __attribute__((unused)), - const uint32_t* stamp __attribute__((unused)), - const struct Int32Vect3* mag) -{ - if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { - ahrs_mlkf_update_mag((struct Int32Vect3*)mag); - } -} - -static void aligner_cb(uint8_t __attribute__((unused)) sender_id, - const uint32_t* stamp __attribute__((unused)), - const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel, - const struct Int32Vect3* lp_mag) -{ - if (ahrs_mlkf.status != AHRS_MLKF_RUNNING) { - ahrs_mlkf_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, - (struct Int32Vect3*)lp_mag); - } -} - - -void ahrs_mlkf_register(void) -{ - ahrs_register_impl(ahrs_mlkf_init, NULL); -} void ahrs_mlkf_init(struct OrientationReps* body_to_imu) { @@ -170,18 +87,6 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) { memcpy(ahrs_mlkf.P, P0, sizeof(P0)); VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z); - - /* - * Subscribe to scaled IMU measurements and attach callbacks - */ - AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); -#endif } bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h index 5d4e02c364..aa5e95044b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h @@ -34,7 +34,6 @@ #include "std.h" #include "math/pprz_algebra_float.h" #include "math/pprz_orientation_conversion.h" -#include "subsystems/ahrs.h" enum AhrsMlkfStatus { AHRS_MLKF_UNINIT, @@ -63,9 +62,6 @@ struct AhrsMlkf { extern struct AhrsMlkf ahrs_mlkf; -#define DefaultAhrsImpl ahrs_mlkf - -extern void ahrs_mlkf_register(void); extern void ahrs_mlkf_init(struct OrientationReps* body_to_imu); extern bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c new file mode 100644 index 0000000000..e786ea683c --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2014 Felix Ruess + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/ahrs/ahrs_float_mlkf_wrapper.c + * + * Paparazzi specific wrapper to run MLKF filter. + */ + +#include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, + &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z); +} +#endif + + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_MLKF_IMU_ID +#define AHRS_MLKF_IMU_ID ABI_BROADCAST +#endif +static abi_event gyro_ev; +static abi_event accel_ev; +static abi_event mag_ev; + +static abi_event aligner_ev; + +static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, + const struct Int32Rates* gyro) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) +PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0 && ahrs_mlkf.status == AHRS_MLKF_RUNNING) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.") +PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt); + } +#endif +} + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + const uint32_t* stamp __attribute__((unused)), + const struct Int32Vect3* accel) +{ + if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { + ahrs_mlkf_update_accel((struct Int32Vect3*)accel); + } +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + const uint32_t* stamp __attribute__((unused)), + const struct Int32Vect3* mag) +{ + if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { + ahrs_mlkf_update_mag((struct Int32Vect3*)mag); + } +} + +static void aligner_cb(uint8_t __attribute__((unused)) sender_id, + const uint32_t* stamp __attribute__((unused)), + const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel, + const struct Int32Vect3* lp_mag) +{ + if (ahrs_mlkf.status != AHRS_MLKF_RUNNING) { + ahrs_mlkf_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, + (struct Int32Vect3*)lp_mag); + } +} + +void ahrs_mlkf_register(void) +{ + ahrs_register_impl(ahrs_mlkf_init, NULL); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h new file mode 100644 index 0000000000..7554859455 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2014 Felix Ruess + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/ahrs/ahrs_float_mlkf_wrapper.h + * + * Paparazzi specific wrapper to run MLKF filter. + */ + +#ifndef AHRS_FLOAT_MLKF_WRAPPER_H +#define AHRS_FLOAT_MLKF_WRAPPER_H + +#include "subsystems/ahrs/ahrs_float_mlkf.h" + +#define DefaultAhrsImpl ahrs_mlkf + +extern void ahrs_mlkf_register(void); + +#endif /* AHRS_FLOAT_MLKF_WRAPPER_H */