From 0072a01493d74ad90cb5a8112dd03af06c10883c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Sep 2012 20:21:18 +0200 Subject: [PATCH] [fixedwing] mandatory ins subsystem * added ins_gps_passthrough which just uses the gps measurements and passes them along to the state interface * fix ins_xsens subsystem --- conf/airframes/ENAC/fixed-wing/weasel.xml | 2 +- .../airframes/examples/delta_wing_minimal.xml | 1 + conf/airframes/examples/funjet.xml | 1 + conf/airframes/examples/funjet_cam.xml | 8 +- .../examples/microjet_lisa_m_xsens.xml | 1 - conf/airframes/examples/twinjet.xml | 1 + conf/airframes/examples/twinstar.xml | 3 +- conf/airframes/twinjet_overo.xml | 1 + conf/conf.xml.example | 2 +- .../fixedwing/ins_alt_float.makefile | 1 - .../fixedwing/ins_gps_passthrough.makefile | 4 + .../subsystems/fixedwing/ins_xsens.makefile | 28 +++---- conf/tests_conf.xml | 2 +- sw/airborne/firmwares/fixedwing/main_ap.c | 11 +-- sw/airborne/modules/ins/ins_module.h | 2 +- sw/airborne/modules/ins/ins_xsens.c | 45 +++++++---- sw/airborne/modules/ins/ins_xsens.h | 2 +- .../subsystems/ins/ins_gps_passthrough.c | 78 +++++++++++++++++++ 18 files changed, 140 insertions(+), 53 deletions(-) create mode 100644 conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile create mode 100644 sw/airborne/subsystems/ins/ins_gps_passthrough.c diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml index f34fd0b78d3..25c8678dff3 100644 --- a/conf/airframes/ENAC/fixed-wing/weasel.xml +++ b/conf/airframes/ENAC/fixed-wing/weasel.xml @@ -37,7 +37,7 @@ - + diff --git a/conf/airframes/examples/delta_wing_minimal.xml b/conf/airframes/examples/delta_wing_minimal.xml index cb09a41fb5c..605589ad313 100644 --- a/conf/airframes/examples/delta_wing_minimal.xml +++ b/conf/airframes/examples/delta_wing_minimal.xml @@ -20,6 +20,7 @@ + diff --git a/conf/airframes/examples/funjet.xml b/conf/airframes/examples/funjet.xml index 824ac1b0a62..c99707e1e4e 100644 --- a/conf/airframes/examples/funjet.xml +++ b/conf/airframes/examples/funjet.xml @@ -30,6 +30,7 @@ + diff --git a/conf/airframes/examples/funjet_cam.xml b/conf/airframes/examples/funjet_cam.xml index befcb47f0cb..6d14f4906c6 100644 --- a/conf/airframes/examples/funjet_cam.xml +++ b/conf/airframes/examples/funjet_cam.xml @@ -26,10 +26,10 @@ - - - - + + + + diff --git a/conf/airframes/examples/microjet_lisa_m_xsens.xml b/conf/airframes/examples/microjet_lisa_m_xsens.xml index 824f7598de8..3058b03c306 100644 --- a/conf/airframes/examples/microjet_lisa_m_xsens.xml +++ b/conf/airframes/examples/microjet_lisa_m_xsens.xml @@ -28,7 +28,6 @@ - diff --git a/conf/airframes/examples/twinjet.xml b/conf/airframes/examples/twinjet.xml index 801f5217531..b7027e79255 100644 --- a/conf/airframes/examples/twinjet.xml +++ b/conf/airframes/examples/twinjet.xml @@ -26,6 +26,7 @@ + diff --git a/conf/airframes/examples/twinstar.xml b/conf/airframes/examples/twinstar.xml index 7d7803a210f..77cc38b320f 100644 --- a/conf/airframes/examples/twinstar.xml +++ b/conf/airframes/examples/twinstar.xml @@ -21,8 +21,9 @@ - + + diff --git a/conf/airframes/twinjet_overo.xml b/conf/airframes/twinjet_overo.xml index a3b66c33cc7..ec388c7d1c5 100644 --- a/conf/airframes/twinjet_overo.xml +++ b/conf/airframes/twinjet_overo.xml @@ -27,6 +27,7 @@ + diff --git a/conf/conf.xml.example b/conf/conf.xml.example index 36b52711566..e518dae1a82 100644 --- a/conf/conf.xml.example +++ b/conf/conf.xml.example @@ -39,7 +39,7 @@ ac_id="24" airframe="airframes/examples/bixler_lisa_m_2.xml" radio="radios/cockpitSX.xml" - telemetry="telemetry/default_fixedwing.xml" + telemetry="telemetry/default_fixedwing_imu.xml" flight_plan="flight_plans/versatile.xml" settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml" gui_color="blue" diff --git a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile index f89e761b655..1abd1c78817 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile @@ -1,6 +1,5 @@ # Hey Emacs, this is a -*- makefile -*- -ap_CFLAGS += -DUSE_INS 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 diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile new file mode 100644 index 00000000000..19f3fdf8dc5 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile @@ -0,0 +1,4 @@ + +ap.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c + +sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index c969604bb6f..030d9e7a5d9 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -12,9 +12,7 @@ ######################################### ## ATTITUDE -ifeq ($(TARGET), ap) - -ap.CFLAGS += -DUSE_INS +ap.CFLAGS += -DUSE_INS_MODULE # AHRS Results ap.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_xsens.h\" @@ -34,7 +32,6 @@ ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP -endif ifeq ($(TARGET), fbw) @@ -45,16 +42,6 @@ fbw.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\" endif -ifeq ($(TARGET), sim) - -sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" -sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR - -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c - -endif - ######################################### ## GPS @@ -65,6 +52,19 @@ ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\" ap.srcs += $(SRC_SUBSYSTEMS)/gps.c + +######################################### +## Simulator + +sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" +sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR + +sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c +sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c + +sim.CFLAGS += -DUSE_INS_MODULE -DINS_TYPE_H=\"subsystems/ins/ins_gps_only.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_only.c + sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/conf/tests_conf.xml b/conf/tests_conf.xml index 76ab26144cc..96790747ed3 100644 --- a/conf/tests_conf.xml +++ b/conf/tests_conf.xml @@ -39,7 +39,7 @@ ac_id="24" airframe="airframes/examples/bixler_lisa_m_2.xml" radio="radios/cockpitSX.xml" - telemetry="telemetry/default_fixedwing.xml" + telemetry="telemetry/default_fixedwing_imu.xml" flight_plan="flight_plans/versatile.xml" settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml" gui_color="blue" diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 4426d1b944f..aa174d33636 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -56,9 +56,8 @@ #if USE_BAROMETER #include "subsystems/sensors/baro.h" #endif -#if USE_INS #include "subsystems/ins.h" -#endif + // autopilot & control #include "state.h" @@ -184,9 +183,7 @@ void init_ap( void ) { baro_init(); #endif -#if USE_INS ins_init(); -#endif stateInit(); @@ -566,9 +563,7 @@ void sensors_task( void ) { baro_periodic(); #endif -#if USE_INS ins_periodic(); -#endif } @@ -670,9 +665,7 @@ void event_task_ap( void ) { #if USE_GPS static inline void on_gps_solution( void ) { -#if USE_INS ins_update_gps(); -#endif #if USE_AHRS ahrs_update_gps(); #endif @@ -774,9 +767,7 @@ static inline void on_mag_event(void) #if USE_BAROMETER static inline void on_baro_abs_event( void ) { -#if USE_INS ins_update_baro(); -#endif } static inline void on_baro_dif_event( void ) { diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h index 06a516b2849..7c2a0a1698f 100644 --- a/sw/airborne/modules/ins/ins_module.h +++ b/sw/airborne/modules/ins/ins_module.h @@ -59,7 +59,7 @@ extern INS_FORMAT ins_mx; extern INS_FORMAT ins_my; extern INS_FORMAT ins_mz; -#if USE_INS +#if USE_INS_MODULE extern INS_FORMAT ins_roll_neutral; extern INS_FORMAT ins_pitch_neutral; #endif diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index e7f71534d52..1cedb9e8ea5 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -20,7 +20,7 @@ * */ -/** @file xsens.c +/** @file ins_xsens.c * Parser for the Xsens protocol. */ @@ -33,7 +33,6 @@ #include "generated/airframe.h" #include "mcu_periph/sys_time.h" -#include "subsystems/datalink/downlink.h" #include "messages.h" #if USE_GPS_XSENS @@ -71,7 +70,7 @@ INS_FORMAT ins_mx; INS_FORMAT ins_my; INS_FORMAT ins_mz; -#if USE_INS +#if USE_INS_MODULE float ins_pitch_neutral; float ins_roll_neutral; #endif @@ -207,12 +206,15 @@ uint8_t send_ck; volatile int xsens_configured = 0; -void ins_init( void ) { +void xsens_init(void); +void xsens_periodic(void); + +void xsens_init(void) { xsens_status = UNINIT; xsens_configured = 20; -#if USE_INS +#if USE_INS_MODULE ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; #endif @@ -227,17 +229,30 @@ void ins_init( void ) { struct ImuXsens imu_xsens; void imu_impl_init(void) { - ins_init(); + xsens_init(); imu_xsens.gyro_available = FALSE; imu_xsens.accel_available = FALSE; imu_xsens.mag_available = FALSE; } void imu_periodic(void) { - ins_periodic(); + xsens_periodic(); } #endif /* USE_IMU */ +#if USE_INS_MODULE +void ins_init(void) { + xsens_init(); +} + +void ins_periodic(void) { + xsens_periodic(); +} + +void ins_update_gps(void) { +} +#endif + #if USE_GPS_XSENS void gps_impl_init(void) { gps.nb_channels = 0; @@ -245,7 +260,7 @@ void gps_impl_init(void) { } #endif -void ins_periodic( void ) { +void xsens_periodic(void) { if (xsens_configured > 0) { switch (xsens_configured) @@ -306,11 +321,7 @@ void ins_periodic( void ) { RunOnceEvery(100,XSENS_ReqGPSStatus()); } -void ins_update_gps(void) { - -} - -#if USE_INS +#if USE_INS_MODULE #include "state.h" static inline void update_fw_estimator(void) { @@ -341,11 +352,11 @@ static inline void update_fw_estimator(void) { stateSetNedToBodyEulers_f(&att); stateSetBodyRates_f(&rates); } -#endif /* USE_INS */ +#endif /* USE_INS_MODULE */ void handle_ins_msg(void) { -#if USE_INS +#if USE_INS_MODULE update_fw_estimator(); #endif @@ -388,7 +399,7 @@ void handle_ins_msg(void) { #endif // USE_GPS_XSENS } -void parse_ins_msg( void ) { +void parse_ins_msg(void) { uint8_t offset = 0; if (xsens_id == XSENS_ReqOutputModeAck_ID) { xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf); @@ -633,7 +644,7 @@ void parse_ins_msg( void ) { } -void parse_ins_buffer( uint8_t c ) { +void parse_ins_buffer(uint8_t c) { ck += c; switch (xsens_status) { case UNINIT: diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 595336315ef..00cb603419b 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -80,7 +80,7 @@ extern struct ImuXsens imu_xsens; /* use Xsens as a full INS solution */ -#if USE_INS +#if USE_INS_MODULE #define InsEvent(_ins_handler) { \ InsEventCheckAndHandle(handle_ins_msg()) \ } diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c new file mode 100644 index 00000000000..51625cada08 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2004-2012 The Paparazzi Team + * + * 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 ins_gps_passthrough.c + * Simply passes GPS position and velocity through to the state interface. + */ + +#include "subsystems/ins.h" + +#include +#include + +#include "state.h" +#include "subsystems/gps.h" +#include "subsystems/nav.h" + +void ins_init() { + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + stateSetPositionUtm_f(&utm0); +} + +void ins_periodic( void ) { +} + +void ins_update_gps(void) { + struct UtmCoor_f utm; + utm.east = gps.utm_pos.east / 100.; + utm.north = gps.utm_pos.north / 100.; + utm.zone = nav_utm_zone0; + utm.alt = gps.hmsl / 1000.; + + // set position + stateSetPositionUtm_f(&utm); + + struct NedCoor_f ned_vel = { + gps.ned_vel.x / 100., + gps.ned_vel.y / 100., + gps.ned_vel.z / 100. + }; + // set velocity + stateSetSpeedNed_f(&ned_vel); +} + + +void ins_propagate() { +} + +void ins_update_baro() { +} + +void ins_update_sonar() { +} + +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { +} + +void ins_realign_v(float z __attribute__ ((unused))) { +} +