diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml index 7af675ada2..6f937d627d 100644 --- a/conf/modules/ins_arduimu_basic.xml +++ b/conf/modules/ins_arduimu_basic.xml @@ -1,5 +1,8 @@ - +
diff --git a/conf/settings/ins_arduimu_basic.xml b/conf/settings/ins_arduimu_basic.xml new file mode 100644 index 0000000000..b3f3818b79 --- /dev/null +++ b/conf/settings/ins_arduimu_basic.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde index d1850d86e9..9fb3d6f4e3 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde @@ -24,10 +24,10 @@ void parse_pprz_gps() { ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D 4,5,6,7 ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11 stGpsFix = Paparazzi_GPS_buffer[12]; - stFlags = Paparazzi_GPS_buffer[13]; + calibrate_neutrals = Paparazzi_GPS_buffer[13]; high_accel_flag = Paparazzi_GPS_buffer[14]; - if((stGpsFix >= 0x03) && (stFlags&0x01)) { + if(stGpsFix >= 0x03) { gpsFix = 0; //valid position digitalWrite(6,HIGH); //Turn LED when gps is fixed. GPS_timer = DIYmillis(); //Restarting timer... @@ -37,8 +37,6 @@ void parse_pprz_gps() { digitalWrite(6,LOW); } - if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw); - } diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde index 2ee09a3d7c..da5cd3db63 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde @@ -44,8 +44,6 @@ void receiveEvent(int howMany){ parse_pprz_gps(); // Parse new GPS packet... GPS_timer=DIYmillis(); //Restarting timer... - gpsDataReady=1; - } @@ -130,19 +128,16 @@ void printdata(void){ #endif #if PRINT_GPS == 1 - if(gpsFixnew==1) { - gpsFixnew=0; - Serial.print("COG:"); - Serial.print((ground_course)); - Serial.print(",SOG:"); - Serial.print(ground_speed); - Serial.print(",FIX:"); - Serial.print((int)gpsFix); - Serial.print (","); + Serial.print("COG:"); + Serial.print((ground_course)); + Serial.print(",SOG:"); + Serial.print(ground_speed); + Serial.print(",FIX:"); + Serial.print((int)gpsFix); + Serial.print (","); #if PERFORMANCE_REPORTING == 1 - gps_messages_sent++; + gps_messages_sent++; #endif - } Serial.println(""); #endif diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde index b213b961b0..2e5652fad5 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde @@ -20,12 +20,9 @@ // *** NOTE! Hardware version - Can be used for v1 (daughterboards) or v2 (flat) #define BOARD_VERSION 2 // 1 For V1 and 2 for V2 -// Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot -#define ENABLE_AIR_START 1 // 1 if using airstart/groundstart signaling, 0 if not -#define GROUNDSTART_PIN 8 // Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware) - /*Min Speed Filter for Yaw drift Correction*/ #define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter +// SPEEDFILT is also used to prevent ground_start calibration when moving /*For debugging propurses*/ #define PRINT_DEBUG 0 //Will print Debug messages @@ -92,7 +89,6 @@ long timeNow=0; // Hold the milliseond value for now long timer=0; //general purpuse timer long timer_old; long timer24=0; //Second timer used to print values -boolean groundstartDone = false; // Used to not repeat ground start float AN[8]; //array that store the 6 ADC filtered data float AN_OFFSET[8]; //Array that stores the Offset of the gyros @@ -105,6 +101,7 @@ float Omega_I[3]= {0,0,0};//Omega Integrator float Omega[3]= {0,0,0}; boolean high_accel_flag = false; // disable update on accelerometers if true +boolean calibrate_neutrals = false; // perform a calibration of gyro and accel neutrals // Euler angles float roll; @@ -153,21 +150,15 @@ union int_union { /*Flight GPS variables*/ int gpsFix=1; //This variable store the status of the GPS -int gpsFixnew=0; //used to flag when new gps data received - used for binary output message flags -int gps_fix_count = 5; //used to count 5 good fixes at ground startup float speed_3d=0; //Speed (3-D) float ground_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots float ground_course=90;//This is the runaway direction of you "plane" in degrees -float gc_offset = 0; // Force yaw output to ground course when fresh data available (only implemented for ublox&binary message) unsigned long GPS_timer=0; //***********************GPS PAPARAZZI************************************************************************ #define PPRZ_MAXPAYLOAD 32 byte Paparazzi_GPS_buffer[PPRZ_MAXPAYLOAD]; -int gpsDataReady = 0; // sind neuen GPS daten vorhanden ?? byte stGpsFix; -byte stFlags; -byte messageNr; //************************************************************************************************************ //ADC variables @@ -214,8 +205,6 @@ void setup() pinMode(5,OUTPUT); //Red LED pinMode(6,OUTPUT); // Blue LED pinMode(7,OUTPUT); // Yellow LED - pinMode(GROUNDSTART_PIN,INPUT); // Remove Before Fly flag (pin 6 on ArduPilot) - digitalWrite(GROUNDSTART_PIN,HIGH); //************Define I2C Output Handler************************ Wire.begin(17); // join i2c bus with address #2 @@ -240,13 +229,8 @@ void setup() //Serial.println(SOFTWARE_VER); debug_handler(0);//Printing version - if(ENABLE_AIR_START) { - debug_handler(1); - startup_air(); - } else { - debug_handler(2); - startup_ground(); - } + startup_air(); + debug_handler(1); delay(250); Read_adc_raw(); // ADC initialization @@ -318,8 +302,11 @@ void loop() //Main Loop break; case(1): - //Here we will check if we are getting a signal to ground start - if (digitalRead(GROUNDSTART_PIN) == LOW && groundstartDone == false) startup_ground(); + //Here we will check if we are getting a signal to ground start and speed is high enough + if (calibrate_neutrals && ground_speed < SPEEDFILT) { + startup_ground(); + calibrate_neutrals = false; + } break; case(2): @@ -430,7 +417,6 @@ void startup_ground(void) eeprom_write_word((uint16_t *) (y*2+2), store); } - groundstartDone = true; debug_handler(6); } diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 52bf037f90..8ebbcfad64 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -1,22 +1,36 @@ /* -C Datei für die Einbindung eines ArduIMU's -Autoren@ZHAW: schmiemi - chaneren -*/ - + * Copyright (C) 2011 Gautier Hattenberger + * based on ArduIMU driver: + * Autoren@ZHAW: schmiemi + * chaneren + * + * 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. + * + */ #include #include "modules/ins/ins_arduimu_basic.h" -//#include "firmwares/fixedwing/main_fbw.h" #include "mcu_periph/i2c.h" -// test +// Estimator interface #include "estimator.h" // GPS data for ArduIMU -#ifndef UBX -#error "currently only compatible with uBlox GPS modules" -#endif #include "subsystems/gps.h" // Command vector for thrust @@ -55,6 +69,10 @@ struct FloatVect3 arduimu_accel; float ins_roll_neutral; float ins_pitch_neutral; +// Ask the arduimu to recalibrate the gyros and accels neutrals +// After calibration, values are store in the arduimu eeprom +bool_t arduimu_calibrate_neutrals; + // High Accel Flag #define HIGH_ACCEL_LOW_SPEED 15.0 #define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis @@ -73,6 +91,7 @@ void ArduIMU_init( void ) { ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; + arduimu_calibrate_neutrals = FALSE; high_accel_done = FALSE; high_accel_flag = FALSE; @@ -109,11 +128,13 @@ void ArduIMU_periodicGPS( void ) { FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6)); // course - ardu_gps_trans.buf[12] = gps.fix; // status gps fix - ardu_gps_trans.buf[13] = gps_ubx.status_flags; // status flags + ardu_gps_trans.buf[12] = gps.fix; // status gps fix + ardu_gps_trans.buf[13] = (uint8_t)arduimu_calibrate_neutrals; // calibration flag ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter) I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15); + // Reset calibration flag + if (arduimu_calibrate_neutrals) arduimu_calibrate_neutrals = FALSE; } void ArduIMU_periodic( void ) { diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.h b/sw/airborne/modules/ins/ins_arduimu_basic.h index 3e64cf3125..f21b12b160 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.h +++ b/sw/airborne/modules/ins/ins_arduimu_basic.h @@ -1,7 +1,32 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * based on ArduIMU driver: + * Autoren@ZHAW: schmiemi + * chaneren + * + * 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. + * + */ + #ifndef ArduIMU_H #define ArduIMU_H -#include +#include "std.h" #include "math/pprz_algebra_float.h" extern struct FloatEulers arduimu_eulers; @@ -10,6 +35,7 @@ extern struct FloatVect3 arduimu_accel; extern float ins_roll_neutral; extern float ins_pitch_neutral; +extern bool_t arduimu_calibrate_neutrals; void ArduIMU_init( void ); void ArduIMU_periodic( void );