diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile index 26b63dc8da..4c15e066e8 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile @@ -5,7 +5,7 @@ USE_MAGNETOMETER ?= 0 AHRS_ALIGNER_LED ?= none -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\" AHRS_CFLAGS += -DUSE_AHRS_ALIGNER AHRS_CFLAGS += -DUSE_AHRS @@ -16,6 +16,7 @@ endif AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm_wrapper.c ifneq ($(AHRS_ALIGNER_LED),none) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index abb1f0ef5d..e594493df8 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -149,7 +149,7 @@ volatile uint8_t ahrs_timeout_counter = 0; static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { uint8_t mde = 3; - if (ahrs.status == AHRS_UNINIT) { mde = 2; } + if (!DefaultAhrsImpl.is_aligned) { mde = 2; } if (ahrs_timeout_counter > 10) { mde = 5; } uint16_t val = 0; pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); @@ -770,19 +770,6 @@ static inline void on_gyro_event(void) // current timestamp uint32_t now_ts = get_sys_time_usec(); -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for INS propagation.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; - // dt between this and last callback in seconds - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; -#else - PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.") - PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); -#endif - ahrs_timeout_counter = 0; imu_scale_gyro(&imu); @@ -790,12 +777,8 @@ static inline void on_gyro_event(void) AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev); #if USE_AHRS_ALIGNER - // Run aligner on raw data as it also makes averages. - if (ahrs.status != AHRS_RUNNING) { + if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) { ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { - ahrs.status = AHRS_RUNNING; - } return; } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 9e53a01a71..e1c14a06ac 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -27,7 +27,6 @@ #include "std.h" -#include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_float_dcm.h" #include "subsystems/ahrs/ahrs_float_utils.h" #include "firmwares/fixedwing/autopilot.h" // launch detection @@ -36,7 +35,6 @@ #include "math/pprz_algebra_float.h" #include "state.h" -#include "subsystems/abi.h" #if USE_GPS #include "subsystems/gps.h" @@ -93,70 +91,6 @@ float imu_health = 0.; #endif -/** ABI binding for IMU data. - * Used for gyro, accel and mag ABI messages. - */ -#ifndef AHRS_DCM_IMU_ID -#define AHRS_DCM_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 dcm propagation.") - /* timestamp in usec when last callback was received */ - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_dcm.status == AHRS_DCM_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_dcm_propagate((struct Int32Rates*)gyro, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - if (ahrs_dcm.status == AHRS_DCM_RUNNING) { - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); - ahrs_dcm_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_dcm.status == AHRS_DCM_RUNNING) { - ahrs_dcm_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_dcm.status == AHRS_DCM_RUNNING) { - ahrs_dcm_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_dcm.status != AHRS_DCM_RUNNING) { - ahrs_dcm_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, - (struct Int32Vect3*)lp_mag); - } -} - - static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) { for (int i=0; i<3; i++) { @@ -166,40 +100,24 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) } } -void ahrs_dcm_register(void) +void ahrs_dcm_init(void) { - ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps); -} - -void ahrs_dcm_init(struct OrientationReps* body_to_imu) -{ - /* save body_to_imu pointer */ - ahrs_dcm.body_to_imu = body_to_imu; - ahrs_dcm.status = AHRS_DCM_UNINIT; + ahrs_dcm.is_aligned = FALSE; - /* Set ltp_to_imu so that body is zero */ - memcpy(&ahrs_dcm.ltp_to_imu_euler, orientationGetEulers_f(ahrs_dcm.body_to_imu), - sizeof(struct FloatEulers)); + /* init ltp_to_imu euler with zero */ + FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler); FLOAT_RATES_ZERO(ahrs_dcm.imu_rate); /* set inital filter dcm */ - set_dcm_matrix_from_rmat(orientationGetRMat_f(ahrs_dcm.body_to_imu)); + set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); ahrs_dcm.gps_speed = 0; ahrs_dcm.gps_acceleration = 0; ahrs_dcm.gps_course = 0; ahrs_dcm.gps_course_valid = FALSE; ahrs_dcm.gps_age = 100; - - /* - * Subscribe to scaled IMU measurements and attach callbacks - */ - AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); } bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, @@ -452,7 +370,7 @@ void Normalize(void) // Reset on trouble if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! - set_dcm_matrix_from_rmat(orientationGetRMat_f(ahrs_dcm.body_to_imu)); + set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); problem = FALSE; } } @@ -599,7 +517,7 @@ void Matrix_update(float dt) */ static inline void set_body_orientation_and_rates(void) { - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(ahrs_dcm.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu); struct FloatRates body_rate; float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate); @@ -645,3 +563,18 @@ static inline void compute_ahrs_representations(void) { */ } +void ahrs_dcm_set_body_to_imu(struct OrientationReps* body_to_imu) +{ + ahrs_dcm_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); +} + +void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat* q_b2i) +{ + orientationSetQuat_f(&ahrs_dcm.body_to_imu, q_b2i); + + if (!ahrs_dcm.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_dcm.ltp_to_imu_euler, orientationGetEulers_f(&ahrs_dcm.body_to_imu), + sizeof(struct FloatEulers)); + } +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index 67ebb0be98..838b65ad46 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -26,6 +26,7 @@ #include #include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" enum AhrsDCMStatus { AHRS_DCM_UNINIT, @@ -45,14 +46,13 @@ struct AhrsFloatDCM { bool_t gps_course_valid; uint8_t gps_age; - struct OrientationReps* body_to_imu; + struct OrientationReps body_to_imu; enum AhrsDCMStatus status; + bool_t is_aligned; }; extern struct AhrsFloatDCM ahrs_dcm; -#define DefaultAhrsImpl ahrs_dcm - // DCM Parameters //#define Kp_ROLLPITCH 0.2 @@ -79,8 +79,9 @@ extern int renorm_blowup_count; extern float imu_health; #endif -extern void ahrs_dcm_register(void); -extern void ahrs_dcm_init(struct OrientationReps* body_to_imu); +extern void ahrs_dcm_init(void); +extern void ahrs_dcm_set_body_to_imu(struct OrientationReps* body_to_imu); +extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat* q_b2i); extern bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag); extern void ahrs_dcm_propagate(struct Int32Rates* gyro, float dt); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c new file mode 100644 index 0000000000..9cf35b009e --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2015 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, see + * . + */ + +/** + * @file subsystems/ahrs/ahrs_float_dcm_wrapper.c + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#include "subsystems/ahrs/ahrs_float_dcm_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "state.h" + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_DCM_IMU_ID +#define AHRS_DCM_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 abi_event body_to_imu_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 dcm propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_dcm.is_aligned) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_dcm_propagate((struct Int32Rates*)gyro, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.") +PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + if (ahrs_dcm.is_aligned) { + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + ahrs_dcm_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_dcm.is_aligned) { + ahrs_dcm_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_dcm.is_aligned) { + ahrs_dcm_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_dcm.is_aligned) { + ahrs_dcm_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, + (struct Int32Vect3*)lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + const struct FloatQuat* q_b2i_f) +{ + ahrs_dcm_set_body_to_imu_quat((struct FloatQuat*)q_b2i_f); +} + +void ahrs_dcm_register(void) +{ + ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); + AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h new file mode 100644 index 0000000000..8c9ba605be --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2015 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, see + * . + */ + +/** + * @file subsystems/ahrs/ahrs_float_dcm_wrapper.h + * + * Paparazzi specific wrapper to run floating point DCM filter. + */ + +#ifndef AHRS_FLOAT_DCM_WRAPPER_H +#define AHRS_FLOAT_DCM_WRAPPER_H + +#include "subsystems/ahrs/ahrs_float_dcm.h" + +#define DefaultAhrsImpl ahrs_dcm + +extern void ahrs_dcm_register(void); + +#endif /* AHRS_FLOAT_DCM_WRAPPER_H */