diff --git a/conf/airframes/osam_xsens_twog.xml b/conf/airframes/osam_xsens_twog.xml index 06fc9867d4..e4a27e110c 100644 --- a/conf/airframes/osam_xsens_twog.xml +++ b/conf/airframes/osam_xsens_twog.xml @@ -2,6 +2,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -72,6 +117,12 @@ + +
+ + +
+
- +
@@ -159,56 +210,20 @@ --> - -CONFIG=\"tiny_2_1.h\" -include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile - -FLASH_MODE=IAP - -ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DLED -DTIME_LED=1 -ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c - -ap.srcs += commands.c - -ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c - -# 72MHz -ap.CFLAGS += -DRADIO_CONTROL -ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + diff --git a/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile b/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile new file mode 100644 index 0000000000..accfc304e0 --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile @@ -0,0 +1,5 @@ +# OSAM UGEAR + +#ap.CFLAGS += -DGPS + +$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c diff --git a/conf/modules/ins_osam_ugear.xml b/conf/modules/ins_osam_ugear.xml new file mode 100644 index 0000000000..82f0bdfa78 --- /dev/null +++ b/conf/modules/ins_osam_ugear.xml @@ -0,0 +1,13 @@ + + + +
+ +
+ + + + + + + diff --git a/doc/OSAM_IMU/readme_pprz_osam_imu.txt b/doc/OSAM_IMU/readme_pprz_osam_imu.txt index a61b166d05..3e5411224f 100644 --- a/doc/OSAM_IMU/readme_pprz_osam_imu.txt +++ b/doc/OSAM_IMU/readme_pprz_osam_imu.txt @@ -6,38 +6,13 @@ Configuration files in 5 are generated by Daniel Morgan & Haiyang Chao. 1. Brief This file is to explain what we have done to make the pprz compatible with IMU (xsens-mtig and microstrain-gx2). Haiyang Chao on 20080912 for version 1 +adapted to new build config 20101022 2. New Airborne files added -~/paparazzi3/sw/airborne/osam_imu_ugear.h -~/paparazzi3/sw/airborne/osam_imu_ugear.c +~/paparazzi3/sw/airborne/modules/ins/ins_osam_ugear.h +~/paparazzi3/sw/airborne/modules/ins/ins_osam_ugear.c 3. Airborne Files modified (all modifications we made are triggered only if UGEAR is defined) -~/paparazzi3/sw/airborne/main_ap.c ------Line 122------------------------ -#ifdef UGEAR -#include "osam_imu_ugear.h" -#endif - -#ifdef XSENS -#include "xsens_ins.h" -#endif ------Line 733------------------------ -#ifdef UGEAR - if (UgearBuffer()){ - ReadUgearBuffer(); - } - if (ugear_msg_received){ - parse_ugear_msg(); - ugear_msg_received = FALSE; - if (gps_pos_available){ - gps_downlink(); - gps_verbose_downlink = !launch; - UseGpsPos(estimator_update_state_gps); - gps_pos_available = FALSE; - } - } -#endif /* UGEAR*/ ------------------------------ ~/paparazzi3/sw/airborne/ap_downlink.h -----Line 165------------------------ #if defined UGEAR @@ -51,20 +26,6 @@ Haiyang Chao on 20080912 for version 1 #elif defined UGEAR #define GPS_NB_CHANNELS 16 ----------------------------- -~/paparazzi3/sw/airborne/infrared.c ------Line 42------------------------ -#ifdef UGEAR -#include "osam_imu_ugear.h" -#endif ------Line 239------------------------ -#if defined UGEAR - #if !(defined IMUIR) - ugear_debug3 = 333; - estimator_phi = (float)ugear_phi/10000 - ir_roll_neutral; - estimator_theta = (float)ugear_theta/10000 - ir_pitch_neutral; - #endif -#endif ------------------------------ 4. Configuration files changed ~/paparazzi3/conf/message.xml @@ -83,4 +44,5 @@ Haiyang Chao on 20080912 for version 1 ~/paparazzi3/conf/telemetry/osam_imu.xml ~/paparazzi3/conf/flight_plans/xsens_cachejunction.xml ~/paparazzi3/conf/airframes/osam_xsens_twog.xml +~/paparazzi3/conf/settings/tuning_ins.xml diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c index c99cef5052..38c9a995b6 100644 --- a/sw/airborne/infrared.c +++ b/sw/airborne/infrared.c @@ -39,10 +39,6 @@ #include "sys_time.h" #include "airframe.h" -#ifdef UGEAR -#include "osam_imu_ugear.h" -#endif - #if defined IR_ESTIMATED_PHI_PI_4 || defined IR_ESTIMATED_PHI_MINUS_PI_4 || defined IR_ESTIMATED_THETA_PI_4 #error "IR_ESTIMATED_PHI_PI_4 correction has been deprecated. Please remove the definition from your airframe config file" #endif @@ -229,12 +225,4 @@ void estimator_update_state_infrared( void ) { else estimator_theta *= ir_correction_down; -#if defined UGEAR - #if !(defined IMUIR) - ugear_debug3 = 333; - estimator_phi = (float)ugear_phi/10000 - ir_roll_neutral; - estimator_theta = (float)ugear_theta/10000 - ir_pitch_neutral; - #endif -#endif - } diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index 4e28207b98..ba7060d756 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -99,15 +99,8 @@ #include "baro_ets.h" #endif // USE_BARO_ETS -/*code added by Haiyang Chao for using Xsens IMU for fixed wing UAV 20080804*/ -#ifdef UGEAR -#include "osam_imu_ugear.h" -#endif - -/*code added by Haiyang Chao ends*/ - #if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY -#warning "LOW_BATTERY deprecated. Renammed into CATASTROPHIC_BAT_LEVEL (in aiframe file)" +#warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in airframe file)" #define CATASTROPHIC_BAT_LEVEL LOW_BATTERY #endif @@ -614,10 +607,6 @@ void init_ap( void ) { gps_configure_uart(); #endif -#ifdef UGEAR - ugear_init(); -#endif /*added by haiyang for ugear communication*/ - #if defined DATALINK #if DATALINK == XBEE @@ -647,34 +636,6 @@ void init_ap( void ) { /*********** EVENT ***********************************************************/ void event_task_ap( void ) { -#ifdef UGEAR - if (UgearBuffer()){ - ReadUgearBuffer(); - } - if (ugear_msg_received){ - parse_ugear_msg(); - ugear_msg_received = FALSE; - if (gps_pos_available){ - //gps_downlink(); - gps_verbose_downlink = !launch; - UseGpsPosNoSend(estimator_update_state_gps); - gps_msg_received_counter = gps_msg_received_counter+1; - #ifdef GX2 - if (gps_msg_received_counter == 1){ - gps_send(); - gps_msg_received_counter = 0; - } - #endif - #ifdef XSENSDL - if (gps_msg_received_counter == 25){ - gps_send(); - gps_msg_received_counter = 0; - } - #endif - gps_pos_available = FALSE; - } - } -#endif /* UGEAR*/ #ifdef GPS #if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */ diff --git a/sw/airborne/osam_imu_ugear.c b/sw/airborne/modules/ins/ins_osam_ugear.c similarity index 85% rename from sw/airborne/osam_imu_ugear.c rename to sw/airborne/modules/ins/ins_osam_ugear.c index cf516220fe..9acd4839ea 100644 --- a/sw/airborne/osam_imu_ugear.c +++ b/sw/airborne/modules/ins/ins_osam_ugear.c @@ -30,8 +30,9 @@ #include #include -#include "osam_imu_ugear.h" +#include "ins_osam_ugear.h" #include "gps.h" +#include "gps_ubx.h" #include "latlong.h" #include "sys_time.h" #include "airframe.h" @@ -56,10 +57,15 @@ #define RAD2DEG 57.3 #define WrapUp(x) (x < 0 ? x+3600 : x) +/* from main_ap.c */ +extern bool_t launch; + /* variable defined for dlsetting generated by Haiyang 20080717*/ bool_t imu1_ir0 = 0; float fd_alpha = 0; float fi_alpha = 0; +float ins_roll_neutral; +float ins_pitch_neutral; /* variable definition copied from gps_ubx.c 20080508*/ uint16_t gps_week; @@ -89,7 +95,7 @@ uint8_t gps_nb_channels; /* variable definition copied from gps.c20080515*/ -uint16_t last_gps_msg_t; /** cputime of the last gps message */ +//uint16_t last_gps_msg_t; /** cputime of the last gps message */ volatile bool_t ugear_msg_received; @@ -114,7 +120,7 @@ int32_t ugear_debug4; int32_t ugear_debug5; int32_t ugear_debug6; -bool_t gps_verbose_downlink; +//bool_t gps_verbose_downlink; uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD] __attribute__ ((aligned)); @@ -138,6 +144,8 @@ void ugear_init( void ) { ugear_psi = 0; ugear_theta = 0; ugear_debug2 = 0; + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; } void parse_ugear( uint8_t c ) { @@ -211,6 +219,8 @@ void decode_imupacket( struct imu *data, uint8_t* buffer){ */ void parse_ugear_msg( void ){ + float ins_phi, ins_psi, ins_theta; + switch (ugear_type){ case 0: /*gps*/ ugear_debug1 = ugear_debug1+1; @@ -251,6 +261,13 @@ void parse_ugear_msg( void ){ ugear_debug4 = (int32_t)ugear_phi; ugear_debug5 = (int32_t)ugear_theta; ugear_debug6 = (int32_t)ugear_psi; + ugear_debug3 = 333; + ins_phi = (float)ugear_phi/10000 - ins_roll_neutral; + ins_psi = 0; + ins_theta = (float)ugear_theta/10000 - ins_pitch_neutral; +#ifndef INFRARED + EstimatorSetAtt(ins_phi, ins_psi, ins_theta); +#endif break; case 2: /*GPS status*/ // ugear_debug1 = 2; @@ -280,3 +297,33 @@ void parse_ugear_msg( void ){ void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) { } +void ugear_event( void ) { + if (UgearBuffer()){ + ReadUgearBuffer(); + } + if (ugear_msg_received){ + parse_ugear_msg(); + ugear_msg_received = FALSE; + if (gps_pos_available){ + //gps_downlink(); + gps_verbose_downlink = !launch; + UseGpsPosNoSend(estimator_update_state_gps); + gps_msg_received_counter = gps_msg_received_counter+1; + #ifdef GX2 + if (gps_msg_received_counter == 1){ + gps_send(); + gps_msg_received_counter = 0; + } + #endif + #ifdef XSENSDL + if (gps_msg_received_counter == 25){ + gps_send(); + gps_msg_received_counter = 0; + } + #endif + gps_pos_available = FALSE; + } + } +} + + diff --git a/sw/airborne/osam_imu_ugear.h b/sw/airborne/modules/ins/ins_osam_ugear.h similarity index 97% rename from sw/airborne/osam_imu_ugear.h rename to sw/airborne/modules/ins/ins_osam_ugear.h index fac5382172..e2db0ea36b 100644 --- a/sw/airborne/osam_imu_ugear.h +++ b/sw/airborne/modules/ins/ins_osam_ugear.h @@ -32,6 +32,7 @@ void ugear_init( void ); void parse_ugear(uint8_t c); void parse_ugear_msg( void ); +void ugear_event( void ); #define __UgearLink(dev, _x) dev##_x #define _UgearLink(dev, _x) __UgearLink(dev, _x) @@ -89,5 +90,6 @@ extern struct imu imupacket; extern struct gps gpspacket; extern uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD]; -/* add the following function only to get rid of compilation error in datalink.c 20080608 Haiyang*/ -void ubxsend_cfg_rst(uint16_t, uint8_t); +extern float ins_roll_neutral; +extern float ins_pitch_neutral; +