diff --git a/conf/abi.xml b/conf/abi.xml index 536fce48e8..45918ac3a0 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -20,6 +20,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml index a7f1c9e6c1..2ea681eb1c 100644 --- a/conf/airframes/examples/quadrotor_lisa_mx.xml +++ b/conf/airframes/examples/quadrotor_lisa_mx.xml @@ -33,7 +33,12 @@ - + + + + diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile index efa3ababb4..735f1e4b5a 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile @@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile index 1202eac965..101367d89b 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile @@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY 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/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile index 1df57d57ad..c20c52a23b 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile @@ -4,17 +4,8 @@ GX3_PORT ?= UART3 GX3_BAUD ?= B921600 -AHRS_ALIGNER_LED ?= none - AHRS_CFLAGS = -DUSE_AHRS AHRS_CFLAGS += -DUSE_IMU -AHRS_CFLAGS += -DUSE_IMU_FLOAT - -AHRS_CFLAGS += -DUSE_AHRS_ALIGNER - -ifneq ($(AHRS_ALIGNER_LED),none) - AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -endif AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\" AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile index 6f2de55e7a..c7574641d4 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile @@ -22,9 +22,10 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\" AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile index 9f45359b39..be6fb91b62 100644 --- a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile @@ -19,10 +19,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile index b195db2edd..0e349069a8 100644 --- a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile @@ -18,10 +18,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY 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/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile b/conf/firmwares/subsystems/shared/ahrs_gx3.makefile similarity index 71% rename from conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile rename to conf/firmwares/subsystems/shared/ahrs_gx3.makefile index b5cd5a6f82..6223239c8c 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_gx3.makefile @@ -1,18 +1,11 @@ -# Rotorcraft AHRS module for GX3 +# AHRS subsystem for GX3 # 2013, Utah State University, http://aggieair.usu.edu/ GX3_PORT ?= UART3 GX3_BAUD ?= B921600 -AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS -AHRS_CFLAGS += -DAHRS_FLOAT AHRS_CFLAGS += -DUSE_IMU -AHRS_CFLAGS += -DUSE_IMU_FLOAT - -ifneq ($(AHRS_ALIGNER_LED),none) - AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -endif AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\" AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile index 80f9d1e062..656d4dd064 100644 --- a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile @@ -17,9 +17,10 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\" AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile index 44357ace4d..c574244c72 100644 --- a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile @@ -19,9 +19,10 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\" AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/modules/ahrs_chimu_spi.xml b/conf/modules/ahrs_chimu_spi.xml index 32c1038581..dd57db80cc 100644 --- a/conf/modules/ahrs_chimu_spi.xml +++ b/conf/modules/ahrs_chimu_spi.xml @@ -19,7 +19,7 @@ - ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\" + ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\" diff --git a/conf/modules/ahrs_chimu_uart.xml b/conf/modules/ahrs_chimu_uart.xml index 2a203d69e9..78767fbfd3 100644 --- a/conf/modules/ahrs_chimu_uart.xml +++ b/conf/modules/ahrs_chimu_uart.xml @@ -23,7 +23,7 @@ - ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\" + ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\" diff --git a/conf/settings/estimation/ahrs_float_cmpl.xml b/conf/settings/estimation/ahrs_float_cmpl.xml index 950b651e70..bcc12458b4 100644 --- a/conf/settings/estimation/ahrs_float_cmpl.xml +++ b/conf/settings/estimation/ahrs_float_cmpl.xml @@ -4,11 +4,11 @@ - - - - - + + + + + diff --git a/conf/settings/estimation/ahrs_float_mlkf.xml b/conf/settings/estimation/ahrs_float_mlkf.xml index 68cf7b0012..ba402dead9 100644 --- a/conf/settings/estimation/ahrs_float_mlkf.xml +++ b/conf/settings/estimation/ahrs_float_mlkf.xml @@ -4,9 +4,9 @@ - - - + + + diff --git a/conf/settings/estimation/ahrs_int_cmpl_euler.xml b/conf/settings/estimation/ahrs_int_cmpl_euler.xml index 9c6bebad52..7562b41d46 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_euler.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_euler.xml @@ -4,7 +4,7 @@ - + diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 600de04e88..f1d8478b5f 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -4,11 +4,11 @@ - - - - - + + + + + diff --git a/sw/airborne/arch/sim/sim_ahrs.c b/sw/airborne/arch/sim/sim_ahrs.c index 559c25688e..1750099d15 100644 --- a/sw/airborne/arch/sim/sim_ahrs.c +++ b/sw/airborne/arch/sim/sim_ahrs.c @@ -15,7 +15,6 @@ float sim_psi; ///< in radians float sim_p; ///< in radians/s float sim_q; ///< in radians/s float sim_r; ///< in radians/s -bool_t ahrs_sim_available; // Updates from Ocaml sim @@ -25,8 +24,6 @@ value provide_attitude(value phi, value theta, value psi) sim_theta = Double_val(theta); sim_psi = - Double_val(psi) + M_PI / 2.; - ahrs_sim_available = TRUE; - return Val_unit; } @@ -36,8 +33,6 @@ value provide_rates(value p, value q, value r) sim_q = Double_val(q); sim_r = Double_val(r); - ahrs_sim_available = TRUE; - return Val_unit; } diff --git a/sw/airborne/boards/ardrone/actuators_at.c b/sw/airborne/boards/ardrone/actuators_at.c index 060b8f8c92..20b19c4799 100644 --- a/sw/airborne/boards/ardrone/actuators_at.c +++ b/sw/airborne/boards/ardrone/actuators_at.c @@ -43,28 +43,28 @@ void actuators_set(pprz_t commands[]) float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ); //Starting engine - if (thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT - || ahrs_impl.control_state == CTRL_LANDED)) { + if(thrust > 0 && (ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT + || ahrs_ardrone2.control_state == CTRL_LANDED)) { at_com_send_ref(REF_TAKEOFF); } //Check emergency or stop engine - if ((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0) { + if ((ahrs_ardrone2.state & ARDRONE_EMERGENCY_MASK) != 0) { at_com_send_ref(REF_EMERGENCY); - } else if (thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT - || ahrs_impl.control_state == CTRL_LANDED)) { + } else if(thrust < -0.9 && !(ahrs_ardrone2.control_state == CTRL_DEFAULT || + ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED)) { at_com_send_ref(0); } //Calibration - if ((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING - || ahrs_impl.control_state == CTRL_HOVERING)) { + if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && + (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) { at_com_send_calib(0); } //Moving - if ((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING - || ahrs_impl.control_state == CTRL_HOVERING)) { + if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && + (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) { at_com_send_pcmd(1, thrust, roll, pitch, yaw); } diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index e791cdb8e6..24ff5a3e4a 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -157,7 +157,7 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev) 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 (imu_lost) { mde = 5; } uint16_t val = imu_lost_counter; pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 1f87d7cc62..7a77b4ce43 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -126,6 +126,11 @@ PRINT_CONFIG_VAR(MODULES_FREQUENCY) PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY) #endif + +#define __DefaultAhrsRegister(_x) _x ## _register() +#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x) +#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl) + #if USE_AHRS && USE_IMU #ifdef AHRS_PROPAGATE_FREQUENCY @@ -144,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); @@ -195,8 +200,15 @@ void init_ap(void) #endif #if USE_AHRS +#if defined SITL && !USE_NPS && !USE_INFRARED + ahrs_sim_init(); +#else ahrs_init(); + DefaultAhrsRegister(); #endif +#endif + + ins_init(); #if USE_AHRS && USE_IMU register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); @@ -206,8 +218,6 @@ void init_ap(void) baro_init(); #endif - ins_init(); - /************* Links initialization ***************/ #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK link_mcu_init(); @@ -261,6 +271,11 @@ void init_ap(void) ap_state->command_roll_trim = COMMAND_ROLL_TRIM; ap_state->command_pitch_trim = COMMAND_PITCH_TRIM; ap_state->command_yaw_trim = COMMAND_YAW_TRIM; + +#if USE_IMU + // send body_to_imu from here for now + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); +#endif } @@ -615,9 +630,8 @@ void sensors_task(void) #endif // USE_IMU //FIXME: this is just a kludge -#if USE_AHRS && defined SITL && !USE_NPS - // dt is not really used in ahrs_sim - ahrs_propagate(1. / PERIODIC_FREQUENCY); +#if USE_AHRS && defined SITL && !USE_NPS && !USE_INFRARED + update_ahrs_from_sim(); #endif #if USE_GPS @@ -738,10 +752,10 @@ void event_task_ap(void) #if USE_GPS static inline void on_gps_solution(void) { - ins_update_gps(); #if USE_AHRS ahrs_update_gps(); #endif + ins_update_gps(); #ifdef GPS_TRIGGERED_FUNCTION GPS_TRIGGERED_FUNCTION(); #endif @@ -749,69 +763,53 @@ static inline void on_gps_solution(void) #endif -#if USE_AHRS #if USE_IMU static inline void on_accel_event(void) { -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; // current timestamp uint32_t now_ts = get_sys_time_usec(); - // 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_CORRECT_FREQUENCY for AHRS accel update.") - PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) - const float dt = 1. / AHRS_CORRECT_FREQUENCY; -#endif imu_scale_accel(&imu); - if (ahrs.status != AHRS_UNINIT) { - ahrs_update_accel(dt); - } + + AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel); } static inline void on_gyro_event(void) { -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; // current timestamp uint32_t now_ts = get_sys_time_usec(); - // 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 AHRS/INS propagation.") - PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); -#endif ahrs_timeout_counter = 0; imu_scale_gyro(&imu); + 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_UNINIT) { + if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) { ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { - ahrs_align(); - } return; } #endif - ahrs_propagate(dt); - #if defined SITL && USE_NPS if (nps_bypass_ahrs) { sim_overwrite_ahrs(); } #endif +#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 + ins_propagate(dt); + #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; #endif @@ -821,28 +819,12 @@ static inline void on_gyro_event(void) static inline void on_mag_event(void) { #if USE_MAGNETOMETER -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; // current timestamp uint32_t now_ts = get_sys_time_usec(); - // 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_MAG_CORRECT_FREQUENCY for AHRS mag update.") - PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) - const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); -#endif imu_scale_mag(&imu); - if (ahrs.status == AHRS_RUNNING) { - ahrs_update_mag(dt); - } + AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag); #endif } #endif // USE_IMU - -#endif // USE_AHRS diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 9795b05802..b23a9803a0 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -92,7 +92,7 @@ bool_t autopilot_detect_ground_once; #include "subsystems/ahrs.h" static inline int ahrs_is_aligned(void) { - return (ahrs.status == AHRS_RUNNING); + return DefaultAhrsImpl.is_aligned; } #else PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL") diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 64d786825f..20493daac9 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -69,7 +69,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO #include "firmwares/rotorcraft/guidance.h" #include "subsystems/ahrs.h" +#if USE_AHRS_ALIGNER #include "subsystems/ahrs/ahrs_aligner.h" +#endif #include "subsystems/ins.h" #include "state.h" @@ -112,6 +114,10 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t #endif #endif +#define __DefaultAhrsRegister(_x) _x ## _register() +#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x) +#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl) + static inline void on_gyro_event(void); static inline void on_accel_event(void); static inline void on_gps_event(void); @@ -160,10 +166,14 @@ STATIC_INLINE void main_init(void) baro_init(); #endif imu_init(); +#if USE_AHRS_ALIGNER ahrs_aligner_init(); +#endif ahrs_init(); ins_init(); + DefaultAhrsRegister(); + #if USE_GPS gps_init(); #endif @@ -190,6 +200,9 @@ STATIC_INLINE void main_init(void) #if USE_BARO_BOARD baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL); #endif + + // send body_to_imu from here for now + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); } STATIC_INLINE void handle_periodic_tasks(void) @@ -343,61 +356,48 @@ STATIC_INLINE void main_event(void) } -static inline void on_accel_event(void) -{ -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; +static inline void on_accel_event( void ) { // current timestamp uint32_t now_ts = get_sys_time_usec(); - // dt between this and last callback - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; -#else - PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.") - PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) - const float dt = 1. / (AHRS_CORRECT_FREQUENCY); -#endif imu_scale_accel(&imu); - if (ahrs.status != AHRS_UNINIT) { - ahrs_update_accel(dt); - } + AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel); } -static inline void on_gyro_event(void) -{ -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; +static inline void on_gyro_event( void ) { // current timestamp uint32_t now_ts = get_sys_time_usec(); + + imu_scale_gyro(&imu); + + AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev); + +#if USE_AHRS_ALIGNER + if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) { + ahrs_aligner_run(); + return; + } +#endif + +#ifdef SITL + if (nps_bypass_ahrs) sim_overwrite_ahrs(); +#endif + +#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 AHRS/INS propagation.") - PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) +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 + ins_propagate(dt); - imu_scale_gyro(&imu); - - if (ahrs.status == AHRS_UNINIT) { - ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { - ahrs_align(); - } - } else { - ahrs_propagate(dt); -#ifdef SITL - if (nps_bypass_ahrs) { sim_overwrite_ahrs(); } -#endif - ins_propagate(dt); - } #ifdef USE_VEHICLE_INTERFACE vi_notify_imu_available(); #endif @@ -417,27 +417,10 @@ static inline void on_gps_event(void) static inline void on_mag_event(void) { imu_scale_mag(&imu); - -#if USE_MAGNETOMETER -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; // current timestamp uint32_t now_ts = get_sys_time_usec(); - // 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_MAG_CORRECT_FREQUENCY for AHRS mag update.") - PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) - const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); -#endif - if (ahrs.status == AHRS_RUNNING) { - ahrs_update_mag(dt); - } -#endif // USE_MAGNETOMETER + AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag); #ifdef USE_VEHICLE_INTERFACE vi_notify_mag_available(); diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index b562b348c4..f6f807152e 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -296,7 +296,7 @@ static inline void mavlink_send_heartbeat(void) #else uint8_t mav_type = MAV_TYPE_QUADROTOR; #endif - if (ahrs.status == AHRS_RUNNING) { + if (DefaultAhrsImpl.is_aligned) { if (kill_throttle) { mav_state = MAV_STATE_STANDBY; } diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c index 01da5dfe78..a9d3a95d59 100644 --- a/sw/airborne/modules/geo_mag/geo_mag.c +++ b/sw/airborne/modules/geo_mag/geo_mag.c @@ -76,11 +76,11 @@ void geo_mag_event(void) // copy to ahrs #ifdef AHRS_FLOAT - VECT3_COPY(ahrs_impl.mag_h, geo_mag.vect); + VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect); #else // convert to MAG_BFP and copy to ahrs - VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), MAG_BFP_OF_REAL(geo_mag.vect.y), - MAG_BFP_OF_REAL(geo_mag.vect.z)); + VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), + MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z)); #endif geo_mag.ready = TRUE; diff --git a/sw/airborne/modules/ins/ahrs_chimu.h b/sw/airborne/modules/ins/ahrs_chimu.h new file mode 100644 index 0000000000..6ab50e450a --- /dev/null +++ b/sw/airborne/modules/ins/ahrs_chimu.h @@ -0,0 +1,43 @@ +/* + * 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 modules/ins/ahrs_chimu.h + */ + +#ifndef AHRS_CHIMU_H +#define AHRS_CHIMU_H + +#include "modules/ins/ins_module.h" +#include "subsystems/ahrs.h" + +struct AhrsChimu { + bool_t is_aligned; +}; + +extern struct AhrsChimu ahrs_chimu; + +#define DefaultAhrsImpl ahrs_chimu + +extern void ahrs_chimu_register(void); +extern void ahrs_chimu_init(void); + +#endif diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index 75b0167b02..40b705d18c 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -26,6 +26,7 @@ #include "ins_module.h" #include "imu_chimu.h" +#include "ahrs_chimu.h" #include "led.h" @@ -34,9 +35,18 @@ CHIMU_PARSER_DATA CHIMU_DATA; INS_FORMAT ins_roll_neutral; INS_FORMAT ins_pitch_neutral; -void ahrs_init(void) +struct AhrsChimu ahrs_chimu; + +void ahrs_chimu_update_gps(void); + +void ahrs_chimu_register(void) { - ahrs.status = AHRS_UNINIT; + ahrs_register_impl(ahrs_chimu_init, ahrs_chimu_update_gps); +} + +void ahrs_chimu_init(void) +{ + ahrs_chimu.is_aligned = FALSE; // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 }; uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI @@ -68,10 +78,6 @@ void ahrs_init(void) InsSend(rate, 12); } -void ahrs_align(void) -{ - ahrs.status = AHRS_RUNNING; -} void parse_ins_msg(void) { @@ -98,6 +104,8 @@ void parse_ins_msg(void) 0. }; // FIXME rate r stateSetBodyRates_f(&rates); + //FIXME + ahrs_chimu.is_aligned = TRUE; } else if (CHIMU_DATA.m_MsgID == 0x02) { #if CHIMU_DOWNLINK_IMMEDIATE RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], @@ -109,7 +117,7 @@ void parse_ins_msg(void) } -void ahrs_update_gps(void) +void ahrs_chimu_update_gps(void) { // Send SW Centripetal Corrections uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 }; @@ -130,11 +138,3 @@ void ahrs_update_gps(void) // Downlink Send } - -void ahrs_propagate(float dt __attribute__((unused))) -{ -} - -void ahrs_update_accel(float dt __attribute__((unused))) -{ -} diff --git a/sw/airborne/modules/ins/ahrs_chimu_uart.c b/sw/airborne/modules/ins/ahrs_chimu_uart.c index d45708e0c2..b993ee3f92 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_uart.c +++ b/sw/airborne/modules/ins/ahrs_chimu_uart.c @@ -30,9 +30,16 @@ CHIMU_PARSER_DATA CHIMU_DATA; INS_FORMAT ins_roll_neutral; INS_FORMAT ins_pitch_neutral; -void ahrs_init(void) +struct AhrsChimu ahrs_chimu; + +void ahrs_chimu_register(void) { - ahrs.status = AHRS_UNINIT; + ahrs_register_impl(ahrs_chimu_init, NULL); +} + +void ahrs_chimu_init(void) +{ + ahrs_chimu.is_aligned = FALSE; uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 }; uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI @@ -61,10 +68,6 @@ void ahrs_init(void) CHIMU_Checksum(rate, 12); InsSend(rate, 12); } -void ahrs_align(void) -{ - ahrs.status = AHRS_RUNNING; -} void parse_ins_msg(void) @@ -86,6 +89,7 @@ void parse_ins_msg(void) CHIMU_DATA.m_attitude.euler.psi }; stateSetNedToBodyEulers_f(&att); + ahrs_chimu.is_aligned = TRUE; #if CHIMU_DOWNLINK_IMMEDIATE DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); @@ -95,7 +99,3 @@ void parse_ins_msg(void) } } } - -void ahrs_update_gps(void) -{ -} diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c index 87bc8eff41..f3ed0cd50c 100644 --- a/sw/airborne/modules/sensors/mag_hmc58xx.c +++ b/sw/airborne/modules/sensors/mag_hmc58xx.c @@ -32,7 +32,7 @@ #if MODULE_HMC58XX_UPDATE_AHRS #include "subsystems/imu.h" -#include "subsystems/ahrs.h" +#include "subsystems/abi.h" #ifndef HMC58XX_CHAN_X #define HMC58XX_CHAN_X 0 @@ -60,20 +60,13 @@ void mag_hmc58xx_module_periodic(void) void mag_hmc58xx_module_event(void) { -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) - PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; -#else - PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") - PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) - const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); -#endif - hmc58xx_event(&mag_hmc58xx); -#if MODULE_HMC58XX_UPDATE_AHRS if (mag_hmc58xx.data_available) { +#if MODULE_HMC58XX_UPDATE_AHRS + // current timestamp + uint32_t now_ts = get_sys_time_usec(); + // set channel order struct Int32Vect3 mag = { (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]), @@ -84,26 +77,16 @@ void mag_hmc58xx_module_event(void) VECT3_COPY(imu.mag_unscaled, mag); // scale vector imu_scale_mag(&imu); - // update ahrs - if (ahrs.status == AHRS_RUNNING) { -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) - // current timestamp - uint32_t now_ts = get_sys_time_usec(); - // dt between this and last callback in seconds - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; -#endif - ahrs_update_mag(dt); - } - mag_hmc58xx.data_available = FALSE; - } + + AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag); #endif #if MODULE_HMC58XX_SYNC_SEND - if (mag_hmc58xx.data_available) { mag_hmc58xx_report(); - mag_hmc58xx.data_available = FALSE; - } #endif +#if MODULE_HMC58XX_UPDATE_AHRS || MODULE_HMC58XX_SYNC_SEND + mag_hmc58xx.data_available = FALSE; +#endif + } } void mag_hmc58xx_report(void) diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 83240cee41..953635ce0c 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -103,5 +103,12 @@ #define AGL_SONAR_NPS_ID 3 #endif +/* + * IDs of magnetometer sensors (including IMUs with mag) + */ + +#ifndef MAG_HMC58XX_SENDER_ID +#define MAG_HMC58XX_SENDER_ID 2 +#endif #endif /* ABI_SENDER_IDS_H */ diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c index 229037001b..cb05e487a1 100644 --- a/sw/airborne/subsystems/ahrs.c +++ b/sw/airborne/subsystems/ahrs.c @@ -29,12 +29,23 @@ struct Ahrs ahrs; -// weak functions, used if not explicitly provided by implementation +void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps) +{ + ahrs.init = init; + ahrs.update_gps = update_gps; -void WEAK ahrs_propagate(float dt __attribute__((unused))) {} + ahrs.init(); +} -void WEAK ahrs_update_accel(float dt __attribute__((unused))) {} +void ahrs_init(void) +{ + ahrs.init = NULL; + ahrs.update_gps = NULL; +} -void WEAK ahrs_update_mag(float dt __attribute__((unused))) {} - -void WEAK ahrs_update_gps(void) {} +void ahrs_update_gps(void) +{ + if (ahrs.update_gps != NULL) { + ahrs.update_gps(); + } +} diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 0df80faec2..8d192febf5 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -28,61 +28,35 @@ #define AHRS_H #include "std.h" -#include "math/pprz_algebra_int.h" -#include "math/pprz_algebra_float.h" -#include "state.h" - -#define AHRS_UNINIT 0 -#define AHRS_RUNNING 1 /* underlying includes (needed for parameters) */ #ifdef AHRS_TYPE_H #include AHRS_TYPE_H #endif +typedef void (*AhrsInit)(void); +typedef void (*AhrsUpdateGps)(void); + /** Attitude and Heading Reference System state */ struct Ahrs { - uint8_t status; ///< status of the AHRS, AHRS_UNINIT or AHRS_RUNNING + /* function pointers to actual implementation, set by ahrs_register_impl */ + AhrsInit init; + AhrsUpdateGps update_gps; }; /** global AHRS state */ extern struct Ahrs ahrs; +extern void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps); + /** AHRS initialization. Called at startup. - * Needs to be implemented by each AHRS algorithm. + * Initialized the global AHRS struct. */ extern void ahrs_init(void); -/** Aligns the AHRS. Called after ahrs_aligner has run to set initial attitude and biases. - * Must set the ahrs status to AHRS_RUNNING. - * Needs to be implemented by each AHRS algorithm. - */ -extern void ahrs_align(void); - -/** Propagation. Usually integrates the gyro rates to angles. - * Reads the global #imu data struct. - * Does nothing if not implemented by specific AHRS algorithm. - * @param dt time difference since last propagation in seconds - */ -extern void ahrs_propagate(float dt); - -/** Update AHRS state with accerleration measurements. - * Reads the global #imu data struct. - * Does nothing if not implemented by specific AHRS algorithm. - * @param dt time difference since last update in seconds - */ -extern void ahrs_update_accel(float dt); - -/** Update AHRS state with magnetometer measurements. - * Reads the global #imu data struct. - * Does nothing if not implemented by specific AHRS algorithm. - * @param dt time difference since last update in seconds - */ -extern void ahrs_update_mag(float dt); - /** Update AHRS state with GPS measurements. + * Calls implementation if registered. * Reads the global #gps data struct. - * Does nothing if not implemented by specific AHRS algorithm. */ extern void ahrs_update_gps(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c index 2ca53fc394..d0a7c2ab6c 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c @@ -31,6 +31,8 @@ #include /* for abs() */ #include "subsystems/imu.h" #include "led.h" +#include "subsystems/abi.h" +#include "mcu_periph/sys_time.h" struct AhrsAligner ahrs_aligner; @@ -45,21 +47,23 @@ static uint32_t samples_idx; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_aligner(struct transport_tx *trans, struct link_device *dev) { +static void send_aligner(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID, - &ahrs_aligner.lp_gyro.p, - &ahrs_aligner.lp_gyro.q, - &ahrs_aligner.lp_gyro.r, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r, - &ahrs_aligner.noise, - &ahrs_aligner.low_noise_cnt, - &ahrs_aligner.status); + &ahrs_aligner.lp_gyro.p, + &ahrs_aligner.lp_gyro.q, + &ahrs_aligner.lp_gyro.r, + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r, + &ahrs_aligner.noise, + &ahrs_aligner.low_noise_cnt, + &ahrs_aligner.status); } #endif -void ahrs_aligner_init(void) { +void ahrs_aligner_init(void) +{ ahrs_aligner.status = AHRS_ALIGNER_RUNNING; INT_RATES_ZERO(gyro_sum); @@ -81,7 +85,8 @@ void ahrs_aligner_init(void) { #define LOW_NOISE_TIME 5 #endif -void ahrs_aligner_run(void) { +void ahrs_aligner_run(void) +{ RATES_ADD(gyro_sum, imu.gyro); VECT3_ADD(accel_sum, imu.accel); @@ -96,15 +101,16 @@ void ahrs_aligner_run(void) { if (samples_idx >= SAMPLES_NB) { int32_t avg_ref_sensor = accel_sum.z; - if ( avg_ref_sensor >= 0) + if (avg_ref_sensor >= 0) { avg_ref_sensor += SAMPLES_NB / 2; - else + } else { avg_ref_sensor -= SAMPLES_NB / 2; + } avg_ref_sensor /= SAMPLES_NB; ahrs_aligner.noise = 0; int i; - for (i=0; i 0) - ahrs_aligner.low_noise_cnt--; + } else if (ahrs_aligner.low_noise_cnt > 0) { + ahrs_aligner.low_noise_cnt--; + } if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) { ahrs_aligner.status = AHRS_ALIGNER_LOCKED; #ifdef AHRS_ALIGNER_LED LED_ON(AHRS_ALIGNER_LED); #endif + uint32_t now_ts = get_sys_time_usec(); + AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, now_ts, &ahrs_aligner.lp_gyro, + &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); } } diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index c02d2b7e93..5b567457b7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -32,6 +32,7 @@ # include #endif +#include "subsystems/ahrs.h" #include "ahrs_ardrone2.h" #include "state.h" #include "math/pprz_algebra_float.h" @@ -42,79 +43,87 @@ #include "subsystems/gps/gps_ardrone2.h" #endif -struct AhrsARDrone ahrs_impl; -struct AhrsAligner ahrs_aligner; +struct AhrsARDrone ahrs_ardrone2; unsigned char buffer[4096]; //Packet buffer #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev) { +static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_AHRS_ARDRONE2(trans, dev, AC_ID, - &ahrs_impl.state, - &ahrs_impl.control_state, - &ahrs_impl.eulers.phi, - &ahrs_impl.eulers.theta, - &ahrs_impl.eulers.psi, - &ahrs_impl.speed.x, - &ahrs_impl.speed.y, - &ahrs_impl.speed.z, - &ahrs_impl.accel.x, - &ahrs_impl.accel.y, - &ahrs_impl.accel.z, - &ahrs_impl.altitude, - &ahrs_impl.battery); + &ahrs_ardrone2.state, + &ahrs_ardrone2.control_state, + &ahrs_ardrone2.eulers.phi, + &ahrs_ardrone2.eulers.theta, + &ahrs_ardrone2.eulers.psi, + &ahrs_ardrone2.speed.x, + &ahrs_ardrone2.speed.y, + &ahrs_ardrone2.speed.z, + &ahrs_ardrone2.accel.x, + &ahrs_ardrone2.accel.y, + &ahrs_ardrone2.accel.z, + &ahrs_ardrone2.altitude, + &ahrs_ardrone2.battery); } #endif -void ahrs_init(void) { +void ahrs_ardrone2_register(void) +{ + ahrs_register_impl(ahrs_ardrone2_init, NULL); +} + +void ahrs_ardrone2_init(void) +{ init_at_com(); //Set navdata_demo to FALSE and flat trim the ar drone at_com_send_config("general:navdata_demo", "FALSE"); at_com_send_ftrim(); - ahrs.status = AHRS_RUNNING; + ahrs_ardrone2.is_aligned = TRUE; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2); #endif } -void ahrs_align(void) { - -} - #ifdef ARDRONE2_DEBUG -static void dump(const void *_b, size_t s) { +static void dump(const void *_b, size_t s) +{ const unsigned char *b = _b; size_t n; - for(n = 0; n < s; ++n) { + for (n = 0; n < s; ++n) { printf("%02x ", b[n]); - if (n%16 == 15) + if (n % 16 == 15) { printf("\n"); + } } - if (n%16 != 0) + if (n % 16 != 0) { printf("\n"); + } } #endif -void ahrs_propagate(float dt __attribute__((unused))) { +void ahrs_ardrone2_propagate(void) +{ int l; //Recieve the main packet l = at_com_recieve_navdata(buffer); - navdata_t* main_packet = (navdata_t*) &buffer; + navdata_t *main_packet = (navdata_t *) &buffer; #ifdef ARDRONE2_DEBUG - if (l < 0) + if (l < 0) { printf("errno = %d\n", errno); + } #endif //When this isn't a valid packet return - if(l < 0 || main_packet->header != NAVDATA_HEADER) + if (l < 0 || main_packet->header != NAVDATA_HEADER) { return; + } #ifdef ARDRONE2_DEBUG printf("Read %d\n", l); @@ -122,81 +131,75 @@ void ahrs_propagate(float dt __attribute__((unused))) { #endif //Set the state - ahrs_impl.state = main_packet->ardrone_state; + ahrs_ardrone2.state = main_packet->ardrone_state; //Init the option - navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]); + navdata_option_t *navdata_option = (navdata_option_t *) & (main_packet->options[0]); bool_t full_read = FALSE; //The possible packets - navdata_demo_t* navdata_demo; - navdata_gps_t* navdata_gps; - navdata_phys_measures_t* navdata_phys_measures; + navdata_demo_t *navdata_demo; + navdata_gps_t *navdata_gps; + navdata_phys_measures_t *navdata_phys_measures; //Read the navdata until packet is fully readed - while(!full_read && navdata_option->size > 0) { + while (!full_read && navdata_option->size > 0) { #ifdef ARDRONE2_DEBUG - printf ("tag = %d\n", navdata_option->tag); + printf("tag = %d\n", navdata_option->tag); #endif //Check the tag for the right option - switch(navdata_option->tag) { - case 0: //NAVDATA_DEMO - navdata_demo = (navdata_demo_t*) navdata_option; + switch (navdata_option->tag) { + case 0: //NAVDATA_DEMO + navdata_demo = (navdata_demo_t *) navdata_option; - //Set the AHRS state - ahrs_impl.control_state = navdata_demo->ctrl_state >> 16; - ahrs_impl.eulers.phi = navdata_demo->phi; - ahrs_impl.eulers.theta = navdata_demo->theta; - ahrs_impl.eulers.psi = navdata_demo->psi; - ahrs_impl.speed.x = navdata_demo->vx / 1000; - ahrs_impl.speed.y = navdata_demo->vy / 1000; - ahrs_impl.speed.z = navdata_demo->vz / 1000; - ahrs_impl.altitude = navdata_demo->altitude / 10; - ahrs_impl.battery = navdata_demo->vbat_flying_percentage; + //Set the AHRS state + ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16; + ahrs_ardrone2.eulers.phi = navdata_demo->phi; + ahrs_ardrone2.eulers.theta = navdata_demo->theta; + ahrs_ardrone2.eulers.psi = navdata_demo->psi; + ahrs_ardrone2.speed.x = navdata_demo->vx / 1000; + ahrs_ardrone2.speed.y = navdata_demo->vy / 1000; + ahrs_ardrone2.speed.z = navdata_demo->vz / 1000; + ahrs_ardrone2.altitude = navdata_demo->altitude / 10; + ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage; - //Set the ned to body eulers - struct FloatEulers angles; - angles.theta = navdata_demo->theta/180000.*M_PI; - angles.psi = navdata_demo->psi/180000.*M_PI; - angles.phi = navdata_demo->phi/180000.*M_PI; - stateSetNedToBodyEulers_f(&angles); + //Set the ned to body eulers + struct FloatEulers angles; + angles.theta = navdata_demo->theta / 180000.*M_PI; + angles.psi = navdata_demo->psi / 180000.*M_PI; + angles.phi = navdata_demo->phi / 180000.*M_PI; + stateSetNedToBodyEulers_f(&angles); - //Update the electrical supply - electrical.vsupply = navdata_demo->vbat_flying_percentage; - break; - case 3: //NAVDATA_PHYS_MEASURES - navdata_phys_measures = (navdata_phys_measures_t*) navdata_option; + //Update the electrical supply + electrical.vsupply = navdata_demo->vbat_flying_percentage; + break; + case 3: //NAVDATA_PHYS_MEASURES + navdata_phys_measures = (navdata_phys_measures_t *) navdata_option; - //Set the AHRS accel state - INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000) - break; + //Set the AHRS accel state + INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000) + break; #ifdef USE_GPS_ARDRONE2 - case 27: //NAVDATA_GPS + case 27: //NAVDATA_GPS # ifdef ARDRONE2_DEBUG - dump(navdata_option, navdata_option->size); + dump(navdata_option, navdata_option->size); # endif - navdata_gps = (navdata_gps_t*) navdata_option; + navdata_gps = (navdata_gps_t *) navdata_option; - // Send the data to the gps parser - gps_ardrone2_parse(navdata_gps); - break; + // Send the data to the gps parser + gps_ardrone2_parse(navdata_gps); + break; #endif - case 0xFFFF: //CHECKSUM - //TODO: Check the checksum - full_read = TRUE; - break; - default: + case 0xFFFF: //CHECKSUM + //TODO: Check the checksum + full_read = TRUE; + break; + default: #ifdef ARDRONE2_DEBUG - printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); + printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); #endif - break; + break; } - navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size); + navdata_option = (navdata_option_t *)((uint32_t)navdata_option + navdata_option->size); } } - -void ahrs_aligner_init(void) { -} - -void ahrs_aligner_run(void) { -} diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h index edd25a819d..7aa81ece64 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h @@ -30,10 +30,9 @@ #ifndef AHRS_ARDRONE2_H #define AHRS_ARDRONE2_H -#include "subsystems/ahrs.h" -#include "subsystems/ahrs/ahrs_aligner.h" #include "std.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_geodetic_float.h" struct AhrsARDrone { uint32_t state; // ARDRONE_STATES @@ -44,7 +43,14 @@ struct AhrsARDrone { int32_t altitude; // in cm above ground uint32_t battery; // in percentage struct Int32Quat ltp_to_imu_quat; + bool_t is_aligned; }; -extern struct AhrsARDrone ahrs_impl; +extern struct AhrsARDrone ahrs_ardrone2; + +#define DefaultAhrsImpl ahrs_ardrone2 + +extern void ahrs_ardrone2_register(void); +extern void ahrs_ardrone2_init(void); +extern void ahrs_ardrone2_propagate(void); #endif /* AHRS_ARDRONE2_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index a2a5be25a6..0db6c94fb3 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -27,11 +27,8 @@ * Propagation can be done in rotation matrix or quaternion representation. */ -#include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_float_cmpl.h" #include "subsystems/ahrs/ahrs_float_utils.h" -#include "subsystems/ahrs/ahrs_aligner.h" -#include "subsystems/imu.h" #include "math/pprz_algebra_float.h" #include "math/pprz_algebra_int.h" #include "math/pprz_simple_matrix.h" @@ -39,6 +36,7 @@ #if USE_GPS #include "subsystems/gps.h" #endif +#include "state.h" //#include "../../test/pprz_algebra_print.h" @@ -81,185 +79,140 @@ #endif -void ahrs_update_mag_full(float dt); -void ahrs_update_mag_2d(float dt); -void ahrs_update_mag_2d_dumb(void); +void ahrs_fc_update_mag_full(struct Int32Vect3 *mag, float dt); +void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt); +void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag); static inline void compute_body_orientation_and_rates(void); -struct AhrsFloatCmpl ahrs_impl; +struct AhrsFloatCmpl ahrs_fc; -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" +void ahrs_fc_init(void) +{ + ahrs_fc.status = AHRS_FC_UNINIT; + ahrs_fc.is_aligned = FALSE; -static void send_att(struct transport_tx *trans, struct link_device *dev) { - struct FloatEulers ltp_to_imu_euler; - float_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); - struct Int32Eulers euler_i; - EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); - struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); - pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - &euler_i.phi, - &euler_i.theta, - &euler_i.psi, - &(eulers_body->phi), - &(eulers_body->theta), - &(eulers_body->psi)); -} + ahrs_fc.ltp_vel_norm_valid = FALSE; + ahrs_fc.heading_aligned = FALSE; -static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_GEO_MAG(trans, dev, AC_ID, - &ahrs_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z); -} + /* init ltp_to_imu quaternion as zero/identity rotation */ + float_quat_identity(&ahrs_fc.ltp_to_imu_quat); -// TODO convert from float to int if we really need this one -/* -static void send_rmat(struct transport_tx *trans, struct link_device *dev) { - struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); - pprz_msg_send_AHRS_RMAT(trans, dev, AC_ID, - &ahrs_impl.ltp_to_imu_rmat.m[0], - &ahrs_impl.ltp_to_imu_rmat.m[1], - &ahrs_impl.ltp_to_imu_rmat.m[2], - &ahrs_impl.ltp_to_imu_rmat.m[3], - &ahrs_impl.ltp_to_imu_rmat.m[4], - &ahrs_impl.ltp_to_imu_rmat.m[5], - &ahrs_impl.ltp_to_imu_rmat.m[6], - &ahrs_impl.ltp_to_imu_rmat.m[7], - &ahrs_impl.ltp_to_imu_rmat.m[8], - &(att_rmat->m[0]), - &(att_rmat->m[1]), - &(att_rmat->m[2]), - &(att_rmat->m[3]), - &(att_rmat->m[4]), - &(att_rmat->m[5]), - &(att_rmat->m[6]), - &(att_rmat->m[7]), - &(att_rmat->m[8])); -} -*/ -#endif - -void ahrs_init(void) { - ahrs.status = AHRS_UNINIT; - ahrs_impl.ltp_vel_norm_valid = FALSE; - ahrs_impl.heading_aligned = FALSE; - - /* Set ltp_to_imu so that body is zero */ - memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu), - sizeof(struct FloatQuat)); - memcpy(&ahrs_impl.ltp_to_imu_rmat, orientationGetRMat_f(&imu.body_to_imu), - sizeof(struct FloatRMat)); - - FLOAT_RATES_ZERO(ahrs_impl.imu_rate); + FLOAT_RATES_ZERO(ahrs_fc.imu_rate); /* set default filter cut-off frequency and damping */ - ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; - ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; - ahrs_impl.mag_omega = AHRS_MAG_OMEGA; - ahrs_impl.mag_zeta = AHRS_MAG_ZETA; + ahrs_fc.accel_omega = AHRS_ACCEL_OMEGA; + ahrs_fc.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_fc.mag_omega = AHRS_MAG_OMEGA; + ahrs_fc.mag_zeta = AHRS_MAG_ZETA; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN - ahrs_impl.correct_gravity = TRUE; + ahrs_fc.correct_gravity = TRUE; #else - ahrs_impl.correct_gravity = FALSE; + ahrs_fc.correct_gravity = FALSE; #endif - ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; + ahrs_fc.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; - VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); + VECT3_ASSIGN(ahrs_fc.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); - ahrs_impl.accel_cnt = 0; - ahrs_impl.mag_cnt = 0; - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); - register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); -#endif + ahrs_fc.accel_cnt = 0; + ahrs_fc.mag_cnt = 0; } -void ahrs_align(void) { +bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); - ahrs_impl.heading_aligned = TRUE; + ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_imu_quat, lp_accel, lp_mag); + ahrs_fc.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ - ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); - ahrs_impl.heading_aligned = FALSE; + ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_imu_quat, lp_accel); + ahrs_fc.heading_aligned = FALSE; #endif /* Convert initial orientation from quat to rotation matrix representations. */ - float_rmat_of_quat(&ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); + float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); /* Compute initial body orientation */ compute_body_orientation_and_rates(); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; - RATES_COPY(bias0, ahrs_aligner.lp_gyro); - RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); - ahrs.status = AHRS_RUNNING; + RATES_COPY(bias0, *lp_gyro); + RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0); + + ahrs_fc.status = AHRS_FC_RUNNING; + ahrs_fc.is_aligned = TRUE; + + return TRUE; } -void ahrs_propagate(float dt) { +void ahrs_fc_propagate(struct Int32Rates *gyro, float dt) +{ /* converts gyro to floating point */ struct FloatRates gyro_float; - RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); + RATES_FLOAT_OF_BFP(gyro_float, *gyro); /* unbias measurement */ - RATES_SUB(gyro_float, ahrs_impl.gyro_bias); + RATES_SUB(gyro_float, ahrs_fc.gyro_bias); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES const float alpha = 0.1; - FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha); + FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha); #else - RATES_COPY(ahrs_impl.imu_rate,gyro_float); + RATES_COPY(ahrs_fc.imu_rate, gyro_float); #endif /* add correction */ struct FloatRates omega; - RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction); + RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction); /* and zeros it */ - FLOAT_RATES_ZERO(ahrs_impl.rate_correction); + FLOAT_RATES_ZERO(ahrs_fc.rate_correction); #if AHRS_PROPAGATE_RMAT - float_rmat_integrate_fi(&ahrs_impl.ltp_to_imu_rmat, &omega, dt); - float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat); - float_quat_of_rmat(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.ltp_to_imu_rmat); + float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt); + float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat); + float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat); #endif #if AHRS_PROPAGATE_QUAT - float_quat_integrate(&ahrs_impl.ltp_to_imu_quat, &omega, dt); - float_quat_normalize(&ahrs_impl.ltp_to_imu_quat); - float_rmat_of_quat(&ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); + float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt); + float_quat_normalize(&ahrs_fc.ltp_to_imu_quat); + float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); #endif compute_body_orientation_and_rates(); // increase accel and mag propagation counters - ahrs_impl.accel_cnt++; - ahrs_impl.mag_cnt++; + ahrs_fc.accel_cnt++; + ahrs_fc.mag_cnt++; } -void ahrs_update_accel(float dt) { +void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt) +{ // check if we had at least one propagation since last update - if (ahrs_impl.accel_cnt == 0) + if (ahrs_fc.accel_cnt == 0) { return; + } /* last column of roation matrix = ltp z-axis in imu-frame */ - struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2), - RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2), - RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; + struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2), + RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2), + RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2) + }; struct FloatVect3 imu_accel_float; - ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel); + ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel); struct FloatVect3 residual; struct FloatVect3 pseudo_gravity_measurement; - if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { + if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) @@ -267,13 +220,13 @@ void ahrs_update_accel(float dt) { * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ - const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0}; + const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0}; struct FloatVect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ @@ -286,13 +239,13 @@ void ahrs_update_accel(float dt) { VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); /* FIR filtered pseudo_gravity_measurement */ - #define FIR_FILTER_SIZE 8 +#define FIR_FILTER_SIZE 8 static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.}; - VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1); VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - if (ahrs_impl.gravity_heuristic_factor) { + if (ahrs_fc.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. * e.g. for gravity_heuristic_factor 30: @@ -300,56 +253,58 @@ void ahrs_update_accel(float dt) { */ const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81; - ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; - Bound(ahrs_impl.weight, 0.15, 1.0); - } - else { - ahrs_impl.weight = 1.0; + ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; + Bound(ahrs_fc.weight, 0.15, 1.0); + } else { + ahrs_fc.weight = 1.0; } /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * weight * ahrs_impl.accel_cnt - * with ahrs_impl.accel_cnt beeing the number of propagations since last update + * Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt + * with ahrs_fc.accel_cnt beeing the number of propagations since last update */ - const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * - ahrs_impl.weight * ahrs_impl.accel_cnt / 9.81; - RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain); + const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega * + ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81; + RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain); // reset accel propagation counter - ahrs_impl.accel_cnt = 0; + ahrs_fc.accel_cnt = 0; /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2 * dt */ - const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega * - ahrs_impl.weight * ahrs_impl.weight * dt / 9.81; - RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain); + const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega * + ahrs_fc.weight * ahrs_fc.weight * dt / 9.81; + RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ } -void ahrs_update_mag(float dt) { +void ahrs_fc_update_mag(struct Int32Vect3 *mag, float dt) +{ // check if we had at least one propagation since last update - if (ahrs_impl.mag_cnt == 0) + if (ahrs_fc.mag_cnt == 0) { return; + } #if AHRS_MAG_UPDATE_ALL_AXES - ahrs_update_mag_full(dt); + ahrs_fc_update_mag_full(mag, dt); #else - ahrs_update_mag_2d(dt); + ahrs_fc_update_mag_2d(mag, dt); #endif // reset mag propagation counter - ahrs_impl.mag_cnt = 0; + ahrs_fc.mag_cnt = 0; } -void ahrs_update_mag_full(float dt) { +void ahrs_fc_update_mag_full(struct Int32Vect3 *mag, float dt) +{ struct FloatVect3 expected_imu; - float_rmat_vmult(&expected_imu, &ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.mag_h); + float_rmat_vmult(&expected_imu, &ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.mag_h); struct FloatVect3 measured_imu; - MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); + MAGS_FLOAT_OF_BFP(measured_imu, *mag); struct FloatVect3 residual_imu; VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu); @@ -358,146 +313,153 @@ void ahrs_update_mag_full(float dt) { // DISPLAY_FLOAT_VECT3("# residual", residual); /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * weight * ahrs_impl.mag_cnt - * with ahrs_impl.mag_cnt beeing the number of propagations since last update + * Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt + * with ahrs_fc.mag_cnt beeing the number of propagations since last update */ - const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * ahrs_impl.mag_cnt; - RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); + const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt; + RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2 * dt */ - const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) * dt; - RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); + const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt; + RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain); } -void ahrs_update_mag_2d(float dt) { +void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt) +{ struct FloatVect2 expected_ltp; - VECT2_COPY(expected_ltp, ahrs_impl.mag_h); + VECT2_COPY(expected_ltp, ahrs_fc.mag_h); // normalize expected ltp in 2D (x,y) float_vect2_normalize(&expected_ltp); struct FloatVect3 measured_imu; - MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); + MAGS_FLOAT_OF_BFP(measured_imu, *mag); struct FloatVect3 measured_ltp; - float_rmat_transp_vmult(&measured_ltp, &ahrs_impl.ltp_to_imu_rmat, &measured_imu); + float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_imu_rmat, &measured_imu); - struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y}; + struct FloatVect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y}; // normalize measured ltp in 2D (x,y) float_vect2_normalize(&measured_ltp_2d); - struct FloatVect3 residual_ltp = - { 0, - 0, - measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x }; + struct FloatVect3 residual_ltp = { + 0, + 0, + measured_ltp_2d.x *expected_ltp.y - measured_ltp_2d.y * expected_ltp.x + }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; - float_rmat_vmult(&residual_imu, &ahrs_impl.ltp_to_imu_rmat, &residual_ltp); + float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp); /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * weight * ahrs_impl.mag_cnt - * with ahrs_impl.mag_cnt beeing the number of propagations since last update + * Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt + * with ahrs_fc.mag_cnt beeing the number of propagations since last update */ - const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * ahrs_impl.mag_cnt; - RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); + const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt; + RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2 * dt */ - const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) * dt; - RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); + const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt; + RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain); } -void ahrs_update_mag_2d_dumb(void) { +void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag) +{ /* project mag on local tangeant plane */ struct FloatEulers ltp_to_imu_euler; - float_eulers_of_rmat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_rmat); + float_eulers_of_rmat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat); struct FloatVect3 magf; - MAGS_FLOAT_OF_BFP(magf, imu.mag); + MAGS_FLOAT_OF_BFP(magf, *mag); const float cphi = cosf(ltp_to_imu_euler.phi); const float sphi = sinf(ltp_to_imu_euler.phi); const float ctheta = cosf(ltp_to_imu_euler.theta); const float stheta = sinf(ltp_to_imu_euler.theta); - const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; - const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; + const float mn = ctheta * magf.x + sphi * stheta * magf.y + cphi * stheta * magf.z; + const float me = 0. * magf.x + cphi * magf.y - sphi * magf.z; - const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0) * me + - RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0) * mn; - const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,0), - RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,1), - RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; + const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 0) * me + + RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn; + const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0), + RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 1), + RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2) + }; const float mag_rate_update_gain = 2.5; - RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm)); + RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm)); const float mag_bias_update_gain = -2.5e-4; - RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm)); + RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm)); } -void ahrs_update_gps(void) { +void ahrs_fc_update_gps(void) +{ #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS if (gps.fix == GPS_FIX_3D) { - ahrs_impl.ltp_vel_norm = gps.speed_3d / 100.; - ahrs_impl.ltp_vel_norm_valid = TRUE; + ahrs_fc.ltp_vel_norm = gps.speed_3d / 100.; + ahrs_fc.ltp_vel_norm_valid = TRUE; } else { - ahrs_impl.ltp_vel_norm_valid = FALSE; + ahrs_fc.ltp_vel_norm_valid = FALSE; } #endif #if AHRS_USE_GPS_HEADING && USE_GPS //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg - if(gps.fix == GPS_FIX_3D && - gps.gspeed >= 500 && - gps.cacc <= RadOfDeg(10*1e7)) { + if (gps.fix == GPS_FIX_3D && + gps.gspeed >= 500 && + gps.cacc <= RadOfDeg(10 * 1e7)) { // gps.course is in rad * 1e7, we need it in rad float course = gps.course / 1e7; - if (ahrs_impl.heading_aligned) { + if (ahrs_fc.heading_aligned) { /* the assumption here is that there is no side-slip, so heading=course */ - ahrs_update_heading(course); - } - else { + ahrs_fc_update_heading(course); + } else { /* hard reset the heading if this is the first measurement */ - ahrs_realign_heading(course); + ahrs_fc_realign_heading(course); } } #endif } -void ahrs_update_heading(float heading) { +void ahrs_fc_update_heading(float heading) +{ FLOAT_ANGLE_NORMALIZE(heading); - struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f(); + struct FloatRMat *ltp_to_body_rmat = stateGetNedToBodyRMat_f(); // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y - struct FloatVect2 expected_ltp = - { RMAT_ELMT(*ltp_to_body_rmat, 0, 0), - RMAT_ELMT(*ltp_to_body_rmat, 0, 1) }; + struct FloatVect2 expected_ltp = { + RMAT_ELMT(*ltp_to_body_rmat, 0, 0), + RMAT_ELMT(*ltp_to_body_rmat, 0, 1) + }; // expected_heading cross measured_heading - struct FloatVect3 residual_ltp = - { 0, - 0, - expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)}; + struct FloatVect3 residual_ltp = { + 0, + 0, + expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading) + }; struct FloatVect3 residual_imu; - float_rmat_vmult(&residual_imu, &ahrs_impl.ltp_to_imu_rmat, &residual_ltp); + float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp); const float heading_rate_update_gain = 2.5; - RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain); + RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain); float heading_bias_update_gain; /* crude attempt to only update bias if deviation is small @@ -506,25 +468,27 @@ void ahrs_update_heading(float heading) { * the gps course information you get once you have a gps fix. * Otherwise the bias will be falsely "corrected". */ - if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) + if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) { heading_bias_update_gain = -2.5e-4; - else + } else { heading_bias_update_gain = 0.0; - RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain); + } + RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain); } -void ahrs_realign_heading(float heading) { +void ahrs_fc_realign_heading(float heading) +{ FLOAT_ANGLE_NORMALIZE(heading); /* quaternion representing only the heading rotation from ltp to body */ struct FloatQuat q_h_new; q_h_new.qx = 0.0; q_h_new.qy = 0.0; - q_h_new.qz = sinf(heading/2.0); - q_h_new.qi = cosf(heading/2.0); + q_h_new.qz = sinf(heading / 2.0); + q_h_new.qi = cosf(heading / 2.0); - struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f(); + struct FloatQuat *ltp_to_body_quat = stateGetNedToBodyQuat_f(); /* quaternion representing current heading only */ struct FloatQuat q_h; QUAT_COPY(q_h, *ltp_to_body_quat); @@ -541,31 +505,50 @@ void ahrs_realign_heading(float heading) { float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); - float_quat_comp(&ahrs_impl.ltp_to_imu_quat, &q, body_to_imu_quat); - float_rmat_of_quat(&ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); + float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat); + float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); /* set state */ stateSetNedToBodyQuat_f(&q); - ahrs_impl.heading_aligned = TRUE; + ahrs_fc.heading_aligned = TRUE; } /** * Compute body orientation and rates from imu orientation and rates */ -static inline void compute_body_orientation_and_rates(void) { +static inline void compute_body_orientation_and_rates(void) +{ /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); - float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); + float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); - float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate); stateSetBodyRates_f(&body_rate); } + +void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu) +{ + ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); +} + +void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i) +{ + orientationSetQuat_f(&ahrs_fc.body_to_imu, q_b2i); + + if (!ahrs_fc.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_fc.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_fc.body_to_imu), + sizeof(struct FloatQuat)); + memcpy(&ahrs_fc.ltp_to_imu_rmat, orientationGetRMat_f(&ahrs_fc.body_to_imu), + sizeof(struct FloatRMat)); + } +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index b1d65ee32d..38d507238a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -27,10 +27,17 @@ * Propagation can be done in rotation matrix or quaternion representation. */ -#ifndef AHRS_FLOAT_CMPL -#define AHRS_FLOAT_CMPL +#ifndef AHRS_FLOAT_CMPL_H +#define AHRS_FLOAT_CMPL_H #include "std.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" + +enum AhrsFCStatus { + AHRS_FC_UNINIT, + AHRS_FC_RUNNING +}; struct AhrsFloatCmpl { struct FloatRates gyro_bias; @@ -60,23 +67,37 @@ struct AhrsFloatCmpl { /* internal counters for the gains */ uint16_t accel_cnt; ///< number of propagations since last accel update uint16_t mag_cnt; ///< number of propagations since last mag update + + struct OrientationReps body_to_imu; + + enum AhrsFCStatus status; + bool_t is_aligned; }; -extern struct AhrsFloatCmpl ahrs_impl; +extern struct AhrsFloatCmpl ahrs_fc; +extern void ahrs_fc_init(void); +extern void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu); +extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i); +extern bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag); +extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt); +extern void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt); +extern void ahrs_fc_update_mag(struct Int32Vect3 *mag, float dt); +extern void ahrs_fc_update_gps(void); /** Update yaw based on a heading measurement. * e.g. from GPS course * @param heading Heading in body frame, radians (CW/north) */ -void ahrs_update_heading(float heading); +void ahrs_fc_update_heading(float heading); /** Hard reset yaw to a heading. * Doesn't affect the bias. - * Sets ahrs_impl.heading_aligned to TRUE. + * Sets ahrs_fc.heading_aligned to TRUE. * @param heading Heading in body frame, radians (CW/north) */ -void ahrs_realign_heading(float heading); +void ahrs_fc_realign_heading(float heading); -#endif /* AHRS_FLOAT_CMPL_RMAT */ +#endif /* AHRS_FLOAT_CMPL_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c new file mode 100644 index 0000000000..ebc4c45442 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c @@ -0,0 +1,173 @@ +/* + * 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_cmpl_wrapper.c + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#include "subsystems/ahrs/ahrs_float_cmpl_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "state.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_att(struct transport_tx *trans, struct link_device *dev) +{ + struct FloatEulers ltp_to_imu_euler; + float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat); + struct Int32Eulers euler_i; + EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); + struct Int32Eulers *eulers_body = stateGetNedToBodyEulers_i(); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, + &euler_i.phi, + &euler_i.theta, + &euler_i.psi, + &(eulers_body->phi), + &(eulers_body->theta), + &(eulers_body->psi)); +} + +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, + &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z); +} +#endif + + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_FC_IMU_ID +#define AHRS_FC_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, + uint32_t __attribute__((unused)) stamp, + struct Int32Rates *gyro) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0 && ahrs_fc.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_fc_propagate(gyro, dt); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_FC propagation.") + PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + if (ahrs_fc.status == AHRS_FC_RUNNING) { + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + ahrs_fc_propagate(gyro, dt); + } +#endif +} + +static void accel_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + struct Int32Vect3 *accel) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_fc.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.") + PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + if (ahrs_fc.is_aligned) { + const float dt = 1. / (AHRS_CORRECT_FREQUENCY); + ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt); + } +#endif +} + +static void mag_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + struct Int32Vect3 *mag) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_fc.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_fc_update_mag(mag, dt); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.") + PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + if (ahrs_fc.is_aligned) { + const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); + ahrs_fc_update_mag(mag, dt); + } +#endif +} + +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 (!ahrs_fc.is_aligned) { + ahrs_fc_align(lp_gyro, lp_accel, lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + struct FloatQuat *q_b2i_f) +{ + ahrs_fc_set_body_to_imu_quat(q_b2i_f); +} + +void ahrs_fc_register(void) +{ + ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_FC_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); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); + register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h new file mode 100644 index 0000000000..b6ae7ed973 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_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_cmpl_wrapper.h + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#ifndef AHRS_FLOAT_CMPL_WRAPPER_H +#define AHRS_FLOAT_CMPL_WRAPPER_H + +#include "subsystems/ahrs/ahrs_float_cmpl.h" + +#define DefaultAhrsImpl ahrs_fc + +extern void ahrs_fc_register(void); + +#endif /* AHRS_FLOAT_CMPL_WRAPPER_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 56f1d1c4d1..321463f186 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -27,12 +27,9 @@ #include "std.h" -#include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_float_dcm.h" #include "subsystems/ahrs/ahrs_float_utils.h" -#include "subsystems/ahrs/ahrs_aligner.h" -#include "subsystems/imu.h" -#include "firmwares/fixedwing/autopilot.h" // launch detection +#include "firmwares/fixedwing/autopilot.h" // launch detection #include "subsystems/ahrs/ahrs_float_dcm_algebra.h" #include "math/pprz_algebra_float.h" @@ -55,23 +52,24 @@ #endif -struct AhrsFloatDCM ahrs_impl; +struct AhrsFloatDCM ahrs_dcm; // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. // Positive pitch : nose up // Positive roll : right wing down // Positive yaw : clockwise -struct FloatVect3 accel_float = {0,0,0}; +struct FloatVect3 accel_float = {0, 0, 0}; +struct FloatVect3 mag_float = {0, 0, 0}; -float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data -float Omega_P[3]= {0,0,0}; //Omega Proportional correction -float Omega_I[3]= {0,0,0}; //Omega Integrator -float Omega[3]= {0,0,0}; +float Omega_Vector[3] = {0, 0, 0}; //Corrected Gyro_Vector data +float Omega_P[3] = {0, 0, 0}; //Omega Proportional correction +float Omega_I[3] = {0, 0, 0}; //Omega Integrator +float Omega[3] = {0, 0, 0}; -float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; -float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here -float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; +float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; +float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; //Gyros here +float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; #if USE_MAGNETOMETER float MAG_Heading_X = 1; @@ -95,41 +93,42 @@ float imu_health = 0.; static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) { - for (int i=0; i<3; i++) { - for (int j=0; j<3; j++) { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i); } } } +void ahrs_dcm_init(void) +{ + ahrs_dcm.status = AHRS_DCM_UNINIT; + ahrs_dcm.is_aligned = FALSE; -void ahrs_init(void) { - ahrs.status = AHRS_UNINIT; + /* init ltp_to_imu euler with zero */ + FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler); - /* Set ltp_to_imu so that body is zero */ - memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu), - sizeof(struct FloatEulers)); - - FLOAT_RATES_ZERO(ahrs_impl.imu_rate); + FLOAT_RATES_ZERO(ahrs_dcm.imu_rate); /* set inital filter dcm */ - set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu)); + set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); - ahrs_impl.gps_speed = 0; - ahrs_impl.gps_acceleration = 0; - ahrs_impl.gps_course = 0; - ahrs_impl.gps_course_valid = FALSE; - ahrs_impl.gps_age = 100; + 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; } -void ahrs_align(void) +bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) { /* Compute an initial orientation using euler angles */ - ahrs_float_get_euler_from_accel_mag(&ahrs_impl.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ struct FloatRMat ltp_to_imu_rmat; - float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); + float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler); /* set filter dcm */ set_dcm_matrix_from_rmat(<p_to_imu_rmat); @@ -139,35 +138,37 @@ void ahrs_align(void) /* use averaged gyro as initial value for bias */ struct Int32Rates bias0; - RATES_COPY(bias0, ahrs_aligner.lp_gyro); - RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); + RATES_COPY(bias0, *lp_gyro); + RATES_FLOAT_OF_BFP(ahrs_dcm.gyro_bias, bias0); - ahrs.status = AHRS_RUNNING; + ahrs_dcm.status = AHRS_DCM_RUNNING; + + return TRUE; } -void ahrs_propagate(float dt) +void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt) { /* convert imu data to floating point */ struct FloatRates gyro_float; - RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); + RATES_FLOAT_OF_BFP(gyro_float, *gyro); /* unbias rate measurement */ - RATES_DIFF(ahrs_impl.imu_rate, gyro_float, ahrs_impl.gyro_bias); + RATES_DIFF(ahrs_dcm.imu_rate, gyro_float, ahrs_dcm.gyro_bias); /* Uncouple Motions */ #ifdef IMU_GYRO_P_Q - float dp=0,dq=0,dr=0; - dp += ahrs_impl.imu_rate.q * IMU_GYRO_P_Q; - dp += ahrs_impl.imu_rate.r * IMU_GYRO_P_R; - dq += ahrs_impl.imu_rate.p * IMU_GYRO_Q_P; - dq += ahrs_impl.imu_rate.r * IMU_GYRO_Q_R; - dr += ahrs_impl.imu_rate.p * IMU_GYRO_R_P; - dr += ahrs_impl.imu_rate.q * IMU_GYRO_R_Q; + float dp = 0, dq = 0, dr = 0; + dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q; + dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R; + dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P; + dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R; + dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P; + dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q; - ahrs_impl.imu_rate.p += dp; - ahrs_impl.imu_rate.q += dq; - ahrs_impl.imu_rate.r += dr; + ahrs_dcm.imu_rate.p += dp; + ahrs_dcm.imu_rate.q += dq; + ahrs_dcm.imu_rate.r += dr; #endif Matrix_update(dt); @@ -177,34 +178,34 @@ void ahrs_propagate(float dt) compute_ahrs_representations(); } -void ahrs_update_gps(void) +void ahrs_dcm_update_gps(void) { static float last_gps_speed_3d = 0; #if USE_GPS if (gps.fix == GPS_FIX_3D) { - ahrs_impl.gps_age = 0; - ahrs_impl.gps_speed = gps.speed_3d/100.; + ahrs_dcm.gps_age = 0; + ahrs_dcm.gps_speed = gps.speed_3d / 100.; - if(gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s - ahrs_impl.gps_course = ((float)gps.course)/1.e7; - ahrs_impl.gps_course_valid = TRUE; + if (gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s + ahrs_dcm.gps_course = ((float)gps.course) / 1.e7; + ahrs_dcm.gps_course_valid = TRUE; } else { - ahrs_impl.gps_course_valid = FALSE; + ahrs_dcm.gps_course_valid = FALSE; } } else { - ahrs_impl.gps_age = 100; + ahrs_dcm.gps_age = 100; } #endif - ahrs_impl.gps_acceleration += ( ((ahrs_impl.gps_speed - last_gps_speed_3d)*4.0f) - ahrs_impl.gps_acceleration) / 5.0f; - last_gps_speed_3d = ahrs_impl.gps_speed; + ahrs_dcm.gps_acceleration += (((ahrs_dcm.gps_speed - last_gps_speed_3d) * 4.0f) - ahrs_dcm.gps_acceleration) / 5.0f; + last_gps_speed_3d = ahrs_dcm.gps_speed; } -void ahrs_update_accel(float dt __attribute__((unused))) +void ahrs_dcm_update_accel(struct Int32Vect3 *accel) { - ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); + ACCELS_FLOAT_OF_BFP(accel_float, *accel); // DCM filter uses g-force as positive // accelerometer measures [0 0 -g] in a static case @@ -213,65 +214,65 @@ void ahrs_update_accel(float dt __attribute__((unused))) accel_float.z = -accel_float.z; - ahrs_impl.gps_age ++; - if (ahrs_impl.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration + ahrs_dcm.gps_age ++; + if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration #if USE_AHRS_GPS_ACCELERATIONS -PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.") - accel_float.x += ahrs_impl.gps_acceleration; // Longitudinal acceleration + PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.") + accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration #endif - accel_float.y += ahrs_impl.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ - accel_float.z -= ahrs_impl.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY - } - else - { - ahrs_impl.gps_speed = 0; - ahrs_impl.gps_acceleration = 0; - ahrs_impl.gps_age = 100; + accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ + accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY + } else { + ahrs_dcm.gps_speed = 0; + ahrs_dcm.gps_acceleration = 0; + ahrs_dcm.gps_age = 100; } Drift_correction(); } -void ahrs_update_mag(float dt __attribute__((unused))) +void ahrs_dcm_update_mag(struct Int32Vect3 *mag) { #if USE_MAGNETOMETER #warning MAGNETOMETER FEEDBACK NOT TESTED YET + MAG_FLOAT_OF_BFP(mag_float, *mag); + float cos_roll; float sin_roll; float cos_pitch; float sin_pitch; - cos_roll = cosf(ahrs_impl.ltp_to_imu_euler.phi); - sin_roll = sinf(ahrs_impl.ltp_to_imu_euler.phi); - cos_pitch = cosf(ahrs_impl.ltp_to_imu_euler.theta); - sin_pitch = sinf(ahrs_impl.ltp_to_imu_euler.theta); + cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi); + sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi); + cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta); + sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta); // Pitch&Roll Compensation: - MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch; - MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll; + MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch; + MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll; -/* - * - // Magnetic Heading - Heading = atan2(-Head_Y,Head_X); + /* + * + // Magnetic Heading + Heading = atan2(-Head_Y,Head_X); - // Declination correction (if supplied) - if( declination != 0.0 ) - { - Heading = Heading + declination; - if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) - Heading -= (2.0 * M_PI); - else if (Heading < -M_PI) - Heading += (2.0 * M_PI); - } + // Declination correction (if supplied) + if( declination != 0.0 ) + { + Heading = Heading + declination; + if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) + Heading -= (2.0 * M_PI); + else if (Heading < -M_PI) + Heading += (2.0 * M_PI); + } - // Optimization for external DCM use. Calculate normalized components - Heading_X = cos(Heading); - Heading_Y = sin(Heading); -*/ + // Optimization for external DCM use. Calculate normalized components + Heading_X = cos(Heading); + Heading_Y = sin(Heading); + */ struct FloatVect3 ltp_mag; @@ -280,23 +281,27 @@ void ahrs_update_mag(float dt __attribute__((unused))) #if FLOAT_DCM_SEND_DEBUG // Downlink - RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, <p_mag.x, <p_mag.y, <p_mag.z)); + RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, <p_mag.x, <p_mag.y, <p_mag.z)); #endif // Magnetic Heading - // MAG_Heading = atan2(imu.mag.y, -imu.mag.x); + // MAG_Heading = atan2(mag->y, -mag->x); + +#else // !USE_MAGNETOMETER + // get rid of unused param warning... + mag = mag; #endif } void Normalize(void) { - float error=0; + float error = 0; float temporary[3][3]; - float renorm=0; - uint8_t problem=FALSE; + float renorm = 0; + uint8_t problem = FALSE; // Find the non-orthogonality of X wrt Y - error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 + error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19 // Add half the XY error to X, and half to Y Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 @@ -305,17 +310,17 @@ void Normalize(void) Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19 // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z) - Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 + Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20 // Normalize lenght of X - renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); + renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]); // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT // b) if the norm is further from 1, use a real sqrt // c) norm is huge: disaster! reset! mayday! if (renorm < 1.5625f && renorm > 0.64f) { - renorm= .5 * (3-renorm); //eq.21 + renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { - renorm= 1. / sqrt(renorm); + renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif @@ -328,11 +333,11 @@ void Normalize(void) Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); // Normalize lenght of Y - renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); + renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]); if (renorm < 1.5625f && renorm > 0.64f) { - renorm= .5 * (3-renorm); //eq.21 + renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { - renorm= 1. / sqrt(renorm); + renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif @@ -345,11 +350,11 @@ void Normalize(void) Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); // Normalize lenght of Z - renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); + renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]); if (renorm < 1.5625f && renorm > 0.64f) { - renorm= .5 * (3-renorm); //eq.21 + renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { - renorm= 1. / sqrt(renorm); + renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif @@ -363,13 +368,13 @@ 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(&imu.body_to_imu)); + set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); problem = FALSE; } } -void Drift_correction(void) +void Drift_correction() { //Compensation the Roll, Pitch and Yaw drift. static float Scaled_Omega_P[3]; @@ -386,27 +391,27 @@ void Drift_correction(void) //*****Roll and Pitch*************** // Calculate the magnitude of the accelerometer vector - Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z); + Accel_magnitude = sqrt(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z); Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. // Dynamic weighting of accelerometer info (reliability filter) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); // + Accel_weight = Chop(1 - 2 * fabs(1 - Accel_magnitude), 0, 1); // - #if PERFORMANCE_REPORTING == 1 +#if PERFORMANCE_REPORTING == 1 { - - float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction + //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction + float tempfloat = ((Accel_weight - 0.5) * 256.0f); imu_health += tempfloat; - Bound(imu_health,129,65405); + Bound(imu_health, 129, 65405); } - #endif +#endif - Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference - Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); + Vector_Cross_Product(&errorRollPitch[0], &accel_float.x, &DCM_Matrix[2][0]); //adjust the ground of reference + Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight); - Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); + Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight); + Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //*****YAW*************** @@ -415,52 +420,55 @@ void Drift_correction(void) // float mag_heading_x = cos(MAG_Heading); // float mag_heading_y = sin(MAG_Heading); // 2D dot product - errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error - Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + //Calculating YAW error + errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X); + //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); - Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); - Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. + Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW); + Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional. - Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I + Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW); + Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I #else // Use GPS Ground course to correct yaw gyro drift - if (ahrs_impl.gps_course_valid) { - float course = ahrs_impl.gps_course - M_PI; //This is the runaway direction of you "plane" in rad + if (ahrs_dcm.gps_course_valid) { + float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad float COGX = cosf(course); //Course overground X axis float COGY = sinf(course); //Course overground Y axis - errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error - Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error + //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); - Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); - Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. + Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW); + Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional. - Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I + Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW); + Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I } #if USE_MAGNETOMETER_ONGROUND == 1 -PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight") - else if (launch == FALSE) - { - float COGX = imu.mag.x; // Non-Tilt-Compensated (for filter stability reasons) - float COGY = imu.mag.y; // Non-Tilt-Compensated (for filter stability reasons) + PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight") + else if (launch == FALSE) { + float COGX = mag_float.x; // Non-Tilt-Compensated (for filter stability reasons) + float COGY = mag_float.y; // Non-Tilt-Compensated (for filter stability reasons) - errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error - Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error + //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); // P only - Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW / 10.0); - Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.fi + Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0); + Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi } #endif // USE_MAGNETOMETER_ONGROUND #endif // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros - Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I)); + Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I, Omega_I)); if (Integrator_magnitude > RadOfDeg(300)) { - Vector_Scale(Omega_I,Omega_I,0.5f*RadOfDeg(300)/Integrator_magnitude); + Vector_Scale(Omega_I, Omega_I, 0.5f * RadOfDeg(300) / Integrator_magnitude); } @@ -469,38 +477,36 @@ PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS duri void Matrix_update(float dt) { - Vector_Add(&Omega[0], &ahrs_impl.imu_rate.p, &Omega_I[0]); //adding proportional term + Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term - #if OUTPUTMODE==1 // With corrected data (drift correction) - Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-dt*Omega_Vector[2];//-z - Update_Matrix[0][2]=dt*Omega_Vector[1];//y - Update_Matrix[1][0]=dt*Omega_Vector[2];//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-dt*Omega_Vector[0];//-x - Update_Matrix[2][0]=-dt*Omega_Vector[1];//-y - Update_Matrix[2][1]=dt*Omega_Vector[0];//x - Update_Matrix[2][2]=0; - #else // Uncorrected data (no drift correction) - Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-dt*ahrs_impl.imu_rate.r;//-z - Update_Matrix[0][2]=dt*ahrs_impl.imu_rate.q;//y - Update_Matrix[1][0]=dt*ahrs_impl.imu_rate.r;//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-dt*ahrs_impl.imu_rate.p; - Update_Matrix[2][0]=-dt*ahrs_impl.imu_rate.q; - Update_Matrix[2][1]=dt*ahrs_impl.imu_rate.p; - Update_Matrix[2][2]=0; - #endif +#if OUTPUTMODE==1 // With corrected data (drift correction) + Update_Matrix[0][0] = 0; + Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z + Update_Matrix[0][2] = dt * Omega_Vector[1]; //y + Update_Matrix[1][0] = dt * Omega_Vector[2]; //z + Update_Matrix[1][1] = 0; + Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x + Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y + Update_Matrix[2][1] = dt * Omega_Vector[0]; //x + Update_Matrix[2][2] = 0; +#else // Uncorrected data (no drift correction) + Update_Matrix[0][0] = 0; + Update_Matrix[0][1] = -dt * ahrs_dcm.imu_rate.r; //-z + Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y + Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z + Update_Matrix[1][1] = 0; + Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p; + Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q; + Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p; + Update_Matrix[2][2] = 0; +#endif - Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c + Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); //a*b=c - for(int x=0; x<3; x++) //Matrix Addition (update) - { - for(int y=0; y<3; y++) - { - DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; + for (int x = 0; x < 3; x++) { //Matrix Addition (update) + for (int y = 0; y < 3; y++) { + DCM_Matrix[x][y] += Temporary_Matrix[x][y]; } } } @@ -508,32 +514,34 @@ void Matrix_update(float dt) /* * Compute body orientation and rates from imu orientation and rates */ -static inline void set_body_orientation_and_rates(void) { +static inline void set_body_orientation_and_rates(void) +{ - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.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_impl.imu_rate); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate); stateSetBodyRates_f(&body_rate); struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat; - float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); + float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler); float_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); stateSetNedToBodyRMat_f(<p_to_body_rmat); } -static inline void compute_ahrs_representations(void) { +static inline void compute_ahrs_representations(void) +{ #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) - ahrs_impl.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z) - ahrs_impl.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x) - ahrs_impl.ltp_to_imu_euler.psi = 0; + ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z) + ahrs_dcm.ltp_to_imu_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x) + ahrs_dcm.ltp_to_imu_euler.psi = 0; #else - ahrs_impl.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - ahrs_impl.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); - ahrs_impl.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); - ahrs_impl.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ + ahrs_dcm.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]); + ahrs_dcm.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); + ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]); + ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ #endif set_body_orientation_and_rates(); @@ -556,3 +564,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 d761f08dc2..00398af3bc 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -26,6 +26,12 @@ #include #include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" + +enum AhrsDCMStatus { + AHRS_DCM_UNINIT, + AHRS_DCM_RUNNING +}; struct AhrsFloatDCM { struct FloatRates gyro_bias; @@ -39,9 +45,13 @@ struct AhrsFloatDCM { float gps_course; bool_t gps_course_valid; uint8_t gps_age; -}; -extern struct AhrsFloatDCM ahrs_impl; + struct OrientationReps body_to_imu; + + enum AhrsDCMStatus status; + bool_t is_aligned; +}; +extern struct AhrsFloatDCM ahrs_dcm; // DCM Parameters @@ -69,4 +79,14 @@ extern int renorm_blowup_count; extern float imu_health; #endif +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); +extern void ahrs_dcm_update_accel(struct Int32Vect3 *accel); +extern void ahrs_dcm_update_mag(struct Int32Vect3 *mag); +extern void ahrs_dcm_update_gps(void); + #endif // AHRS_FLOAT_DCM_H diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h index 9027a78175..739b1a1eec 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h @@ -21,34 +21,34 @@ #define AHRS_FLOAT_DCM_ALGEBRA_H -static inline float Vector_Dot_Product(float vector1[3],float vector2[3]) +static inline float Vector_Dot_Product(float vector1[3], float vector2[3]) { - return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2]; + return vector1[0] * vector2[0] + vector1[1] * vector2[1] + vector1[2] * vector2[2]; } -static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) +static inline void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]) { - vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); - vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); - vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); + vectorOut[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]); + vectorOut[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]); + vectorOut[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]); } -static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) +static inline void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2) { - vectorOut[0]=vectorIn[0]*scale2; - vectorOut[1]=vectorIn[1]*scale2; - vectorOut[2]=vectorIn[2]*scale2; + vectorOut[0] = vectorIn[0] * scale2; + vectorOut[1] = vectorIn[1] * scale2; + vectorOut[2] = vectorIn[2] * scale2; } -static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) +static inline void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]) { - vectorOut[0]=vectorIn1[0]+vectorIn2[0]; - vectorOut[1]=vectorIn1[1]+vectorIn2[1]; - vectorOut[2]=vectorIn1[2]+vectorIn2[2]; + vectorOut[0] = vectorIn1[0] + vectorIn2[0]; + vectorOut[1] = vectorIn1[1] + vectorIn2[1]; + vectorOut[2] = vectorIn1[2] + vectorIn2[2]; } /* - #define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \ + #define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \ _m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \ _m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \ _m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \ @@ -61,20 +61,17 @@ static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vecto } */ -static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) +static inline void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3]) { float op[3]; - for(int x=0; x<3; x++) - { - for(int y=0; y<3; y++) - { - for(int w=0; w<3; w++) - { - op[w]=a[x][w]*b[w][y]; - } - mat[x][y]=op[0]+op[1]+op[2]; - } + for (int x = 0; x < 3; x++) { + for (int y = 0; y < 3; y++) { + for (int w = 0; w < 3; w++) { + op[w] = a[x][w] * b[w][y]; + } + mat[x][y] = op[0] + op[1] + op[2]; } + } } #endif // AHRS_FLOAT_DCM_ALGEBRA_H 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..6c99c90aa0 --- /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, + uint32_t __attribute__((unused)) stamp, + 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(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(gyro, dt); + } +#endif +} + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *accel) +{ + if (ahrs_dcm.is_aligned) { + ahrs_dcm_update_accel(accel); + } +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *mag) +{ + if (ahrs_dcm.is_aligned) { + ahrs_dcm_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 (!ahrs_dcm.is_aligned) { + ahrs_dcm_align(lp_gyro, lp_accel, lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + struct FloatQuat *q_b2i_f) +{ + ahrs_dcm_set_body_to_imu_quat(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 */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index f5dbcf2177..7d7f0fbcd3 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -29,16 +29,12 @@ */ #include "subsystems/ahrs/ahrs_float_mlkf.h" -#include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ahrs/ahrs_float_utils.h" -#include /* for FLT_MIN */ #include /* for memcpy */ -#include /* for M_PI */ #include "state.h" -#include "subsystems/imu.h" #include "math/pprz_algebra_float.h" #include "math/pprz_algebra_int.h" #include "math/pprz_simple_matrix.h" @@ -52,119 +48,153 @@ #define AHRS_MAG_NOISE_Z 0.2 #endif -static inline void propagate_ref(float dt); +static inline void propagate_ref(struct Int32Rates *gyro, float dt); static inline void propagate_state(float dt); -static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise); +static inline void update_state(const struct FloatVect3 *i_expected, + struct FloatVect3 *b_measured, + struct FloatVect3 *noise); +static inline void update_state_heading(const struct FloatVect3 *i_expected, + struct FloatVect3 *b_measured, + struct FloatVect3 *noise); static inline void reset_state(void); static inline void set_body_state_from_quat(void); -struct AhrsMlkf ahrs_impl; +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_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z); -} -#endif +void ahrs_mlkf_init(void) +{ -void ahrs_init(void) { + ahrs_mlkf.is_aligned = FALSE; - ahrs.status = AHRS_UNINIT; + /* init ltp_to_imu quaternion as zero/identity rotation */ + float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat); + FLOAT_RATES_ZERO(ahrs_mlkf.imu_rate); - /* Set ltp_to_imu so that body is zero */ - memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu), - sizeof(struct FloatQuat)); - - FLOAT_RATES_ZERO(ahrs_impl.imu_rate); - - VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); + VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); /* * Initialises our state */ - FLOAT_RATES_ZERO(ahrs_impl.gyro_bias); + FLOAT_RATES_ZERO(ahrs_mlkf.gyro_bias); const float P0_a = 1.; const float P0_b = 1e-4; float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. }, - { 0., P0_a, 0., 0., 0., 0. }, - { 0., 0., P0_a, 0., 0., 0. }, - { 0., 0., 0., P0_b, 0., 0. }, - { 0., 0., 0., 0., P0_b, 0. }, - { 0., 0., 0., 0., 0., P0_b}}; - memcpy(ahrs_impl.P, P0, sizeof(P0)); + { 0., P0_a, 0., 0., 0., 0. }, + { 0., 0., P0_a, 0., 0., 0. }, + { 0., 0., 0., P0_b, 0., 0. }, + { 0., 0., 0., 0., P0_b, 0. }, + { 0., 0., 0., 0., 0., P0_b} + }; + memcpy(ahrs_mlkf.P, P0, sizeof(P0)); - VECT3_ASSIGN(ahrs_impl.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z); - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); -#endif + VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z); } -void ahrs_align(void) { +void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu) +{ + ahrs_mlkf_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); +} + +void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i) +{ + orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i); + + if (!ahrs_mlkf.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_mlkf.body_to_imu), + sizeof(struct FloatQuat)); + } +} + + +bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag); /* set initial body orientation */ set_body_state_from_quat(); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; - RATES_COPY(bias0, ahrs_aligner.lp_gyro); - RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); + RATES_COPY(bias0, *lp_gyro); + RATES_FLOAT_OF_BFP(ahrs_mlkf.gyro_bias, bias0); - ahrs.status = AHRS_RUNNING; + ahrs_mlkf.is_aligned = TRUE; + + return TRUE; } -void ahrs_propagate(float dt) { - propagate_ref(dt); +void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt) +{ + propagate_ref(gyro, dt); propagate_state(dt); set_body_state_from_quat(); } -void ahrs_update_accel(float dt __attribute__((unused))) { +void ahrs_mlkf_update_accel(struct Int32Vect3 *accel) +{ struct FloatVect3 imu_g; - ACCELS_FLOAT_OF_BFP(imu_g, imu.accel); + ACCELS_FLOAT_OF_BFP(imu_g, *accel); const float alpha = 0.92; - ahrs_impl.lp_accel = alpha * ahrs_impl.lp_accel + - (1. - alpha) *(float_vect3_norm(&imu_g) - 9.81); + ahrs_mlkf.lp_accel = alpha * ahrs_mlkf.lp_accel + + (1. - alpha) * (float_vect3_norm(&imu_g) - 9.81); const struct FloatVect3 earth_g = {0., 0., -9.81 }; - const float dn = 250*fabs( ahrs_impl.lp_accel ); - struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn}; + const float dn = 250 * fabs(ahrs_mlkf.lp_accel); + struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn}; update_state(&earth_g, &imu_g, &g_noise); reset_state(); } +void ahrs_mlkf_update_mag(struct Int32Vect3 *mag) +{ +#if AHRS_MAG_UPDATE_ALL_AXES + ahrs_mlkf_update_mag_full(mag); +#else + ahrs_mlkf_update_mag_2d(mag); +#endif +} -void ahrs_update_mag(float dt __attribute__((unused))) { +void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag) +{ struct FloatVect3 imu_h; - MAGS_FLOAT_OF_BFP(imu_h, imu.mag); - update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise); + MAGS_FLOAT_OF_BFP(imu_h, *mag); + update_state_heading(&ahrs_mlkf.mag_h, &imu_h, &ahrs_mlkf.mag_noise); + reset_state(); +} + +void ahrs_mlkf_update_mag_full(struct Int32Vect3 *mag) +{ + struct FloatVect3 imu_h; + MAGS_FLOAT_OF_BFP(imu_h, *mag); + update_state(&ahrs_mlkf.mag_h, &imu_h, &ahrs_mlkf.mag_noise); reset_state(); } -static inline void propagate_ref(float dt) { +static inline void propagate_ref(struct Int32Rates *gyro, float dt) +{ /* converts gyro to floating point */ struct FloatRates gyro_float; - RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); + RATES_FLOAT_OF_BFP(gyro_float, *gyro); /* unbias measurement */ - RATES_SUB(gyro_float, ahrs_impl.gyro_bias); + RATES_SUB(gyro_float, ahrs_mlkf.gyro_bias); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES /* lowpass angular rates */ const float alpha = 0.1; - FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, - (1.-alpha), gyro_float, alpha); + FLOAT_RATES_LIN_CMB(ahrs_mlkf.imu_rate, ahrs_mlkf.imu_rate, + (1. - alpha), gyro_float, alpha); #else - RATES_COPY(ahrs_impl.imu_rate, gyro_float); + RATES_COPY(ahrs_mlkf.imu_rate, gyro_float); #endif /* propagate reference quaternion */ - float_quat_integrate(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.imu_rate, dt); + float_quat_integrate(&ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.imu_rate, dt); } @@ -172,48 +202,57 @@ static inline void propagate_ref(float dt) { * Progagate filter's covariance * We don't propagate state as we assume to have reseted */ -static inline void propagate_state(float dt) { +static inline void propagate_state(float dt) +{ /* predict covariance */ - const float dp = ahrs_impl.imu_rate.p*dt; - const float dq = ahrs_impl.imu_rate.q*dt; - const float dr = ahrs_impl.imu_rate.r*dt; + const float dp = ahrs_mlkf.imu_rate.p * dt; + const float dq = ahrs_mlkf.imu_rate.q * dt; + const float dr = ahrs_mlkf.imu_rate.r * dt; float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. }, - { -dr, 1., dp, 0., -dt, 0. }, - { dq, -dp, 1., 0., 0., -dt }, - { 0., 0., 0., 1., 0., 0. }, - { 0., 0., 0., 0., 1., 0. }, - { 0., 0., 0., 0., 0., 1. }}; + { -dr, 1., dp, 0., -dt, 0. }, + { dq, -dp, 1., 0., 0., -dt }, + { 0., 0., 0., 1., 0., 0. }, + { 0., 0., 0., 0., 1., 0. }, + { 0., 0., 0., 0., 0., 1. } + }; // P = FPF' + GQG float tmp[6][6]; - MAT_MUL(6,6,6, tmp, F, ahrs_impl.P); - MAT_MUL_T(6,6,6, ahrs_impl.P, tmp, F); + MAT_MUL(6, 6, 6, tmp, F, ahrs_mlkf.P); + MAT_MUL_T(6, 6, 6, ahrs_mlkf.P, tmp, F); const float dt2 = dt * dt; - const float GQG[6] = {dt2*10e-3, dt2*10e-3, dt2*10e-3, dt2*9e-6, dt2*9e-6, dt2*9e-6 }; - for (int i=0;i<6;i++) - ahrs_impl.P[i][i] += GQG[i]; + const float GQG[6] = {dt2 * 10e-3, dt2 * 10e-3, dt2 * 10e-3, dt2 * 9e-6, dt2 * 9e-6, dt2 * 9e-6 }; + for (int i = 0; i < 6; i++) { + ahrs_mlkf.P[i][i] += GQG[i]; + } } /** - * Incorporate one 3D vector measurement + * Incorporate one 3D vector measurement. + * @param i_expected expected 3d vector in inertial frame + * @param b_measured measured 3d vector in body/imu frame + * @param noise measurement noise vector (diagonal of covariance) */ -static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) { +static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, + struct FloatVect3 *noise) +{ /* converted expected measurement from inertial to body frame */ struct FloatVect3 b_expected; - float_quat_vmult(&b_expected, &ahrs_impl.ltp_to_imu_quat, (struct FloatVect3*)i_expected); + float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected); // S = HPH' + JRJ float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.}, { b_expected.z, 0., -b_expected.x, 0., 0., 0.}, - {-b_expected.y, b_expected.x, 0., 0., 0., 0.}}; + { -b_expected.y, b_expected.x, 0., 0., 0., 0.} + }; float tmp[3][6]; - MAT_MUL(3,6,6, tmp, H, ahrs_impl.P); + MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P); float S[3][3]; - MAT_MUL_T(3,6,3, S, tmp, H); + MAT_MUL_T(3, 6, 3, S, tmp, H); /* add the measurement noise */ S[0][0] += noise->x; @@ -225,68 +264,142 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa // K = PH'invS float tmp2[6][3]; - MAT_MUL_T(6,6,3, tmp2, ahrs_impl.P, H); + MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H); float K[6][3]; - MAT_MUL(6,3,3, K, tmp2, invS); + MAT_MUL(6, 3, 3, K, tmp2, invS); // P = (I-KH)P float tmp3[6][6]; - MAT_MUL(6,3,6, tmp3, K, H); + MAT_MUL(6, 3, 6, tmp3, K, H); float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. }, - { 0., 1., 0., 0., 0., 0. }, - { 0., 0., 1., 0., 0., 0. }, - { 0., 0., 0., 1., 0., 0. }, - { 0., 0., 0., 0., 1., 0. }, - { 0., 0., 0., 0., 0., 1. }}; + { 0., 1., 0., 0., 0., 0. }, + { 0., 0., 1., 0., 0., 0. }, + { 0., 0., 0., 1., 0., 0. }, + { 0., 0., 0., 0., 1., 0. }, + { 0., 0., 0., 0., 0., 1. } + }; float tmp4[6][6]; - MAT_SUB(6,6, tmp4, I6, tmp3); + MAT_SUB(6, 6, tmp4, I6, tmp3); float tmp5[6][6]; - MAT_MUL(6,6,6, tmp5, tmp4, ahrs_impl.P); - memcpy(ahrs_impl.P, tmp5, sizeof(ahrs_impl.P)); + MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P); + memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P)); // X = X + Ke struct FloatVect3 e; VECT3_DIFF(e, *b_measured, b_expected); - ahrs_impl.gibbs_cor.qx += K[0][0]*e.x + K[0][1]*e.y + K[0][2]*e.z; - ahrs_impl.gibbs_cor.qy += K[1][0]*e.x + K[1][1]*e.y + K[1][2]*e.z; - ahrs_impl.gibbs_cor.qz += K[2][0]*e.x + K[2][1]*e.y + K[2][2]*e.z; - ahrs_impl.gyro_bias.p += K[3][0]*e.x + K[3][1]*e.y + K[3][2]*e.z; - ahrs_impl.gyro_bias.q += K[4][0]*e.x + K[4][1]*e.y + K[4][2]*e.z; - ahrs_impl.gyro_bias.r += K[5][0]*e.x + K[5][1]*e.y + K[5][2]*e.z; + ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z; + ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z; + ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z; + ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z; + ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z; + ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z; } +/** + * Incorporate one 3D vector measurement, only correcting heading. + * @param i_expected expected 3d vector in inertial frame + * @param b_measured measured 3d vector in body/imu frame + * @param noise measurement noise vector (diagonal of covariance) + * TODO: optimize + */ +static inline void update_state_heading(const struct FloatVect3 *i_expected, + struct FloatVect3 *b_measured, + struct FloatVect3 *noise) +{ + + /* converted expected measurement from inertial to body frame */ + struct FloatVect3 b_expected; + float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected); + + /* set roll/pitch errors to zero to only correct heading */ + struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f}; + struct FloatVect3 b_yaw; + float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_imu_quat, &i_h_2d); + // S = HPH' + JRJ + float H[3][6] = {{ 0., 0., b_yaw.x, 0., 0., 0.}, + { 0., 0., b_yaw.y, 0., 0., 0.}, + { 0., 0., b_yaw.z, 0., 0., 0.} + }; + float tmp[3][6]; + MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P); + float S[3][3]; + MAT_MUL_T(3, 6, 3, S, tmp, H); + + /* add the measurement noise */ + S[0][0] += noise->x; + S[1][1] += noise->y; + S[2][2] += noise->z; + + float invS[3][3]; + MAT_INV33(invS, S); + + // K = PH'invS + float tmp2[6][3]; + MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H); + float K[6][3]; + MAT_MUL(6, 3, 3, K, tmp2, invS); + + // P = (I-KH)P + float tmp3[6][6]; + MAT_MUL(6, 3, 6, tmp3, K, H); + float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. }, + { 0., 1., 0., 0., 0., 0. }, + { 0., 0., 1., 0., 0., 0. }, + { 0., 0., 0., 1., 0., 0. }, + { 0., 0., 0., 0., 1., 0. }, + { 0., 0., 0., 0., 0., 1. } + }; + float tmp4[6][6]; + MAT_SUB(6, 6, tmp4, I6, tmp3); + float tmp5[6][6]; + MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P); + memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P)); + + // X = X + Ke + struct FloatVect3 e; + VECT3_DIFF(e, *b_measured, b_expected); + ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z; + ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z; + ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z; + ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z; + ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z; + ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z; + +} /** * Incorporate errors to reference and zeros state */ -static inline void reset_state(void) { +static inline void reset_state(void) +{ - ahrs_impl.gibbs_cor.qi = 2.; + ahrs_mlkf.gibbs_cor.qi = 2.; struct FloatQuat q_tmp; - float_quat_comp(&q_tmp, &ahrs_impl.ltp_to_imu_quat, &ahrs_impl.gibbs_cor); + float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.gibbs_cor); float_quat_normalize(&q_tmp); - memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat)); - float_quat_identity(&ahrs_impl.gibbs_cor); + memcpy(&ahrs_mlkf.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_mlkf.ltp_to_imu_quat)); + float_quat_identity(&ahrs_mlkf.gibbs_cor); } /** * Compute body orientation and rates from imu orientation and rates */ -static inline void set_body_state_from_quat(void) { - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); +static inline void set_body_state_from_quat(void) +{ + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu); /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; - float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat); + float_quat_comp_inv(<p_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat); /* Set in state interface */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; - float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate); /* Set state */ stateSetBodyRates_f(&body_rate); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h index 58a4e3c32d..b75501f5d3 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h @@ -31,9 +31,14 @@ #ifndef AHRS_FLOAT_MLKF_H #define AHRS_FLOAT_MLKF_H -#include "subsystems/ahrs.h" #include "std.h" #include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" + +enum AhrsMlkfStatus { + AHRS_MLKF_UNINIT, + AHRS_MLKF_RUNNING +}; struct AhrsMlkf { struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion @@ -48,8 +53,25 @@ struct AhrsMlkf { struct FloatQuat gibbs_cor; float P[6][6]; float lp_accel; + + /** body_to_imu rotation */ + struct OrientationReps body_to_imu; + + enum AhrsMlkfStatus status; + bool_t is_aligned; }; -extern struct AhrsMlkf ahrs_impl; +extern struct AhrsMlkf ahrs_mlkf; + +extern void ahrs_mlkf_init(void); +extern void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu); +extern void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i); +extern bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag); +extern void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt); +extern void ahrs_mlkf_update_accel(struct Int32Vect3 *accel); +extern void ahrs_mlkf_update_mag(struct Int32Vect3 *mag); +extern void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag); +extern void ahrs_mlkf_update_mag_full(struct Int32Vect3 *mag); #endif /* AHRS_FLOAT_MLKF_H */ 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..c1976a13cc --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c @@ -0,0 +1,130 @@ +/* + * 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 abi_event body_to_imu_ev; + + +static void gyro_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + 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.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_mlkf_propagate(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(gyro, dt); + } +#endif +} + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *accel) +{ + if (ahrs_mlkf.is_aligned) { + ahrs_mlkf_update_accel(accel); + } +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *mag) +{ + if (ahrs_mlkf.is_aligned) { + ahrs_mlkf_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 (!ahrs_mlkf.is_aligned) { + ahrs_mlkf_align(lp_accel, lp_accel, lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + struct FloatQuat *q_b2i_f) +{ + ahrs_mlkf_set_body_to_imu_quat(q_b2i_f); +} + +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); + AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_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..4c623a75e3 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h @@ -0,0 +1,36 @@ +/* + * 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, see + * . + */ + +/** + * @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 */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h index 04f837f634..e3289932cb 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h @@ -35,13 +35,15 @@ #include "std.h" // for ABS -static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { +static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct Int32Vect3 *accel, + struct Int32Vect3 *mag) +{ /* get phi and theta from accelerometer */ struct FloatVect3 accelf; ACCELS_FLOAT_OF_BFP(accelf, *accel); const float phi = atan2f(-accelf.y, -accelf.z); const float cphi = cosf(phi); - const float theta = atan2f(cphi*accelf.x, -accelf.z); + const float theta = atan2f(cphi * accelf.x, -accelf.z); /* get psi from magnetometer */ /* project mag on local tangeant plane */ @@ -50,26 +52,26 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st const float sphi = sinf(phi); const float ctheta = cosf(theta); const float stheta = sinf(theta); - const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; - const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; + const float mn = ctheta * magf.x + sphi * stheta * magf.y + cphi * stheta * magf.z; + const float me = 0. * magf.x + cphi * magf.y - sphi * magf.z; float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X); - if (psi > M_PI) psi -= 2.*M_PI; if (psi < -M_PI) psi+= 2.*M_PI; + if (psi > M_PI) { psi -= 2.*M_PI; } if (psi < -M_PI) { psi += 2.*M_PI; } EULERS_ASSIGN(*e, phi, theta, psi); } /** Compute a quaternion representing roll and pitch from an accelerometer measurement. */ -static inline void ahrs_float_get_quat_from_accel(struct FloatQuat* q, struct Int32Vect3* accel) { +static inline void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct Int32Vect3 *accel) +{ /* normalized accel measurement in floating point */ struct FloatVect3 acc_normalized; ACCELS_FLOAT_OF_BFP(acc_normalized, *accel); FLOAT_VECT3_NORMALIZE(acc_normalized); /* check for 180deg case */ - if ( ABS(acc_normalized.z - 1.0) < 5*FLT_MIN ) { + if (ABS(acc_normalized.z - 1.0) < 5 * FLT_MIN) { QUAT_ASSIGN(*q, 0.0, 1.0, 0.0, 0.0); - } - else { + } else { /* * axis we want to rotate around is cross product of accel and reference [0,0,-g] * normalized: cross(acc_normalized, [0,0,-1]) @@ -84,7 +86,9 @@ static inline void ahrs_float_get_quat_from_accel(struct FloatQuat* q, struct In } } -static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) { +static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, + struct Int32Vect3 *mag) +{ /* the quaternion representing roll and pitch from acc measurement */ struct FloatQuat q_a; @@ -109,10 +113,10 @@ static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struc /* |v1||v2| */ float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y)) - * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y)); + * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y)); // catch 180deg case - if (ABS(norm2 + dot) < 5*FLT_MIN) { + if (ABS(norm2 + dot) < 5 * FLT_MIN) { QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0); } else { /* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c index aa6499eddf..d72bf0c17b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c +++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c @@ -19,6 +19,7 @@ * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ + /** * @file ahrs_gx3.c * @@ -29,28 +30,31 @@ * * @author Michal Podhradsky */ + #include "subsystems/ahrs/ahrs_gx3.h" +// for ahrs_register_impl +#include "subsystems/ahrs.h" #define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8) -/* +/** * Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. * Positive pitch : nose up * Positive roll : right wing down * Positive yaw : clockwise */ -struct AhrsFloatQuat ahrs_impl; -struct AhrsAligner ahrs_aligner; +struct AhrsGX3 ahrs_gx3; static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add); static inline float bef(volatile uint8_t *c); /* Big Endian to Float */ -static inline float bef(volatile uint8_t *c) { +static inline float bef(volatile uint8_t *c) +{ float f; - int8_t * p; - p = ((int8_t *)&f)+3; + int8_t *p; + p = ((int8_t *)&f) + 3; *p-- = *c++; *p-- = *c++; *p-- = *c++; @@ -58,17 +62,19 @@ static inline float bef(volatile uint8_t *c) { return f; } -static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add) { - uint16_t i,chk_calc; +static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add) +{ + uint16_t i, chk_calc; chk_calc = 0; - for (i=0;ix; float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y; heading = atan2f(w_ve, w_vn); - if (heading < 0.) + if (heading < 0.) { heading += 2 * M_PI; + } } -void ahrs_update_infrared(void) { +void ahrs_update_infrared(void) +{ float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral; float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral; - if (theta < -M_PI_2) theta += M_PI; - else if (theta > M_PI_2) theta -= M_PI; + if (theta < -M_PI_2) { theta += M_PI; } + else if (theta > M_PI_2) { theta -= M_PI; } - if (phi >= 0) phi *= infrared.correction_right; - else phi *= infrared.correction_left; + if (phi >= 0) { phi *= infrared.correction_right; } + else { phi *= infrared.correction_left; } - if (theta >= 0) theta *= infrared.correction_up; - else theta *= infrared.correction_down; + if (theta >= 0) { theta *= infrared.correction_up; } + else { theta *= infrared.correction_down; } struct FloatEulers att = { phi, theta, heading }; stateSetNedToBodyEulers_f(&att); } - diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h index fddea4cfae..8e40ee013a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.h +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.h @@ -29,9 +29,21 @@ #ifndef AHRS_INFRARED_H #define AHRS_INFRARED_H -#include "subsystems/ahrs.h" #include "std.h" +#include "subsystems/ahrs.h" +#include "math/pprz_orientation_conversion.h" +struct AhrsInfrared { + bool_t is_aligned; +}; + +extern struct AhrsInfrared ahrs_infrared; + +extern void ahrs_infrared_register(void); +extern void ahrs_infrared_init(void); extern void ahrs_update_infrared(void); +extern void ahrs_infrared_update_gps(void); + +#define DefaultAhrsImpl ahrs_infrared #endif /* AHRS_INFRARED_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 5563c0deaa..ba6f6899af 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -31,8 +31,7 @@ #include "ahrs_int_cmpl_euler.h" #include "state.h" -#include "subsystems/imu.h" -#include "subsystems/ahrs/ahrs_aligner.h" +#include "subsystems/abi.h" #include "math/pprz_trig_int.h" #include "math/pprz_algebra_int.h" @@ -46,10 +45,12 @@ #define AHRS_MAG_OFFSET 0. #endif -struct AhrsIntCmplEuler ahrs_impl; +struct AhrsIntCmplEuler ahrs_ice; -static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel); -static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag); +static inline void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, int32_t *theta_meas, + struct Int32Vect3 *accel); +static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, int32_t theta_est, + struct Int32Vect3 *mag); static inline void set_body_state_from_euler(void); #define F_UPDATE 512 @@ -61,82 +62,46 @@ static inline void set_body_state_from_euler(void); while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \ } -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" -static void send_filter(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_FILTER(trans, dev, AC_ID, - &ahrs_impl.ltp_to_imu_euler.phi, - &ahrs_impl.ltp_to_imu_euler.theta, - &ahrs_impl.ltp_to_imu_euler.psi, - &ahrs_impl.measure.phi, - &ahrs_impl.measure.theta, - &ahrs_impl.measure.psi, - &ahrs_impl.hi_res_euler.phi, - &ahrs_impl.hi_res_euler.theta, - &ahrs_impl.hi_res_euler.psi, - &ahrs_impl.residual.phi, - &ahrs_impl.residual.theta, - &ahrs_impl.residual.psi, - &ahrs_impl.gyro_bias.p, - &ahrs_impl.gyro_bias.q, - &ahrs_impl.gyro_bias.r); +void ahrs_ice_init(void) +{ + ahrs_ice.status = AHRS_ICE_UNINIT; + ahrs_ice.is_aligned = FALSE; + + /* init ltp_to_imu to zero */ + INT_EULERS_ZERO(ahrs_ice.ltp_to_imu_euler) + INT_RATES_ZERO(ahrs_ice.imu_rate); + + INT_RATES_ZERO(ahrs_ice.gyro_bias); + ahrs_ice.reinj_1 = FACE_REINJ_1; + + ahrs_ice.mag_offset = AHRS_MAG_OFFSET; } -static void send_euler(struct transport_tx *trans, struct link_device *dev) { - struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); - pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - &ahrs_impl.ltp_to_imu_euler.phi, - &ahrs_impl.ltp_to_imu_euler.theta, - &ahrs_impl.ltp_to_imu_euler.psi, - &(eulers->phi), - &(eulers->theta), - &(eulers->psi)); -} +bool_t ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ -static void send_bias(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, - &ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r); -} -#endif + get_phi_theta_measurement_fom_accel(&ahrs_ice.hi_res_euler.phi, + &ahrs_ice.hi_res_euler.theta, lp_accel); + get_psi_measurement_from_mag(&ahrs_ice.hi_res_euler.psi, + ahrs_ice.hi_res_euler.phi / F_UPDATE, + ahrs_ice.hi_res_euler.theta / F_UPDATE, lp_mag); -void ahrs_init(void) { - ahrs.status = AHRS_UNINIT; - - /* set ltp_to_imu so that body is zero */ - memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_i(&imu.body_to_imu), - sizeof(struct Int32Eulers)); - INT_RATES_ZERO(ahrs_impl.imu_rate); - - INT_RATES_ZERO(ahrs_impl.gyro_bias); - ahrs_impl.reinj_1 = FACE_REINJ_1; - - ahrs_impl.mag_offset = AHRS_MAG_OFFSET; - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter); - register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); - register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); -#endif -} - -void ahrs_align(void) { - - get_phi_theta_measurement_fom_accel(&ahrs_impl.hi_res_euler.phi, &ahrs_impl.hi_res_euler.theta, ahrs_aligner.lp_accel); - get_psi_measurement_from_mag(&ahrs_impl.hi_res_euler.psi, - ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag); - - EULERS_COPY(ahrs_impl.measure, ahrs_impl.hi_res_euler); - EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler); + EULERS_COPY(ahrs_ice.measure, ahrs_ice.hi_res_euler); + EULERS_COPY(ahrs_ice.measurement, ahrs_ice.hi_res_euler); /* Compute LTP to IMU eulers */ - EULERS_SDIV(ahrs_impl.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); + EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE); set_body_state_from_euler(); - RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); - ahrs.status = AHRS_RUNNING; + RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro); + ahrs_ice.status = AHRS_ICE_RUNNING; + ahrs_ice.is_aligned = TRUE; + + return TRUE; } //#define USE_NOISE_CUT 1 @@ -145,7 +110,8 @@ void ahrs_align(void) { #if USE_NOISE_CUT #include "led.h" -static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) { +static inline bool_t cut_rates(struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) +{ struct Int32Rates diff; RATES_DIFF(diff, i1, i2); if (diff.p < -threshold || diff.p > threshold || @@ -158,7 +124,8 @@ static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int3 } #define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1) -static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) { +static inline bool_t cut_accel(struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) +{ struct Int32Vect3 diff; VECT3_DIFF(diff, i1, i2); if (diff.x < -threshold || diff.x > threshold || @@ -189,22 +156,23 @@ static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int3 * */ -void ahrs_propagate(float dt __attribute__((unused))) { +void ahrs_ice_propagate(struct Int32Rates *gyro) +{ /* unbias gyro */ struct Int32Rates uf_rate; - RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias); + RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias); #if USE_NOISE_CUT static struct Int32Rates last_uf_rate = { 0, 0, 0 }; if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) { #endif /* low pass rate */ #if USE_NOISE_FILTER - RATES_SUM_SCALED(ahrs_impl.imu_rate, ahrs_impl.imu_rate, uf_rate, NOISE_FILTER_GAIN); - RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, NOISE_FILTER_GAIN+1); + RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN); + RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1); #else - RATES_ADD(ahrs_impl.imu_rate, uf_rate); - RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 2); + RATES_ADD(ahrs_ice.imu_rate, uf_rate); + RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2); #endif #if USE_NOISE_CUT } @@ -213,74 +181,81 @@ void ahrs_propagate(float dt __attribute__((unused))) { /* integrate eulers */ struct Int32Eulers euler_dot; - int32_eulers_dot_of_rates(&euler_dot, &ahrs_impl.ltp_to_imu_euler, &ahrs_impl.imu_rate); - EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot); + int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate); + EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot); /* low pass measurement */ - EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement); - EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2); + EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement); + EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2); /* compute residual */ - EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler); - INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi); + EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler); + INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi); struct Int32Eulers correction; /* compute a correction */ - EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1); + EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1); /* correct estimation */ - EULERS_ADD(ahrs_impl.hi_res_euler, correction); - INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi); + EULERS_ADD(ahrs_ice.hi_res_euler, correction); + INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi); /* Compute LTP to IMU eulers */ - EULERS_SDIV(ahrs_impl.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); + EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE); set_body_state_from_euler(); } -void ahrs_update_accel(float dt __attribute__((unused))) { +void ahrs_ice_update_accel(struct Int32Vect3 *accel) +{ #if USE_NOISE_CUT || USE_NOISE_FILTER static struct Int32Vect3 last_accel = { 0, 0, 0 }; #endif #if USE_NOISE_CUT - if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) { + if (!cut_accel(*accel, last_accel, ACCEL_CUT_THRESHOLD)) { #endif #if USE_NOISE_FILTER - VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN); - VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1); + VECT3_SUM_SCALED(*accel, *accel, last_accel, NOISE_FILTER_GAIN); + VECT3_SDIV(*accel, *accel, NOISE_FILTER_GAIN + 1); #endif - get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel); + get_phi_theta_measurement_fom_accel(&ahrs_ice.measurement.phi, &ahrs_ice.measurement.theta, accel); #if USE_NOISE_CUT } - VECT3_COPY(last_accel, imu.accel); + VECT3_COPY(last_accel, *accel); #endif } -void ahrs_update_mag(float dt __attribute__((unused))) { +void ahrs_ice_update_mag(struct Int32Vect3 *mag) +{ - get_psi_measurement_from_mag(&ahrs_impl.measurement.psi, ahrs_impl.ltp_to_imu_euler.phi, ahrs_impl.ltp_to_imu_euler.theta, imu.mag); + get_psi_measurement_from_mag(&ahrs_ice.measurement.psi, ahrs_ice.ltp_to_imu_euler.phi, ahrs_ice.ltp_to_imu_euler.theta, + mag); } /* measures phi and theta assuming no dynamic acceleration ?!! */ -__attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) { +__attribute__((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, + int32_t *theta_meas, struct Int32Vect3 *accel) +{ - *phi_meas = int32_atan2(-accel.y, -accel.z); + *phi_meas = int32_atan2(-accel->y, -accel->z); int32_t cphi; PPRZ_ITRIG_COS(cphi, *phi_meas); - int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel.x, INT32_TRIG_FRAC); - *theta_meas = int32_atan2(-cphi_ax, -accel.z); + int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC); + *theta_meas = int32_atan2(-cphi_ax, -accel->z); *phi_meas *= F_UPDATE; *theta_meas *= F_UPDATE; } /* measure psi by projecting magnetic vector in local tangeant plan */ -__attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag) { +__attribute__((always_inline)) static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, + int32_t theta_est, struct Int32Vect3 *mag) +{ int32_t sphi; PPRZ_ITRIG_SIN(sphi, phi_est); @@ -291,28 +266,29 @@ __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag( int32_t ctheta; PPRZ_ITRIG_COS(ctheta, theta_est); - int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC; - int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC; + int32_t sphi_stheta = (sphi * stheta) >> INT32_TRIG_FRAC; + int32_t cphi_stheta = (cphi * stheta) >> INT32_TRIG_FRAC; //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC; //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC; - const int32_t mn = ctheta * mag.x + sphi_stheta * mag.y + cphi_stheta * mag.z; - const int32_t me = 0 * mag.x + cphi * mag.y - sphi * mag.z; + const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z; + const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z; //const int32_t md = - // -stheta * imu.mag.x + - // sphi_ctheta * imu.mag.y + - // cphi_ctheta * imu.mag.z; + // -stheta * mag->x + + // sphi_ctheta * mag->y + + // cphi_ctheta * mag->z; float m_psi = -atan2(me, mn); - *psi_meas = ((m_psi - ahrs_impl.mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE); + *psi_meas = ((m_psi - ahrs_ice.mag_offset) * (float)(1 << (INT32_ANGLE_FRAC)) * F_UPDATE); } /* Rotate angles and rates from imu to body frame and set state */ -static void set_body_state_from_euler(void) { - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); +static void set_body_state_from_euler(void) +{ + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_ice.body_to_imu); struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat; /* Compute LTP to IMU rotation matrix */ - int32_rmat_of_eulers(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); + int32_rmat_of_eulers(<p_to_imu_rmat, &ahrs_ice.ltp_to_imu_euler); /* Compute LTP to BODY rotation matrix */ int32_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); /* Set state */ @@ -320,9 +296,24 @@ static void set_body_state_from_euler(void) { struct Int32Rates body_rate; /* compute body rates */ - int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); + int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_ice.imu_rate); /* Set state */ stateSetBodyRates_i(&body_rate); } +void ahrs_ice_set_body_to_imu(struct OrientationReps *body_to_imu) +{ + ahrs_ice_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); +} + +void ahrs_ice_set_body_to_imu_quat(struct FloatQuat *q_b2i) +{ + orientationSetQuat_f(&ahrs_ice.body_to_imu, q_b2i); + + if (!ahrs_ice.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_ice.ltp_to_imu_euler, orientationGetEulers_i(&ahrs_ice.body_to_imu), + sizeof(struct Int32Eulers)); + } +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h index d410bbe69a..0a0d73079d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h @@ -33,6 +33,12 @@ #include "subsystems/ahrs.h" #include "std.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_orientation_conversion.h" + +enum AhrsICEStatus { + AHRS_ICE_UNINIT, + AHRS_ICE_RUNNING +}; struct AhrsIntCmplEuler { struct Int32Rates gyro_bias; @@ -44,9 +50,22 @@ struct AhrsIntCmplEuler { struct Int32Eulers ltp_to_imu_euler; int32_t reinj_1; float mag_offset; + + struct OrientationReps body_to_imu; + + enum AhrsICEStatus status; + bool_t is_aligned; }; -extern struct AhrsIntCmplEuler ahrs_impl; +extern struct AhrsIntCmplEuler ahrs_ice; +extern void ahrs_ice_init(void); +extern void ahrs_ice_set_body_to_imu(struct OrientationReps *body_to_imu); +extern void ahrs_ice_set_body_to_imu_quat(struct FloatQuat *q_b2i); +extern bool_t ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag); +extern void ahrs_ice_propagate(struct Int32Rates *gyro); +extern void ahrs_ice_update_accel(struct Int32Vect3 *accel); +extern void ahrs_ice_update_mag(struct Int32Vect3 *mag); #endif /* AHRS_INT_CMPL_EULER_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c new file mode 100644 index 0000000000..9937e44db8 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c @@ -0,0 +1,149 @@ +/* + * 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_int_cmpl_euler_wrapper.c + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#include "subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "state.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_filter(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_FILTER(trans, dev, AC_ID, + &ahrs_ice.ltp_to_imu_euler.phi, + &ahrs_ice.ltp_to_imu_euler.theta, + &ahrs_ice.ltp_to_imu_euler.psi, + &ahrs_ice.measure.phi, + &ahrs_ice.measure.theta, + &ahrs_ice.measure.psi, + &ahrs_ice.hi_res_euler.phi, + &ahrs_ice.hi_res_euler.theta, + &ahrs_ice.hi_res_euler.psi, + &ahrs_ice.residual.phi, + &ahrs_ice.residual.theta, + &ahrs_ice.residual.psi, + &ahrs_ice.gyro_bias.p, + &ahrs_ice.gyro_bias.q, + &ahrs_ice.gyro_bias.r); +} + +static void send_euler(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Eulers *eulers = stateGetNedToBodyEulers_i(); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, + &ahrs_ice.ltp_to_imu_euler.phi, + &ahrs_ice.ltp_to_imu_euler.theta, + &ahrs_ice.ltp_to_imu_euler.psi, + &(eulers->phi), + &(eulers->theta), + &(eulers->psi)); +} + +static void send_bias(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, + &ahrs_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, &ahrs_ice.gyro_bias.r); +} +#endif + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_ICE_IMU_ID +#define AHRS_ICE_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 sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Rates *gyro) +{ + if (ahrs_ice.is_aligned) { + ahrs_ice_propagate(gyro); + } + +} + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *accel) +{ + if (ahrs_ice.is_aligned) { + ahrs_ice_update_accel(accel); + } +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *mag) +{ + if (ahrs_ice.is_aligned) { + ahrs_ice_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 (!ahrs_ice.is_aligned) { + ahrs_ice_align(lp_gyro, lp_accel, lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + struct FloatQuat *q_b2i_f) +{ + ahrs_ice_set_body_to_imu_quat(q_b2i_f); +} + +void ahrs_ice_register(void) +{ + ahrs_register_impl(ahrs_ice_init, NULL); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_ICE_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_ICE_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_ICE_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); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter); + register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); + register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h new file mode 100644 index 0000000000..051ce689f2 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_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_int_cmpl_euler_wrapper.h + * + * Paparazzi specific wrapper to run floating point DCM filter. + */ + +#ifndef AHRS_INT_CMPL_EULER_WRAPPER_H +#define AHRS_INT_CMPL_EULER_WRAPPER_H + +#include "subsystems/ahrs/ahrs_int_cmpl_euler.h" + +#define DefaultAhrsImpl ahrs_ice + +extern void ahrs_ice_register(void); + +#endif /* AHRS_INT_CMPL_EULER_WRAPPER_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 031f80f2b9..7aa22a9997 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -31,12 +31,9 @@ #include "generated/airframe.h" #include "subsystems/ahrs/ahrs_int_cmpl_quat.h" -#include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ahrs/ahrs_int_utils.h" - #include "state.h" -#include "subsystems/imu.h" #if USE_GPS #include "subsystems/gps.h" #endif @@ -105,172 +102,131 @@ PRINT_CONFIG_VAR(AHRS_MAG_ZETA) #define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0 #endif -struct AhrsIntCmplQuat ahrs_impl; +struct AhrsIntCmplQuat ahrs_icq; static inline void set_body_state_from_quat(void); -static inline void UNUSED ahrs_update_mag_full(float dt); -static inline void ahrs_update_mag_2d(float dt); +static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt); +static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt); -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" +void ahrs_icq_init(void) +{ -static void send_quat(struct transport_tx *trans, struct link_device *dev) { - struct Int32Quat* quat = stateGetNedToBodyQuat_i(); - pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID, - &ahrs_impl.weight, - &ahrs_impl.ltp_to_imu_quat.qi, - &ahrs_impl.ltp_to_imu_quat.qx, - &ahrs_impl.ltp_to_imu_quat.qy, - &ahrs_impl.ltp_to_imu_quat.qz, - &(quat->qi), - &(quat->qx), - &(quat->qy), - &(quat->qz)); -} + ahrs_icq.status = AHRS_ICQ_UNINIT; + ahrs_icq.is_aligned = FALSE; -static void send_euler(struct transport_tx *trans, struct link_device *dev) { - struct Int32Eulers ltp_to_imu_euler; - int32_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); - struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); - pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - <p_to_imu_euler.phi, - <p_to_imu_euler.theta, - <p_to_imu_euler.psi, - &(eulers->phi), - &(eulers->theta), - &(eulers->psi)); -} + ahrs_icq.ltp_vel_norm_valid = FALSE; + ahrs_icq.heading_aligned = FALSE; -static void send_bias(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, - &ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r); -} + /* init ltp_to_imu quaternion as zero/identity rotation */ + int32_quat_identity(&ahrs_icq.ltp_to_imu_quat); -static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { - struct FloatVect3 h_float; - h_float.x = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.x); - h_float.y = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.y); - h_float.z = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.z); - pprz_msg_send_GEO_MAG(trans, dev, AC_ID, - &h_float.x, &h_float.y, &h_float.z); -} -#endif + INT_RATES_ZERO(ahrs_icq.imu_rate); -void ahrs_init(void) { - - ahrs.status = AHRS_UNINIT; - ahrs_impl.ltp_vel_norm_valid = FALSE; - ahrs_impl.heading_aligned = FALSE; - - /* set ltp_to_imu so that body is zero */ - memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_i(&imu.body_to_imu), - sizeof(struct Int32Quat)); - INT_RATES_ZERO(ahrs_impl.imu_rate); - - INT_RATES_ZERO(ahrs_impl.gyro_bias); - INT_RATES_ZERO(ahrs_impl.rate_correction); - INT_RATES_ZERO(ahrs_impl.high_rez_bias); + INT_RATES_ZERO(ahrs_icq.gyro_bias); + INT_RATES_ZERO(ahrs_icq.rate_correction); + INT_RATES_ZERO(ahrs_icq.high_rez_bias); /* set default filter cut-off frequency and damping */ - ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; - ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; - ahrs_set_accel_gains(); - ahrs_impl.mag_omega = AHRS_MAG_OMEGA; - ahrs_impl.mag_zeta = AHRS_MAG_ZETA; - ahrs_set_mag_gains(); + ahrs_icq.accel_omega = AHRS_ACCEL_OMEGA; + ahrs_icq.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_icq_set_accel_gains(); + ahrs_icq.mag_omega = AHRS_MAG_OMEGA; + ahrs_icq.mag_zeta = AHRS_MAG_ZETA; + ahrs_icq_set_mag_gains(); /* set default gravity heuristic */ - ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; + ahrs_icq.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN - ahrs_impl.correct_gravity = TRUE; + ahrs_icq.correct_gravity = TRUE; #else - ahrs_impl.correct_gravity = FALSE; + ahrs_icq.correct_gravity = FALSE; #endif - VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), + VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); - ahrs_impl.accel_cnt = 0; - ahrs_impl.mag_cnt = 0; - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); - register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); - register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); - register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); -#endif - + ahrs_icq.accel_cnt = 0; + ahrs_icq.mag_cnt = 0; } -void ahrs_align(void) { +bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, - &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); - ahrs_impl.heading_aligned = TRUE; + ahrs_int_get_quat_from_accel_mag(&ahrs_icq.ltp_to_imu_quat, + lp_accel, lp_mag); + ahrs_icq.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ - ahrs_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); - ahrs_impl.heading_aligned = FALSE; + ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_imu_quat, lp_accel); + ahrs_icq.heading_aligned = FALSE; + // supress unused arg warning + lp_mag = lp_mag; #endif set_body_state_from_quat(); /* Use low passed gyro value as initial bias */ - RATES_COPY(ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); - RATES_COPY(ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); - INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28); + RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro); + RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro); + INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28); - ahrs.status = AHRS_RUNNING; + ahrs_icq.status = AHRS_ICQ_RUNNING; + ahrs_icq.is_aligned = TRUE; + + return TRUE; } -void ahrs_propagate(float dt) { - int32_t freq = (int32_t)(1./dt); +void ahrs_icq_propagate(struct Int32Rates *gyro, float dt) +{ + int32_t freq = (int32_t)(1. / dt); /* unbias gyro */ struct Int32Rates omega; - RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias); + RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias); /* low pass rate */ #ifdef AHRS_PROPAGATE_LOW_PASS_RATES - RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate,2); - RATES_ADD(ahrs_impl.imu_rate, omega); - RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 3); + RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 2); + RATES_ADD(ahrs_icq.imu_rate, omega); + RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 3); #else - RATES_COPY(ahrs_impl.imu_rate, omega); + RATES_COPY(ahrs_icq.imu_rate, omega); #endif /* add correction */ - RATES_ADD(omega, ahrs_impl.rate_correction); + RATES_ADD(omega, ahrs_icq.rate_correction); /* and zeros it */ - INT_RATES_ZERO(ahrs_impl.rate_correction); + INT_RATES_ZERO(ahrs_icq.rate_correction); /* integrate quaternion */ - int32_quat_integrate_fi(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.high_rez_quat, + int32_quat_integrate_fi(&ahrs_icq.ltp_to_imu_quat, &ahrs_icq.high_rez_quat, &omega, freq); - int32_quat_normalize(&ahrs_impl.ltp_to_imu_quat); + int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat); set_body_state_from_quat(); // increase accel and mag propagation counters - ahrs_impl.accel_cnt++; - ahrs_impl.mag_cnt++; + ahrs_icq.accel_cnt++; + ahrs_icq.mag_cnt++; } -void ahrs_set_accel_gains(void) { +void ahrs_icq_set_accel_gains(void) +{ /* Complementary filter proportionnal gain (without frequency correction) * Kp = 2 * omega * zeta * * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain) * accel_inv_kp = 4096 * 9.81 / Kp */ - ahrs_impl.accel_inv_kp = 4096 * 9.81 / - (2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta); + ahrs_icq.accel_inv_kp = 4096 * 9.81 / + (2 * ahrs_icq.accel_omega * ahrs_icq.accel_zeta); /* Complementary filter integral gain * Ki = omega^2 @@ -279,25 +235,28 @@ void ahrs_set_accel_gains(void) { * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki * accel_inv_ki = 9.81 / 2048 / Ki */ - ahrs_impl.accel_inv_ki = 9.81 / 2048 / (ahrs_impl.accel_omega * ahrs_impl.accel_omega); + ahrs_icq.accel_inv_ki = 9.81 / 2048 / (ahrs_icq.accel_omega * ahrs_icq.accel_omega); } -void ahrs_update_accel(float dt) { +void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt) +{ // check if we had at least one propagation since last update - if (ahrs_impl.accel_cnt == 0) + if (ahrs_icq.accel_cnt == 0) { return; + } // c2 = ltp z-axis in imu-frame struct Int32RMat ltp_to_imu_rmat; - int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); - struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2), - RMAT_ELMT(ltp_to_imu_rmat, 1,2), - RMAT_ELMT(ltp_to_imu_rmat, 2,2)}; + int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat); + struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2), + RMAT_ELMT(ltp_to_imu_rmat, 1, 2), + RMAT_ELMT(ltp_to_imu_rmat, 2, 2) + }; struct Int32Vect3 residual; struct Int32Vect3 pseudo_gravity_measurement; - if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { + if (ahrs_icq.correct_gravity && ahrs_icq.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) @@ -311,21 +270,21 @@ void ahrs_update_accel(float dt) { #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC const struct Int32Vect3 vel_tangential_body = - {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; + {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement * of the gravity vector */ - VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); + VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu); } else { - VECT3_COPY(pseudo_gravity_measurement, imu.accel); + VECT3_COPY(pseudo_gravity_measurement, *accel); } /* compute the residual of the pseudo gravity vector in imu frame */ @@ -333,14 +292,14 @@ void ahrs_update_accel(float dt) { /* FIR filtered pseudo_gravity_measurement */ - #define FIR_FILTER_SIZE 8 +#define FIR_FILTER_SIZE 8 static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; - VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1); VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - if (ahrs_impl.gravity_heuristic_factor) { + if (ahrs_icq.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. * e.g. for gravity_heuristic_factor 30: @@ -349,18 +308,17 @@ void ahrs_update_accel(float dt) { struct FloatVect3 g_meas_f; ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); - const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; - ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10; - Bound(ahrs_impl.weight, 0.15, 1.0); - } - else { - ahrs_impl.weight = 1.0; + const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f) / 9.81; + ahrs_icq.weight = 1.0 - ahrs_icq.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10; + Bound(ahrs_icq.weight, 0.15, 1.0); + } else { + ahrs_icq.weight = 1.0; } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega - * final Kp with frequency correction = Kp * ahrs_impl.accel_cnt - * with ahrs_impl.accel_cnt beeing the number of propagations since last update + * final Kp with frequency correction = Kp * ahrs_icq.accel_cnt + * with ahrs_icq.accel_cnt beeing the number of propagations since last update * * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 @@ -374,16 +332,16 @@ void ahrs_update_accel(float dt) { * inv_rate_scale = 1 / Kp / weight * inv_rate_scale = accel_inv_kp / accel_cnt / weight */ - int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.accel_cnt - / ahrs_impl.weight); + int32_t inv_rate_scale = (int32_t)(ahrs_icq.accel_inv_kp / ahrs_icq.accel_cnt + / ahrs_icq.weight); Bound(inv_rate_scale, 8192, 4194304); - ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; - ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; - ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale; + ahrs_icq.rate_correction.p -= residual.x / inv_rate_scale; + ahrs_icq.rate_correction.q -= residual.y / inv_rate_scale; + ahrs_icq.rate_correction.r -= residual.z / inv_rate_scale; // reset accel propagation counter - ahrs_impl.accel_cnt = 0; + ahrs_icq.accel_cnt = 0; /* Complementary filter integral gain * Correct the gyro bias. @@ -401,54 +359,58 @@ void ahrs_update_accel(float dt) { * inv_bias_gain = accel_inv_ki / weight^2 */ - int32_t inv_bias_gain = (int32_t)(ahrs_impl.accel_inv_ki / - (dt * ahrs_impl.weight * ahrs_impl.weight)); + int32_t inv_bias_gain = (int32_t)(ahrs_icq.accel_inv_ki / + (dt * ahrs_icq.weight * ahrs_icq.weight)); Bound(inv_bias_gain, 8, 65536) - ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; - ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; - ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; + ahrs_icq.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; + ahrs_icq.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; + ahrs_icq.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; - INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); + INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28); } -void ahrs_update_mag(float dt) { +void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt) +{ // check if we had at least one propagation since last update - if (ahrs_impl.mag_cnt == 0) + if (ahrs_icq.mag_cnt == 0) { return; + } #if AHRS_MAG_UPDATE_ALL_AXES - ahrs_update_mag_full(dt); + ahrs_icq_update_mag_full(mag, dt); #else - ahrs_update_mag_2d(dt); + ahrs_icq_update_mag_2d(mag, dt); #endif // reset mag propagation counter - ahrs_impl.mag_cnt = 0; + ahrs_icq.mag_cnt = 0; } -void ahrs_set_mag_gains(void) { +void ahrs_icq_set_mag_gains(void) +{ /* Complementary filter proportionnal gain = 2*omega*zeta */ - ahrs_impl.mag_kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega; + ahrs_icq.mag_kp = 2 * ahrs_icq.mag_zeta * ahrs_icq.mag_omega; /* Complementary filter integral gain = omega^2 */ - ahrs_impl.mag_ki = ahrs_impl.mag_omega * ahrs_impl.mag_omega; + ahrs_icq.mag_ki = ahrs_icq.mag_omega * ahrs_icq.mag_omega; } -static inline void ahrs_update_mag_full(float dt) { +static inline void ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt) +{ struct Int32RMat ltp_to_imu_rmat; - int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); + int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat); struct Int32Vect3 expected_imu; - int32_rmat_vmult(&expected_imu, <p_to_imu_rmat, &ahrs_impl.mag_h); + int32_rmat_vmult(&expected_imu, <p_to_imu_rmat, &ahrs_icq.mag_h); struct Int32Vect3 residual; - VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); + VECT3_CROSS_PRODUCT(residual, *mag, expected_imu); /* Complementary filter proportionnal gain. * Kp = 2 * mag_zeta * mag_omega - * final Kp with frequency correction = Kp * ahrs_impl.mag_cnt - * with ahrs_impl.mag_cnt beeing the number of propagations since last update + * final Kp with frequency correction = Kp * ahrs_icq.mag_cnt + * with ahrs_icq.mag_cnt beeing the number of propagations since last update * * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 @@ -458,11 +420,11 @@ static inline void ahrs_update_mag_full(float dt) { * inv_rate_gain = 1024 / Kp */ - const int32_t inv_rate_gain = (int32_t)(1024.0 / (ahrs_impl.mag_kp * ahrs_impl.mag_cnt)); + const int32_t inv_rate_gain = (int32_t)(1024.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt)); - ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; - ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; - ahrs_impl.rate_correction.r += residual.z / inv_rate_gain; + ahrs_icq.rate_correction.p += residual.x / inv_rate_gain; + ahrs_icq.rate_correction.q += residual.y / inv_rate_gain; + ahrs_icq.rate_correction.r += residual.z / inv_rate_gain; /* Complementary filter integral gain * Correct the gyro bias. @@ -474,39 +436,41 @@ static inline void ahrs_update_mag_full(float dt) { * * bias_gain = Ki * FRAC_conversion = Ki * 2^18 */ - const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * dt * (1<<18)); + const int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 18)); - ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; - ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; - ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; + ahrs_icq.high_rez_bias.p -= residual.x * bias_gain; + ahrs_icq.high_rez_bias.q -= residual.y * bias_gain; + ahrs_icq.high_rez_bias.r -= residual.z * bias_gain; - INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); + INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28); } -static inline void ahrs_update_mag_2d(float dt) { +static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt) +{ - struct Int32Vect2 expected_ltp = {ahrs_impl.mag_h.x, ahrs_impl.mag_h.y}; + struct Int32Vect2 expected_ltp = {ahrs_icq.mag_h.x, ahrs_icq.mag_h.y}; /* normalize expected ltp in 2D (x,y) */ int32_vect2_normalize(&expected_ltp, INT32_MAG_FRAC); struct Int32RMat ltp_to_imu_rmat; - int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); + int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat); struct Int32Vect3 measured_ltp; - int32_rmat_transp_vmult(&measured_ltp, <p_to_imu_rmat, &imu.mag); + int32_rmat_transp_vmult(&measured_ltp, <p_to_imu_rmat, mag); /* normalize measured ltp in 2D (x,y) */ struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y}; int32_vect2_normalize(&measured_ltp_2d, INT32_MAG_FRAC); /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */ - struct Int32Vect3 residual_ltp = - { 0, - 0, - (measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x)/(1<<5)}; + struct Int32Vect3 residual_ltp = { + 0, + 0, + (measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x) / (1 << 5) + }; struct Int32Vect3 residual_imu; @@ -514,8 +478,8 @@ static inline void ahrs_update_mag_2d(float dt) { /* Complementary filter proportionnal gain. * Kp = 2 * mag_zeta * mag_omega - * final Kp with frequency correction = Kp * ahrs_impl.mag_cnt - * with ahrs_impl.mag_cnt beeing the number of propagations since last update + * final Kp with frequency correction = Kp * ahrs_icq.mag_cnt + * with ahrs_icq.mag_cnt beeing the number of propagations since last update * * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 @@ -524,11 +488,11 @@ static inline void ahrs_update_mag_2d(float dt) { * inv_rate_gain = 1 / Kp / FRAC_conversion * inv_rate_gain = 32 / Kp */ - int32_t inv_rate_gain = (int32_t)(32.0 / (ahrs_impl.mag_kp * ahrs_impl.mag_cnt)); + int32_t inv_rate_gain = (int32_t)(32.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt)); - ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain); - ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain); - ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain); + ahrs_icq.rate_correction.p += (residual_imu.x / inv_rate_gain); + ahrs_icq.rate_correction.q += (residual_imu.y / inv_rate_gain); + ahrs_icq.rate_correction.r += (residual_imu.z / inv_rate_gain); /* Complementary filter integral gain * Correct the gyro bias. @@ -540,23 +504,24 @@ static inline void ahrs_update_mag_2d(float dt) { * * bias_gain = Ki * FRAC_conversion = Ki * 2^23 */ - int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * dt * (1 << 23)); + int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 23)); - ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain); - ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain); - ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain); + ahrs_icq.high_rez_bias.p -= (residual_imu.x * bias_gain); + ahrs_icq.high_rez_bias.q -= (residual_imu.y * bias_gain); + ahrs_icq.high_rez_bias.r -= (residual_imu.z * bias_gain); - INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); + INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28); } -void ahrs_update_gps(void) { +void ahrs_icq_update_gps(void) +{ #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS if (gps.fix == GPS_FIX_3D) { - ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(gps.speed_3d / 100.); - ahrs_impl.ltp_vel_norm_valid = TRUE; + ahrs_icq.ltp_vel_norm = SPEED_BFP_OF_REAL(gps.speed_3d / 100.); + ahrs_icq.ltp_vel_norm_valid = TRUE; } else { - ahrs_impl.ltp_vel_norm_valid = FALSE; + ahrs_icq.ltp_vel_norm_valid = FALSE; } #endif @@ -565,58 +530,60 @@ void ahrs_update_gps(void) { // and course accuracy is better than 10deg if (gps.fix == GPS_FIX_3D && gps.gspeed >= (AHRS_HEADING_UPDATE_GPS_MIN_SPEED * 100) && - gps.cacc <= RadOfDeg(10*1e7)) { + gps.cacc <= RadOfDeg(10 * 1e7)) { // gps.course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC - int32_t course = gps.course * ((1< + * + * 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_int_cmpl_quat_wrapper.c + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#include "subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "state.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +static void send_quat(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Quat *quat = stateGetNedToBodyQuat_i(); + pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID, + &ahrs_icq.weight, + &ahrs_icq.ltp_to_imu_quat.qi, + &ahrs_icq.ltp_to_imu_quat.qx, + &ahrs_icq.ltp_to_imu_quat.qy, + &ahrs_icq.ltp_to_imu_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz)); +} + +static void send_euler(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Eulers ltp_to_imu_euler; + int32_eulers_of_quat(<p_to_imu_euler, &ahrs_icq.ltp_to_imu_quat); + struct Int32Eulers *eulers = stateGetNedToBodyEulers_i(); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, + <p_to_imu_euler.phi, + <p_to_imu_euler.theta, + <p_to_imu_euler.psi, + &(eulers->phi), + &(eulers->theta), + &(eulers->psi)); +} + +static void send_bias(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, + &ahrs_icq.gyro_bias.p, &ahrs_icq.gyro_bias.q, &ahrs_icq.gyro_bias.r); +} + +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) +{ + struct FloatVect3 h_float; + h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x); + h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y); + h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z); + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, + &h_float.x, &h_float.y, &h_float.z); +} +#endif + + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_ICQ_IMU_ID +#define AHRS_ICQ_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, + uint32_t __attribute__((unused)) stamp, + struct Int32Rates *gyro) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_icq_propagate(gyro, dt); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.") + PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + if (ahrs_icq.status == AHRS_ICQ_RUNNING) { + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + ahrs_icq_propagate(gyro, dt); + } +#endif +} + +static void accel_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + struct Int32Vect3 *accel) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_icq_update_accel(accel, dt); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.") + PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + if (ahrs_icq.is_aligned) { + const float dt = 1. / (AHRS_CORRECT_FREQUENCY); + ahrs_icq_update_accel(accel, dt); + } +#endif +} + +static void mag_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + struct Int32Vect3 *mag) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_icq_update_mag(mag, dt); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.") + PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + if (ahrs_icq.is_aligned) { + const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); + ahrs_icq_update_mag(mag, dt); + } +#endif +} + +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 (!ahrs_icq.is_aligned) { + ahrs_icq_align(lp_gyro, lp_accel, lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + struct FloatQuat *q_b2i_f) +{ + ahrs_icq_set_body_to_imu_quat(q_b2i_f); +} + +void ahrs_icq_register(void) +{ + ahrs_register_impl(ahrs_icq_init, ahrs_icq_update_gps); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_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); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); + register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); + register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); + register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h new file mode 100644 index 0000000000..e64c3a9b04 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_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_int_cmpl_quat_wrapper.h + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#ifndef AHRS_INT_CMPL_QUAT_WRAPPER_H +#define AHRS_INT_CMPL_QUAT_WRAPPER_H + +#include "subsystems/ahrs/ahrs_int_cmpl_quat.h" + +#define DefaultAhrsImpl ahrs_icq + +extern void ahrs_icq_register(void); + +#endif /* AHRS_INT_CMPL_QUAT_WRAPPER_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h index d1f930848e..330be7eeac 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h @@ -37,7 +37,9 @@ #include "subsystems/ahrs/ahrs_float_utils.h" -static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { +static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers *e, struct Int32Vect3 *accel, + struct Int32Vect3 *mag) +{ // DISPLAY_INT32_VECT3("# accel", (*accel)); const float fphi = atan2f(-accel->y, -accel->z); // printf("# atan float %f\n", DegOfRad(fphi)); @@ -56,8 +58,8 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru int32_t ctheta; PPRZ_ITRIG_COS(ctheta, e->theta); - int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC; - int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC; + int32_t sphi_stheta = (sphi * stheta) >> INT32_TRIG_FRAC; + int32_t cphi_stheta = (cphi * stheta) >> INT32_TRIG_FRAC; //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC; //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC; @@ -75,13 +77,16 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru } -static inline void ahrs_int_get_quat_from_accel(struct Int32Quat* q, struct Int32Vect3* accel) { +static inline void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel) +{ struct FloatQuat q_f; ahrs_float_get_quat_from_accel(&q_f, accel); QUAT_BFP_OF_REAL(*q, q_f); } -static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) { +static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, + struct Int32Vect3 *mag) +{ struct FloatQuat q_f; ahrs_float_get_quat_from_accel_mag(&q_f, accel, mag); diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c index 92253a7bcb..2dd120f445 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_sim.c +++ b/sw/airborne/subsystems/ahrs/ahrs_sim.c @@ -26,10 +26,10 @@ * */ -#include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_sim.h" #include "math/pprz_algebra_float.h" #include "generated/airframe.h" +#include "state.h" extern float sim_phi; extern float sim_theta; @@ -37,7 +37,6 @@ extern float sim_psi; extern float sim_p; extern float sim_q; extern float sim_r; -extern bool_t ahrs_sim_available; // for sim of e.g. Xsens ins #ifndef INS_ROLL_NEUTRAL_DEFAULT @@ -50,7 +49,8 @@ float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; -void update_ahrs_from_sim(void) { +void update_ahrs_from_sim(void) +{ struct FloatEulers ltp_to_imu_euler = { sim_phi, sim_theta, sim_psi }; struct FloatRates imu_rate = { sim_p, sim_q, sim_r }; @@ -61,31 +61,6 @@ void update_ahrs_from_sim(void) { } -void ahrs_init(void) { - //ahrs_float.status = AHRS_UNINIT; - // set to running for now - ahrs.status = AHRS_RUNNING; - - ahrs_sim_available = FALSE; - -} - -void ahrs_align(void) +void ahrs_sim_init(void) { - /* Currently not really simulated - * body and imu have the same frame and always set to true value from sim - */ - - update_ahrs_from_sim(); - - ahrs.status = AHRS_RUNNING; } - - -void ahrs_propagate(float dt __attribute__((unused))) { - if (ahrs_sim_available) { - update_ahrs_from_sim(); - ahrs_sim_available = FALSE; - } -} - diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.h b/sw/airborne/subsystems/ahrs/ahrs_sim.h index ad5ff60546..586bd2b4c7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_sim.h +++ b/sw/airborne/subsystems/ahrs/ahrs_sim.h @@ -32,11 +32,10 @@ #include "subsystems/ahrs.h" #include "std.h" -extern bool_t ahrs_sim_available; - extern float ins_roll_neutral; extern float ins_pitch_neutral; extern void update_ahrs_from_sim(void); +extern void ahrs_sim_init(void); #endif /* AHRS_SIM_H */ diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index b1f6880db9..a17ef8fa94 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -30,6 +30,7 @@ #include "subsystems/imu.h" #include "state.h" +#include "subsystems/abi.h" #ifdef IMU_POWER_GPIO #include "mcu_periph/gpio.h" @@ -42,22 +43,6 @@ #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -#if USE_IMU_FLOAT - -static void send_accel(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, - &imuf.accel.x, &imuf.accel.y, &imuf.accel.z); -} - -static void send_gyro(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, - &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r); -} - -#else // !USE_IMU_FLOAT - static void send_accel_raw(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, @@ -117,12 +102,10 @@ static void send_mag(struct transport_tx *trans, struct link_device *dev) pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &mag_float.x, &mag_float.y, &mag_float.z); } -#endif // !USE_IMU_FLOAT -#endif +#endif /* PERIODIC_TELEMETRY */ struct Imu imu; -struct ImuFloat imuf; void imu_init(void) { @@ -149,14 +132,10 @@ void imu_init(void) struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); -#if USE_IMU_FLOAT - orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); -#endif #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); -#if !USE_IMU_FLOAT register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); @@ -166,7 +145,6 @@ void imu_init(void) register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); -#endif // !USE_IMU_FLOAT #endif // DOWNLINK imu_impl_init(); @@ -179,9 +157,7 @@ void imu_SetBodyToImuPhi(float phi) memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.phi = phi; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); -#if USE_IMU_FLOAT - orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); -#endif + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); } void imu_SetBodyToImuTheta(float theta) @@ -190,9 +166,7 @@ void imu_SetBodyToImuTheta(float theta) memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.theta = theta; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); -#if USE_IMU_FLOAT - orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); -#endif + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); } void imu_SetBodyToImuPsi(float psi) @@ -201,9 +175,7 @@ void imu_SetBodyToImuPsi(float psi) memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.psi = psi; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); -#if USE_IMU_FLOAT - orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); -#endif + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); } void imu_SetBodyToImuCurrent(float set) @@ -219,9 +191,7 @@ void imu_SetBodyToImuCurrent(float set) body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi; body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); -#if USE_IMU_FLOAT - orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); -#endif + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); } else { // indicate that we couldn't set to current roll/pitch imu.b2i_set_current = FALSE; @@ -231,9 +201,7 @@ void imu_SetBodyToImuCurrent(float set) struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); -#if USE_IMU_FLOAT - orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); -#endif + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); } } diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h index 1604f42ad1..fcea81e218 100644 --- a/sw/airborne/subsystems/imu.h +++ b/sw/airborne/subsystems/imu.h @@ -58,21 +58,8 @@ struct Imu { bool_t b2i_set_current; }; -/** abstract IMU interface providing floating point interface */ -struct ImuFloat { - struct FloatRates gyro; - struct FloatVect3 accel; - struct FloatVect3 mag; - struct FloatRates gyro_prev; - struct OrientationReps body_to_imu; ///< rotation from body to imu frame - uint32_t sample_count; -}; - - - /** global IMU state */ extern struct Imu imu; -extern struct ImuFloat imuf; /* underlying hardware */ #ifdef IMU_TYPE_H diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index e8e8d23023..b081d7f7ca 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -113,10 +113,10 @@ void ins_propagate(float __attribute__((unused)) dt) /* untilt accels and speeds */ float_rmat_transp_vmult((struct FloatVect3 *)&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), - (struct FloatVect3 *)&ahrs_impl.accel); + (struct FloatVect3 *)&ahrs_ardrone2.accel); float_rmat_transp_vmult((struct FloatVect3 *)&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), - (struct FloatVect3 *)&ahrs_impl.speed); + (struct FloatVect3 *)&ahrs_ardrone2.speed); //Add g to the accelerations ins_impl.ltp_accel.z += 9.81; @@ -128,7 +128,7 @@ void ins_propagate(float __attribute__((unused)) dt) //Don't set the height if we use the one from the gps #if !USE_GPS_HEIGHT //Set the height and save the position - ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; + ins_impl.ltp_pos.z = -(ahrs_ardrone2.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; stateSetPositionNed_i(&ins_impl.ltp_pos); #endif } diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 4955f35404..63b9f515e0 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -34,7 +34,6 @@ #include "subsystems/ahrs/ahrs_int_utils.h" #include "subsystems/ahrs/ahrs_aligner.h" -#include "subsystems/ahrs.h" #include "subsystems/ins.h" #include "subsystems/gps.h" @@ -182,6 +181,21 @@ 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); + + /* gps */ bool_t ins_gps_fix_once; @@ -250,9 +264,6 @@ void ins_init() B.y = INS_H_Y; B.z = INS_H_Z; - // Bind to BARO_ABS message - AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); - // init state and measurements init_invariant_state(); @@ -272,8 +283,14 @@ void ins_init() ins_impl.gains.sh = INS_INV_SH; ins.status = INS_UNINIT; + ins_impl.is_aligned = FALSE; ins_impl.reset = FALSE; + // Bind to ABI messages + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); + AbiBindMsgIMU_MAG_INT32(INS_MAG_ID, &mag_ev, mag_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 @@ -326,29 +343,25 @@ void ins_reset_altitude_ref(void) #endif } -void ahrs_init(void) -{ - ahrs.status = AHRS_UNINIT; -} - -void ahrs_align(void) +void ins_float_invariant_align(struct Int32Rates *lp_gyro, + struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) { /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, lp_accel, lp_mag); /* use average gyro as initial value for bias */ struct FloatRates bias0; - RATES_COPY(bias0, ahrs_aligner.lp_gyro); + RATES_COPY(bias0, *lp_gyro); RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0); // ins and ahrs are now running - ahrs.status = AHRS_RUNNING; + ins_impl.is_aligned = TRUE; ins.status = INS_RUNNING; } -void ahrs_propagate(float dt) +void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* accel, float dt) { - struct FloatVect3 accel; struct FloatRates body_rates; // realign all the filter if needed @@ -356,17 +369,17 @@ void ahrs_propagate(float dt) if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; - ahrs.status = AHRS_UNINIT; + ins_impl.is_aligned = FALSE; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro); + int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; - int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); + int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains @@ -392,10 +405,11 @@ void ahrs_propagate(float dt) // untilt accel and remove gravity struct FloatQuat q_b2n; float_quat_invert(&q_b2n, &ins_impl.state.quat); - float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel); - VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); - VECT3_ADD(accel, A); - stateSetAccelNed_f((struct NedCoor_f *)&accel); + struct FloatVect3 accel_n; + float_quat_vmult(&accel_n, &q_b2n, &ins_impl.cmd.accel); + VECT3_SMUL(accel_n, accel_n, 1. / (ins_impl.state.as)); + VECT3_ADD(accel_n, A); + stateSetAccelNed_f((struct NedCoor_f *)&accel_n); //------------------------------------------------------------// @@ -470,7 +484,7 @@ void ahrs_propagate(float dt) #endif } -void ahrs_update_gps(void) +void ins_update_gps(void) { if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) { @@ -534,14 +548,10 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) } } -void ahrs_update_accel(float dt __attribute__((unused))) -{ -} - // assume mag is dead when values are not moving anymore #define MAG_FROZEN_COUNT 30 -void ahrs_update_mag(float dt __attribute__((unused))) +void ins_float_invariant_update_mag(struct Int32Vect3* mag) { static uint32_t mag_frozen_count = MAG_FROZEN_COUNT; static int32_t last_mx = 0; @@ -558,7 +568,7 @@ void ahrs_update_mag(float dt __attribute__((unused))) struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); struct Int32Vect3 mag_meas_body; // new values in body frame - int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, &imu.mag); + int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, mag); MAGS_FLOAT_OF_BFP(ins_impl.meas.mag, mag_meas_body); // reset counter mag_frozen_count = MAG_FROZEN_COUNT; @@ -738,3 +748,28 @@ 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); } + +/** temporary functions for INS interface compatibility */ +void ins_propagate(float dt) +{ + ins_float_invariant_propagate(&imu.gyro, &imu.accel, dt); +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *mag) +{ + if (ins_impl.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_impl.is_aligned) { + ins_float_invariant_align(lp_gyro, lp_accel, lp_mag); + } +} diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.h b/sw/airborne/subsystems/ins/ins_float_invariant.h index 1411a1486d..d83dc4f29f 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.h +++ b/sw/airborne/subsystems/ins/ins_float_invariant.h @@ -27,8 +27,9 @@ #ifndef INS_FLOAT_INVARIANT_H #define INS_FLOAT_INVARIANT_H -#include "subsystems/ahrs.h" #include "subsystems/ins.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" /** Invariant filter state dimension */ @@ -111,11 +112,26 @@ struct InsFloatInv { bool_t reset; ///< flag to request reset/reinit the filter struct FloatVect3 mag_h; + bool_t is_aligned; }; extern struct InsFloatInv ins_impl; -#define ahrs_impl ins_impl +#define DefaultAhrsImpl ins_impl + +/** dummy for now, will be removed when not using ahrs interface anymore */ +static inline void ins_impl_register(void) {} + +/** Currently still called from ins_propagate (declared in INS interface). */ +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); #endif /* INS_FLOAT_INVARIANT_H */ diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index 33b5ef2408..1abac4cb0a 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -21,6 +21,16 @@ #include +/* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h + * in order to implement telemetry_mode_Main_* + */ +#define PERIODIC_C_MAIN + +#include "generated/periodic_telemetry.h" + +#define ABI_C +#include "subsystems/abi.h" + #include "std.h" #include "mcu.h" #include "mcu_periph/sys_time.h" @@ -34,7 +44,6 @@ #include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_aligner.h" - static inline void main_init(void); static inline void main_periodic_task(void); static inline void main_event_task(void); @@ -44,6 +53,10 @@ static inline void on_gyro_event(void); static inline void on_accel_event(void); static inline void on_mag_event(void); +#define __DefaultAhrsRegister(_x) _x ## _register() +#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x) +#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl) + int main(void) { main_init(); @@ -61,10 +74,14 @@ static inline void main_init(void) mcu_init(); sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); imu_init(); +#if USE_AHRS_ALIGNER ahrs_aligner_init(); +#endif ahrs_init(); downlink_init(); + DefaultAhrsRegister(); + mcu_int_enable(); } @@ -74,8 +91,7 @@ static inline void main_periodic_task(void) imu_periodic(); } RunOnceEvery(10, { LED_PERIODIC();}); - - RunOnceEvery(20, main_report()); + main_report(); } static inline void main_event_task(void) @@ -90,155 +106,45 @@ static inline void main_event_task(void) static inline void on_gyro_event(void) { - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; // current timestamp uint32_t now_ts = get_sys_time_usec(); - // dt between this and last callback in seconds - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; imu_scale_gyro(&imu); - if (ahrs.status == AHRS_UNINIT) { + + AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev); + +#if USE_AHRS_ALIGNER + if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) { ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { - ahrs_align(); - } - } else { - ahrs_propagate(dt); + return; } +#endif } static inline void on_accel_event(void) { - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; // current timestamp uint32_t now_ts = get_sys_time_usec(); - // dt between this and last callback in seconds - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; imu_scale_accel(&imu); - if (ahrs.status != AHRS_UNINIT) { - ahrs_update_accel(dt); - } + + AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel); } static inline void on_mag_event(void) { - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; // current timestamp uint32_t now_ts = get_sys_time_usec(); - // dt between this and last callback in seconds - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; imu_scale_mag(&imu); - if (ahrs.status == AHRS_RUNNING) { - ahrs_update_mag(dt); - } + + AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag); } static inline void main_report(void) { + RunOnceEvery(512, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM)); - PeriodicPrescaleBy10({ - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); - }, { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); - }, { - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); - }, { - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, - &imu.accel.x, - &imu.accel.y, - &imu.accel.z); - }, { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r); - }, - - { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, - &imu.mag.x, - &imu.mag.y, - &imu.mag.z); - }, - - { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - }, { -#if USE_I2C2 - uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt; - uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; - uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; - uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt; - uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; - uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; - uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; - uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; - uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; - uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; - const uint8_t _bus2 = 2; - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, - &i2c2_queue_full_cnt, - &i2c2_ack_fail_cnt, - &i2c2_miss_start_stop_cnt, - &i2c2_arb_lost_cnt, - &i2c2_over_under_cnt, - &i2c2_pec_recep_cnt, - &i2c2_timeout_tlow_cnt, - &i2c2_smbus_alert_cnt, - &i2c2_unexpected_event_cnt, - &i2c2_last_unexpected_event, - &_bus2); -#endif - }, { -#ifdef AHRS_FLOAT - struct FloatEulers ltp_to_imu_euler; - float_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); - struct Int32Eulers euler_i; - EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); - struct Int32Eulers *eulers_body = stateGetNedToBodyEulers_i(); - DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, - &euler_i.phi, - &euler_i.theta, - &euler_i.psi, - &(eulers_body->phi), - &(eulers_body->theta), - &(eulers_body->psi)); -#else - struct Int32Eulers ltp_to_imu_euler; - int32_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); - struct Int32Eulers *eulers = stateGetNedToBodyEulers_i(); - DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, - <p_to_imu_euler.phi, - <p_to_imu_euler.theta, - <p_to_imu_euler.psi, - &(eulers->phi), - &(eulers->theta), - &(eulers->psi)); -#endif - }, { -#ifndef AHRS_FLOAT - DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, - &ahrs_impl.gyro_bias.p, - &ahrs_impl.gyro_bias.q, - &ahrs_impl.gyro_bias.r); -#endif - }); + periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device); }