diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index b798c6025bc..bd1555e4fb4 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -7,7 +7,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_att_float_euler.xml" - settings_modules="" + settings_modules="modules/gps.xml" gui_color="#ffff954c0000" /> diff --git a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile deleted file mode 100644 index fb752015647..00000000000 --- a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile +++ /dev/null @@ -1,9 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_alt_float.h\" -ap_srcs += $(SRC_FIXEDWING)/subsystems/ins.c -ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_alt_float.c - -nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_alt_float.h\" -nps.srcs += $(SRC_FIXEDWING)/subsystems/ins.c -nps.srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_alt_float.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile b/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile deleted file mode 100644 index 8ed97ba1370..00000000000 --- a/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile +++ /dev/null @@ -1,40 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -# attitude and speed estimation for fixedwings via invariant filter - -INS_CFLAGS += -DUSE_AHRS_ALIGNER -INS_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\" -INS_CFLAGS += -DINS_FINV_USE_UTM - -INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c -INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c -INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c -INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant_wrapper.c - - -ifneq ($(AHRS_ALIGNER_LED),none) - INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -endif - -ap.CFLAGS += $(INS_CFLAGS) -ap.srcs += $(INS_SRCS) - -# -# NPS uses the real algorithm -# -nps.CFLAGS += $(INS_CFLAGS) -nps.srcs += $(INS_SRCS) - - -# -# Simple simulation of the AHRS result -# -ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" -ahrssim_CFLAGS += -DUSE_AHRS - -ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c -ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c - -sim.CFLAGS += $(ahrssim_CFLAGS) -sim.srcs += $(ahrssim_srcs) - diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile deleted file mode 100644 index 2bea50bf8b2..00000000000 --- a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile +++ /dev/null @@ -1,16 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -ins_CFLAGS = -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\" -ins_srcs += $(SRC_SUBSYSTEMS)/ins.c -ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c - - -ap.CFLAGS += $(ins_CFLAGS) -ap.srcs += $(ins_srcs) - -sim.CFLAGS += $(ins_CFLAGS) -sim.srcs += $(ins_srcs) - -nps.CFLAGS += $(ins_CFLAGS) -nps.srcs += $(ins_srcs) - diff --git a/conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile b/conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile deleted file mode 100644 index 45591ff4d03..00000000000 --- a/conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile +++ /dev/null @@ -1,43 +0,0 @@ -# Vectornav INS Driver -# 2015, Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu -# Utah State University, http://aggieair.usu.edu/ - -include $(CFG_SHARED)/ins_vectornav.makefile - - -######################################### -## Simulator - -sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" -sim.CFLAGS += -DUSE_AHRS - -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c - -sim.srcs += $(SRC_SUBSYSTEMS)/ins.c -sim.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c - -sim.CFLAGS += -DUSE_GPS -sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c -sim.srcs += $(SRC_SUBSYSTEMS)/gps.c - - -# -# NPS simulator -# -nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" -DUSE_IMU -nps.srcs += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c - -nps.CFLAGS += -DUSE_GPS -nps.srcs += $(SRC_SUBSYSTEMS)/gps.c -nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" -nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c - -nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\" -nps.srcs += $(SRC_SUBSYSTEMS)/ins.c -nps.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c - -nps.CFLAGS += -DNPS_BYPASS_AHRS -DNPS_BYPASS_INS - diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile deleted file mode 100644 index 038ff5b5484..00000000000 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The ins xsens subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile deleted file mode 100644 index e7c485cbe45..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ins.makefile +++ /dev/null @@ -1,11 +0,0 @@ -# -# simple INS with float vertical filter -# - -$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c - -# vertical filter float version -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c - diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile deleted file mode 100644 index 3bb4b4d2491..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile +++ /dev/null @@ -1,11 +0,0 @@ -# -# extended INS with vertical filter using sonar in a better way (flap ground) -# - -$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c - -# vertical filter float version -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c -$(TARGET).CFLAGS += -DUSE_VFF_EXTENDED diff --git a/conf/firmwares/subsystems/rotorcraft/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/rotorcraft/ins_gps_passthrough.makefile deleted file mode 100644 index 610b394e451..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ins_gps_passthrough.makefile +++ /dev/null @@ -1,12 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -ins_CFLAGS = -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough.h\" -ins_srcs += $(SRC_SUBSYSTEMS)/ins.c -ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c - - -ap.CFLAGS += $(ins_CFLAGS) -ap.srcs += $(ins_srcs) - -nps.CFLAGS += $(ins_CFLAGS) -nps.srcs += $(ins_srcs) diff --git a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile deleted file mode 100644 index f14ac113503..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile +++ /dev/null @@ -1,14 +0,0 @@ -# -# simple with float vertical and horizontal filters for INS -# - -$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c - -# vertical filter float version -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c - -# horizontal filter float version -$(TARGET).CFLAGS += -DUSE_HFF -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/hf_float.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins_vectornav.makefile b/conf/firmwares/subsystems/rotorcraft/ins_vectornav.makefile deleted file mode 100644 index a27d5bfe5a4..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ins_vectornav.makefile +++ /dev/null @@ -1,25 +0,0 @@ -# Vectornav INS Driver -# 2015, Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu -# Utah State University, http://aggieair.usu.edu/ - -include $(CFG_SHARED)/ins_vectornav.makefile - - - -# -# NPS simulator -# -nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" -DUSE_IMU -nps.srcs += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c - -nps.CFLAGS += -DUSE_GPS -nps.srcs += $(SRC_SUBSYSTEMS)/gps.c -nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" -nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c - -nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough.h\" -nps.srcs += $(SRC_SUBSYSTEMS)/ins.c -nps.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c - -nps.CFLAGS += -DNPS_BYPASS_AHRS -DNPS_BYPASS_INS - diff --git a/conf/firmwares/subsystems/shared/ins_float_invariant.makefile b/conf/firmwares/subsystems/shared/ins_float_invariant.makefile deleted file mode 100644 index f4275dd6a03..00000000000 --- a/conf/firmwares/subsystems/shared/ins_float_invariant.makefile +++ /dev/null @@ -1,30 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -# attitude and speed estimation via invariant filter - -USE_MAGNETOMETER ?= 1 - -INS_CFLAGS += -DUSE_AHRS_ALIGNER -INS_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\" - -ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) - INS_CFLAGS += -DUSE_MAGNETOMETER -endif - -INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c -INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c -INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c -INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant_wrapper.c - -ifneq ($(AHRS_ALIGNER_LED),none) - INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -endif - -ap.CFLAGS += $(INS_CFLAGS) -ap.srcs += $(INS_SRCS) - -# -# NPS uses the real algorithm -# -nps.CFLAGS += $(INS_CFLAGS) -nps.srcs += $(INS_SRCS) diff --git a/conf/firmwares/subsystems/shared/ins_vectornav.makefile b/conf/firmwares/subsystems/shared/ins_vectornav.makefile deleted file mode 100644 index 670be44ae08..00000000000 --- a/conf/firmwares/subsystems/shared/ins_vectornav.makefile +++ /dev/null @@ -1,55 +0,0 @@ -# Vectornav INS Driver -# 2015, Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu -# Utah State University, http://aggieair.usu.edu/ - -### AP & NPS Targets - -# -# INS defines -# -VN_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_vectornav_wrapper.h\" - -VN_SRCS += $(SRC_SUBSYSTEMS)/ins.c -VN_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_vectornav.c -VN_SRCS += subsystems/ins/ins_vectornav_wrapper.c - -# -# GPS defines -# -VN_CFLAGS += -DUSE_GPS -VN_SRCS += $(SRC_SUBSYSTEMS)/gps.c -ifdef SECONDARY_GPS -ifneq (,$(findstring $(SECONDARY_GPS), vectornav)) -# this is the secondary GPS -VN_CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/ins_vectornav_wrapper.h\" -VN_CFLAGS += -DSECONDARY_GPS=gps_vectornav -else -VN_CFLAGS += -DGPS_TYPE_H=\"subsystems/ins/ins_vectornav_wrapper.h\" -VN_CFLAGS += -DPRIMARY_GPS=gps_vectornav -endif -else -# plain old single GPS usage -VN_CFLAGS += -DGPS_TYPE_H=\"subsystems/ins/ins_vectornav_wrapper.h\" -endif - - -# -# IO defines -# -VN_PORT ?= UART3 -VN_BAUD ?= B921600 - -VN_CFLAGS += -DUSE_$(VN_PORT) -D$(VN_PORT)_BAUD=$(VN_BAUD) -VN_PORT_LOWER=$(shell echo $(VN_PORT) | tr A-Z a-z) -VN_CFLAGS += -DVN_PORT=$(VN_PORT_LOWER) - -VN_SRCS += peripherals/vn200_serial.c - -# -# Add to the target (AP+NPS)) -# add it for all targets except sim and fbw -# -ifeq (,$(findstring $(TARGET),sim fbw nps)) -$(TARGET).CFLAGS += $(VN_CFLAGS) -$(TARGET).srcs += $(VN_SRCS) -endif diff --git a/conf/firmwares/subsystems/shared/ins_xsens700.makefile b/conf/firmwares/subsystems/shared/ins_xsens700.makefile deleted file mode 100644 index bf4eb71b378..00000000000 --- a/conf/firmwares/subsystems/shared/ins_xsens700.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The ins xsens700 subsystem has been converted to a module, replace by ) diff --git a/conf/modules/ins.xml b/conf/modules/ins.xml new file mode 100644 index 00000000000..f2a2e462570 --- /dev/null +++ b/conf/modules/ins.xml @@ -0,0 +1,24 @@ + + + + + + simple INS with vertical filter. + + + + + + + +
+ +
+ + + + + + + + diff --git a/conf/modules/ins_alt_float.xml b/conf/modules/ins_alt_float.xml new file mode 100644 index 00000000000..f4ad65942b0 --- /dev/null +++ b/conf/modules/ins_alt_float.xml @@ -0,0 +1,24 @@ + + + + + + INS with Kalman Filter on altitude. + For fixedwings. + + + + + + + +
+ +
+ + + + + + + diff --git a/conf/modules/ins_extended.xml b/conf/modules/ins_extended.xml new file mode 100644 index 00000000000..24df9dbb99e --- /dev/null +++ b/conf/modules/ins_extended.xml @@ -0,0 +1,30 @@ + + + + + + extended INS with vertical filter using sonar. + + + + + + + + + + + + +
+ +
+ + + + + + + + + diff --git a/conf/modules/ins_float_invariant.xml b/conf/modules/ins_float_invariant.xml new file mode 100644 index 00000000000..dd7a80459d0 --- /dev/null +++ b/conf/modules/ins_float_invariant.xml @@ -0,0 +1,39 @@ + + + + + + Invariant INS (in float). + Estimates attitude, velocity, position and (gyro, accel, baro) biases. + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/ins_gps_passthrough.xml b/conf/modules/ins_gps_passthrough.xml new file mode 100644 index 00000000000..120c62bcfc7 --- /dev/null +++ b/conf/modules/ins_gps_passthrough.xml @@ -0,0 +1,24 @@ + + + + + + INS which just passes GPS through. + + + +
+ +
+ + + + + + + + + + + + diff --git a/conf/modules/ins_hff.xml b/conf/modules/ins_hff.xml new file mode 100644 index 00000000000..512457268a6 --- /dev/null +++ b/conf/modules/ins_hff.xml @@ -0,0 +1,26 @@ + + + + + + INS with float vertical and horizontal filters. + + + + + + + +
+ +
+ + + + + + + + + + diff --git a/conf/modules/ins_nps.xml b/conf/modules/ins_nps.xml new file mode 100644 index 00000000000..0168f422ea4 --- /dev/null +++ b/conf/modules/ins_nps.xml @@ -0,0 +1,27 @@ + + + + + + Simulated AHRS and INS. + + + +
+ +
+ + + + + + + + + + + + + + + diff --git a/conf/modules/ins_sim.xml b/conf/modules/ins_sim.xml new file mode 100644 index 00000000000..0656cc7da00 --- /dev/null +++ b/conf/modules/ins_sim.xml @@ -0,0 +1,24 @@ + + + + + + Simulated AHRS and INS. + + + +
+ +
+ + + + + + + + + + + + diff --git a/conf/modules/ins_skeleton.xml b/conf/modules/ins_skeleton.xml index be3c87787a6..30d679293f4 100644 --- a/conf/modules/ins_skeleton.xml +++ b/conf/modules/ins_skeleton.xml @@ -18,14 +18,12 @@ +
- - - ap.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_skeleton.h\" - nps.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_skeleton.h\" - + + diff --git a/conf/modules/ins_vectornav.xml b/conf/modules/ins_vectornav.xml new file mode 100644 index 00000000000..6fadc8f9480 --- /dev/null +++ b/conf/modules/ins_vectornav.xml @@ -0,0 +1,53 @@ + + + + + + Vectornav INS Driver. + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + ifdef SECONDARY_GPS + ifneq (,$(findstring $(SECONDARY_GPS), vectornav)) + # this is the secondary GPS + ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/ins/ins_vectornav_wrapper.h\" + ap.CFLAGS += -DSECONDARY_GPS=GPS_VECTORNAV + else + ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/ins/ins_vectornav_wrapper.h\" + ap.CFLAGS += -DPRIMARY_GPS=GPS_VECTORNAV + endif + else + # plain old single GPS usage + ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" + endif + + + diff --git a/conf/modules/ins_xsens.xml b/conf/modules/ins_xsens.xml index 02e797128ef..82639f2b573 100644 --- a/conf/modules/ins_xsens.xml +++ b/conf/modules/ins_xsens.xml @@ -8,11 +8,16 @@ + + + + +
- - + + @@ -35,37 +40,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/modules/ins_xsens700.xml b/conf/modules/ins_xsens700.xml index a63a328bbb0..6b22027d9ba 100644 --- a/conf/modules/ins_xsens700.xml +++ b/conf/modules/ins_xsens700.xml @@ -8,11 +8,16 @@ + + + + +
- - + + @@ -34,37 +39,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index ae8c6476e8a..026cac4990c 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -57,7 +57,6 @@ #include "subsystems/sensors/baro.h" PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD) #endif -#include "subsystems/ins.h" // autopilot & control @@ -205,8 +204,6 @@ void init_ap(void) ahrs_init(); #endif - ins_init(); - #if USE_BARO_BOARD baro_init(); #endif @@ -625,14 +622,8 @@ void sensors_task(void) #if USE_AHRS && defined SITL && !USE_NPS update_ahrs_from_sim(); #endif - - //FIXME: temporary hack, remove me -#ifdef InsPeriodic - InsPeriodic(); -#endif } - #ifdef LOW_BATTERY_KILL_DELAY #warning LOW_BATTERY_KILL_DELAY has been renamed to CATASTROPHIC_BAT_KILL_DELAY, please update your airframe file! #endif @@ -698,11 +689,6 @@ void event_task_ap(void) ImuEvent(); #endif -#ifdef InsEvent - TODO("calling InsEvent, remove me..") - InsEvent(); -#endif - #if USE_BARO_BOARD BaroEvent(); #endif diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index ed7a6ba9e13..c982de32576 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -71,7 +71,6 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO #if USE_AHRS_ALIGNER #include "subsystems/ahrs/ahrs_aligner.h" #endif -#include "subsystems/ins.h" #include "state.h" @@ -196,8 +195,6 @@ STATIC_INLINE void main_init(void) ahrs_init(); #endif - ins_init(); - autopilot_init(); modules_init(); @@ -268,11 +265,6 @@ STATIC_INLINE void main_periodic(void) imu_periodic(); #endif - //FIXME: temporary hack, remove me -#ifdef InsPeriodic - InsPeriodic(); -#endif - /* run control loops */ autopilot_periodic(); /* set actuators */ @@ -369,11 +361,6 @@ STATIC_INLINE void main_event(void) ImuEvent(); #endif -#ifdef InsEvent - TODO("calling InsEvent, remove me..") - InsEvent(); -#endif - #if USE_BARO_BOARD BaroEvent(); #endif diff --git a/sw/airborne/modules/ins/ins_skeleton.c b/sw/airborne/modules/ins/ins_skeleton.c index 2460e2080b9..37d14440bfd 100644 --- a/sw/airborne/modules/ins/ins_skeleton.c +++ b/sw/airborne/modules/ins/ins_skeleton.c @@ -163,6 +163,12 @@ void ins_module_wrapper_init(void) INT32_VECT3_ZERO(ins_module.ltp_accel); ins_module_init(); + + // Bind to ABI messages + AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb); + AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb); + AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb); } /** copy position and speed to state interface */ @@ -235,17 +241,3 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), { orientationSetQuat_f(&ins_module.body_to_imu, q_b2i_f); } - -void ins_module_register(void) -{ - ins_register_impl(ins_module_wrapper_init); - - // Bind to ABI messages - AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb); - AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb); - AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb); - -#if PERIODIC_TELEMETRY -#endif -} diff --git a/sw/airborne/modules/ins/ins_skeleton.h b/sw/airborne/modules/ins/ins_skeleton.h index 26c47ee657b..a59606a45c8 100644 --- a/sw/airborne/modules/ins/ins_skeleton.h +++ b/sw/airborne/modules/ins/ins_skeleton.h @@ -56,11 +56,7 @@ struct InsModuleInt { /** global INS state */ extern struct InsModuleInt ins_module; -#ifndef DefaultInsImpl -#define DefaultInsImpl ins_module -#endif - -extern void ins_module_register(void); +extern void ins_module_wrapper_init(void); /* these functions can/should be implemented in your module */ extern void ins_module_init(void); diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index cd4fdb11087..6baede2328a 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -61,12 +61,6 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s); -void ins_xsens_register(void) -{ - ins_register_impl(ins_xsens_init); - AbiBindMsgGPS(INS_XSENS_GPS_ID, &gps_ev, gps_cb); -} - void ins_xsens_init(void) { xsens_init(); @@ -77,6 +71,8 @@ void ins_xsens_init(void) struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); + + AbiBindMsgGPS(INS_XSENS_GPS_ID, &gps_ev, gps_cb); } void ins_xsens_event(void) diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index b590034d174..a7f56fbc603 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -30,8 +30,6 @@ #include "std.h" -// hack to not use this in sim/nps -#ifndef SITL #include "xsens.h" #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP @@ -41,10 +39,7 @@ extern volatile uint8_t new_ins_attitude; extern float ins_pitch_neutral; extern float ins_roll_neutral; -#define DefaultInsImpl ins_xsens - extern void ins_xsens_init(void); -extern void ins_xsens_register(void); extern void ins_xsens_event(void); #if USE_GPS_XSENS @@ -54,11 +49,4 @@ extern void ins_xsens_event(void); extern void gps_xsens_init(void); #endif -#else // SITL - -static inline void xsens_periodic(void) {} -static inline void ins_xsens_event(void) {} - -#endif - #endif diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index b8109e01c15..38eaeac523a 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -64,12 +64,6 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s); -void ins_xsens700_register(void) -{ - ins_register_impl(ins_xsens700_init); - AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb); -} - void ins_xsens700_init(void) { xsens700_init(); @@ -80,6 +74,8 @@ void ins_xsens700_init(void) struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); + + AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb); } void ins_xsens700_event(void) diff --git a/sw/airborne/modules/ins/ins_xsens700.h b/sw/airborne/modules/ins/ins_xsens700.h index 7ac88a45d6b..fd25e1f9d03 100644 --- a/sw/airborne/modules/ins/ins_xsens700.h +++ b/sw/airborne/modules/ins/ins_xsens700.h @@ -30,8 +30,6 @@ #include "std.h" -// hack to not use this in sim/nps -#ifndef SITL #include "xsens700.h" #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP @@ -41,10 +39,7 @@ extern volatile uint8_t new_ins_attitude; extern float ins_pitch_neutral; extern float ins_roll_neutral; -#define DefaultInsImpl ins_xsens700 - extern void ins_xsens700_init(void); -extern void ins_xsens700_register(void); extern void ins_xsens700_event(void); #if USE_GPS_XSENS @@ -54,11 +49,4 @@ extern void ins_xsens700_event(void); extern void gps_xsens700_init(void); #endif -#else // SITL - -static inline void xsens700_periodic(void) {} -static inline void ins_xsens700_event(void) {} - -#endif - #endif diff --git a/sw/airborne/modules/ins/xsens.c b/sw/airborne/modules/ins/xsens.c index 170b11fa950..09a2b65ed9e 100644 --- a/sw/airborne/modules/ins/xsens.c +++ b/sw/airborne/modules/ins/xsens.c @@ -27,6 +27,7 @@ #include "xsens_common.h" #include "generated/airframe.h" +#include "led.h" #if USE_GPS_XSENS #include "math/pprz_geodetic_wgs84.h" diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 279032c29af..b709b3d3ae0 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -35,38 +35,6 @@ #include "generated/flight_plan.h" -#ifndef DefaultInsImpl -#warning "DefaultInsImpl not set!" -#else -PRINT_CONFIG_VAR(DefaultInsImpl) -#endif - -#define __DefaultInsRegister(_x) _x ## _register() -#define _DefaultInsRegister(_x) __DefaultInsRegister(_x) -#define DefaultInsRegister() _DefaultInsRegister(DefaultInsImpl) - -/** Inertial Navigation System state */ -struct Ins { - InsInit init; -}; - -struct Ins ins; - -void ins_register_impl(InsInit init) -{ - ins.init = init; - - ins.init(); -} - -void ins_init(void) -{ - ins.init = NULL; - -#ifdef DefaultInsImpl - DefaultInsRegister(); -#endif -} void ins_init_origin_i_from_flightplan(struct LtpDef_i *ltp_def) { diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index ea718cac58e..0ea3a4c7961 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -37,14 +37,6 @@ #include INS_TYPE_H #endif -typedef void (*InsInit)(void); - -extern void ins_register_impl(InsInit init); - -/** INS initialization. Called at startup. - * Initializes the global ins struct. - */ -extern void ins_init(void); /** INS local origin reset. * Reset horizontal and vertical reference to the current position. diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 41df150cbeb..280135d9d81 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -45,10 +45,6 @@ #include "subsystems/datalink/downlink.h" #endif -#if defined ALT_KALMAN || defined ALT_KALMAN_ENABLED -#warning Please remove the obsolete ALT_KALMAN and ALT_KALMAN_ENABLED defines from your airframe file. -#endif - #ifndef USE_INS_NAV_INIT #define USE_INS_NAV_INIT TRUE PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") @@ -63,14 +59,14 @@ struct InsAltFloat ins_altf; PRINT_CONFIG_MSG("USE_BAROMETER is TRUE: Using baro for altitude estimation.") // Baro event on ABI -#ifndef INS_BARO_ID +#ifndef INS_ALT_BARO_ID #if USE_BARO_BOARD -#define INS_BARO_ID BARO_BOARD_SENDER_ID +#define INS_ALT_BARO_ID BARO_BOARD_SENDER_ID #else -#define INS_BARO_ID ABI_BROADCAST +#define INS_ALT_BARO_ID ABI_BROADCAST #endif #endif -PRINT_CONFIG_VAR(INS_BARO_ID) +PRINT_CONFIG_VAR(INS_ALT_BARO_ID) abi_event baro_ev; static void baro_cb(uint8_t sender_id, float pressure); @@ -84,12 +80,17 @@ static void baro_cb(uint8_t sender_id, float pressure); #endif PRINT_CONFIG_VAR(INS_ALT_GPS_ID) static abi_event gps_ev; -static abi_event accel_ev; -static abi_event body_to_imu_ev; -static struct OrientationReps body_to_imu; +static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); + #ifndef INS_ALT_IMU_ID #define INS_ALT_IMU_ID ABI_BROADCAST #endif +static abi_event accel_ev; +static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel); + +static abi_event body_to_imu_ev; +static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f); +static struct OrientationReps body_to_imu; static void alt_kalman_reset(void); static void alt_kalman_init(void); @@ -124,6 +125,14 @@ void ins_alt_float_init(void) // why do we have this here? alt_kalman(0.0f, 0.1); + +#if USE_BAROMETER + // Bind to BARO_ABS message + AbiBindMsgBARO_ABS(INS_ALT_BARO_ID, &baro_ev, baro_cb); +#endif + AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb); + AbiBindMsgIMU_ACCEL_INT32(INS_ALT_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgBODY_TO_IMU_QUAT(INS_ALT_IMU_ID, &body_to_imu_ev, body_to_imu_cb); } /** Reset the geographic reference to the current GPS fix */ @@ -378,16 +387,3 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), { orientationSetQuat_f(&body_to_imu, q_b2i_f); } - -void ins_altf_register(void) -{ - ins_register_impl(ins_alt_float_init); - -#if USE_BAROMETER - // Bind to BARO_ABS message - AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); -#endif - AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb); - AbiBindMsgIMU_ACCEL_INT32(INS_ALT_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgBODY_TO_IMU_QUAT(INS_ALT_IMU_ID, &body_to_imu_ev, body_to_imu_cb); -} diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index fba52d955d1..90e19874cb0 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -53,9 +53,4 @@ extern struct InsAltFloat ins_altf; extern void ins_alt_float_init(void); extern void ins_alt_float_update_baro(float pressure); -#ifndef DefaultInsImpl -#define DefaultInsImpl ins_altf -#endif -extern void ins_altf_register(void); - #endif /* INS_ALT_FLOAT_H */ diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c index e5764a2a2cc..7cb513385dc 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c @@ -201,9 +201,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), } -void ins_float_invariant_register(void) +void ins_float_invariant_wrapper_init(void) { - ins_register_impl(ins_float_invariant_init); + ins_float_invariant_init(); // Bind to ABI messages AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb); diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h index 9c4cdabf9d5..44f59e5e6e2 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h +++ b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h @@ -29,8 +29,6 @@ #include "subsystems/ins/ins_float_invariant.h" -#define DefaultInsImpl ins_float_invariant - -extern void ins_float_invariant_register(void); +extern void ins_float_invariant_wrapper_init(void); #endif /* INS_FLOAT_INVARIANT_WRAPPER_H */ diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 8a24c76ebf4..fb6ca83c6df 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -26,6 +26,7 @@ * and passes it through to the state interface. */ +#include "subsystems/ins/ins_gps_passthrough.h" #include "subsystems/ins.h" #include @@ -33,6 +34,7 @@ #include "state.h" #include "subsystems/gps.h" +#include "subsystems/abi.h" #ifndef USE_INS_NAV_INIT #define USE_INS_NAV_INIT TRUE @@ -43,7 +45,6 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #include "generated/flight_plan.h" #endif -#include "subsystems/ins/ins_gps_passthrough.h" struct InsGpsPassthrough { struct LtpDef_i ltp_def; @@ -57,6 +58,42 @@ struct InsGpsPassthrough { struct InsGpsPassthrough ins_gp; + +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef INS_PT_GPS_ID +#define INS_PT_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_PT_GPS_ID) +static abi_event gps_ev; + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + if (gps_s->fix < GPS_FIX_3D) { + return; + } + if (!ins_gp.ltp_initialized) { + ins_reset_local_origin(); + } + + /* simply scale and copy pos/speed from gps */ + struct NedCoor_i gps_pos_cm_ned; + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos); + INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned, + INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + stateSetPositionNed_i(&ins_gp.ltp_pos); + + struct NedCoor_i gps_speed_cm_s_ned; + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel); + INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned, + INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); + stateSetSpeedNed_i(&ins_gp.ltp_speed); +} + + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -119,6 +156,8 @@ void ins_gps_passthrough_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref); #endif + + AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb); } void ins_reset_local_origin(void) @@ -141,44 +180,3 @@ void ins_reset_altitude_ref(void) ins_gp.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_gp.ltp_def); } - - -#include "subsystems/abi.h" -/** ABI binding for gps data. - * Used for GPS ABI messages. - */ -#ifndef INS_PT_GPS_ID -#define INS_PT_GPS_ID GPS_MULTI_ID -#endif -PRINT_CONFIG_VAR(INS_PT_GPS_ID) -static abi_event gps_ev; -static void gps_cb(uint8_t sender_id __attribute__((unused)), - uint32_t stamp __attribute__((unused)), - struct GpsState *gps_s) -{ - if (gps_s->fix < GPS_FIX_3D) { - return; - } - if (!ins_gp.ltp_initialized) { - ins_reset_local_origin(); - } - - /* simply scale and copy pos/speed from gps */ - struct NedCoor_i gps_pos_cm_ned; - ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos); - INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned, - INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - stateSetPositionNed_i(&ins_gp.ltp_pos); - - struct NedCoor_i gps_speed_cm_s_ned; - ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel); - INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned, - INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); - stateSetSpeedNed_i(&ins_gp.ltp_speed); -} - -void ins_gps_passthrough_register(void) -{ - ins_register_impl(ins_gps_passthrough_init); - AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb); -} diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.h b/sw/airborne/subsystems/ins/ins_gps_passthrough.h index 96b06e0d600..3d436118fea 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.h +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.h @@ -21,16 +21,12 @@ /** * @file subsystems/ins/ins_gps_passthrough.h * - * Simply converts GPS ECEF position and velocity to NED - * and passes it through to the state interface. + * Simply passes GPS through to the state interface. */ #ifndef INS_GPS_PASSTHROUGH_H #define INS_GPS_PASSTHROUGH_H -#define DefaultInsImpl ins_gps_passthrough - extern void ins_gps_passthrough_init(void); -extern void ins_gps_passthrough_register(void); #endif /* INS_GPS_PASSTHROUGH_H */ diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index 572557012a0..111f17b5c65 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -26,6 +26,7 @@ * For fixedwing firmware since it sets UTM pos only. */ +#include "subsystems/ins/ins_gps_passthrough.h" #include "subsystems/ins.h" #include @@ -35,39 +36,15 @@ #include "subsystems/gps.h" #include "firmwares/fixedwing/nav.h" -#include "subsystems/ins/ins_gps_passthrough_utm.h" - -void ins_gps_utm_init(void) -{ - struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; - stateSetLocalUtmOrigin_f(&utm0); - stateSetPositionUtm_f(&utm0); -} - -void ins_reset_local_origin(void) -{ - struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); - - // reset state UTM ref - stateSetLocalUtmOrigin_f(&utm); -} - -void ins_reset_altitude_ref(void) -{ - struct UtmCoor_f utm = state.utm_origin_f; - utm.alt = gps.hmsl / 1000.0f; - stateSetLocalUtmOrigin_f(&utm); -} - #include "subsystems/abi.h" /** ABI binding for gps data. * Used for GPS ABI messages. */ -#ifndef INS_PTU_GPS_ID -#define INS_PTU_GPS_ID GPS_MULTI_ID +#ifndef INS_PT_GPS_ID +#define INS_PT_GPS_ID GPS_MULTI_ID #endif -PRINT_CONFIG_VAR(INS_PTU_GPS_ID) +PRINT_CONFIG_VAR(INS_PT_GPS_ID) static abi_event gps_ev; static void gps_cb(uint8_t sender_id __attribute__((unused)), @@ -88,8 +65,27 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), stateSetSpeedNed_f(&ned_vel); } -void ins_gps_utm_register(void) + +void ins_gps_passthrough_init(void) { - ins_register_impl(ins_gps_utm_init); - AbiBindMsgGPS(INS_PTU_GPS_ID, &gps_ev, gps_cb); + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + stateSetPositionUtm_f(&utm0); + + AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb); +} + +void ins_reset_local_origin(void) +{ + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); + + // reset state UTM ref + stateSetLocalUtmOrigin_f(&utm); +} + +void ins_reset_altitude_ref(void) +{ + struct UtmCoor_f utm = state.utm_origin_f; + utm.alt = gps.hmsl / 1000.0f; + stateSetLocalUtmOrigin_f(&utm); } diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.h b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.h deleted file mode 100644 index 98e2c78c717..00000000000 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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/ins/ins_gps_passthrough_utm.h - * - * Simply passes GPS UTM position and velocity through to the state interface. - * For fixedwing firmware since it sets UTM pos only. - */ - -#ifndef INS_GPS_PASSTHROUGH_UTM_H -#define INS_GPS_PASSTHROUGH_UTM_H - -#define DefaultInsImpl ins_gps_utm - -extern void ins_gps_utm_init(void); -extern void ins_gps_utm_register(void); - -#endif /* INS_GPS_PASSTHROUGH_UTM_H */ diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 13cc0135e05..584f9c2080c 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -61,8 +61,8 @@ #endif /** default sonar to use in INS */ -#ifndef INS_SONAR_ID -#define INS_SONAR_ID ABI_BROADCAST +#ifndef INS_INT_SONAR_ID +#define INS_INT_SONAR_ID ABI_BROADCAST #endif abi_event sonar_ev; static void sonar_cb(uint8_t sender_id, float distance); @@ -112,19 +112,15 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE") PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif -#ifdef INS_BARO_SENS -#warning INS_BARO_SENS is obsolete, please remove it from your airframe file. -#endif - /** default barometer to use in INS */ -#ifndef INS_BARO_ID +#ifndef INS_INT_BARO_ID #if USE_BARO_BOARD -#define INS_BARO_ID BARO_BOARD_SENDER_ID +#define INS_INT_BARO_ID BARO_BOARD_SENDER_ID #else -#define INS_BARO_ID ABI_BROADCAST +#define INS_INT_BARO_ID ABI_BROADCAST #endif #endif -PRINT_CONFIG_VAR(INS_BARO_ID) +PRINT_CONFIG_VAR(INS_INT_BARO_ID) abi_event baro_ev; static void baro_cb(uint8_t sender_id, float pressure); @@ -134,12 +130,14 @@ static void baro_cb(uint8_t sender_id, float pressure); #ifndef INS_INT_IMU_ID #define INS_INT_IMU_ID ABI_BROADCAST #endif +static abi_event accel_ev; +static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel); + #ifndef INS_INT_GPS_ID #define INS_INT_GPS_ID GPS_MULTI_ID #endif -static abi_event accel_ev; static abi_event gps_ev; - +static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); /** ABI binding for VELOCITY_ESTIMATE. * Usually this is coming from opticflow. @@ -201,13 +199,13 @@ void ins_int_init(void) ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS; // Bind to BARO_ABS message - AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); + AbiBindMsgBARO_ABS(INS_INT_BARO_ID, &baro_ev, baro_cb); ins_int.baro_initialized = false; #if USE_SONAR ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL; // Bind to AGL message - AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb); + AbiBindMsgAGL(INS_INT_SONAR_ID, &sonar_ev, sonar_cb); #endif ins_int.vf_reset = false; @@ -228,6 +226,13 @@ void ins_int_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref); #endif + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb); + AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb); } void ins_reset_local_origin(void) @@ -549,15 +554,3 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)), /* reset the counter to indicate we just had a measurement update */ ins_int.propagation_cnt = 0; } - -void ins_int_register(void) -{ - ins_register_impl(ins_int_init); - - /* - * Subscribe to scaled IMU measurements and attach callbacks - */ - AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb); - AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb); -} diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index f6a71732dbe..e4f42b06a52 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -74,11 +74,4 @@ extern void ins_int_init(void); extern void ins_int_propagate(struct Int32Vect3 *accel, float dt); extern void ins_int_update_gps(struct GpsState *gps_s); - -#ifndef DefaultInsImpl -#define DefaultInsImpl ins_int -#endif - -extern void ins_int_register(void); - #endif /* INS_INT_H */ diff --git a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c index fd71a4e5a09..b819e92682b 100644 --- a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c +++ b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c @@ -28,12 +28,6 @@ */ #include "subsystems/ins/ins_vectornav_wrapper.h" - -void ins_vectornav_register(void) -{ - ins_register_impl(ins_vectornav_init); -} - void gps_vectornav_init(void) { gps.nb_channels = 0; diff --git a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h index bc3526631e5..d797f3461d2 100644 --- a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h +++ b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h @@ -31,14 +31,6 @@ #include "subsystems/ins/ins_vectornav.h" -#ifndef DefaultInsImpl -#define DefaultInsImpl ins_vectornav -#endif - -#define InsEvent ins_vectornav_event - -extern void ins_vectornav_register(void); - #ifndef PRIMARY_GPS #define PRIMARY_GPS GPS_VECTORNAV #endif