arduimu_basic is independent of gps type and calibration is triggered

from a setting

conf/modules/ins_arduimu_basic.xml

@@ -1,5 +1,8 @@
 <!DOCTYPE module SYSTEM "module.dtd">
-
+<!-- optional flags:
+    - USE_HIGH_ACCEL_FLAG: will disable accelerometers on high acceleration detection (low speed, high thrust)
+    - ARDUIMU_SYNC_SEND: downlink imu gyro and accels
+-->
 <module name="ins_ArduIMU" dir="ins">
   <header>
     <file name="ins_arduimu_basic.h"/>

conf/settings/ins_arduimu_basic.xml

@@ -0,0 +1,11 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<settings>
+  <dl_settings>
+    <dl_settings NAME="ins">
+      <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
+      <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="arduimu_calibrate_neutrals" shortname="calibrate" values="FALSE|TRUE"/>
+    </dl_settings>
+  </dl_settings>
+</settings>

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);
-
 }
 
 

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) {
+  Serial.print("COG:");
-    gpsFixnew=0;
+  Serial.print((ground_course));
-    Serial.print("COG:");
+  Serial.print(",SOG:");
-    Serial.print((ground_course));
+  Serial.print(ground_speed);
-    Serial.print(",SOG:");
+  Serial.print(",FIX:");
-    Serial.print(ground_speed);
+  Serial.print((int)gpsFix);
-    Serial.print(",FIX:");
+  Serial.print (",");
-    Serial.print((int)gpsFix);
-    Serial.print (",");
 #if PERFORMANCE_REPORTING == 1
-    gps_messages_sent++;
+  gps_messages_sent++;
 #endif
-  }
   Serial.println("");
 #endif
 

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) {
+  startup_air();
-    debug_handler(1);
+  debug_handler(1);
-    startup_air();
-  } else {
-    debug_handler(2);
-    startup_ground();
-  }
 
   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
+        //Here we will check if we are getting a signal to ground start and speed is high enough
-        if (digitalRead(GROUNDSTART_PIN) == LOW && groundstartDone == false) startup_ground();
+        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);
 }
 

sw/airborne/modules/ins/ins_arduimu_basic.c

@@ -1,22 +1,36 @@
 /*
-C Datei für die Einbindung eines ArduIMU's
+ * Copyright (C) 2011 Gautier Hattenberger
-Autoren@ZHAW:   schmiemi
+ * based on ArduIMU driver:
-        chaneren
+ *   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 <stdbool.h>
 #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[12] = gps.fix;                               // status gps fix
-  ardu_gps_trans.buf[13] = gps_ubx.status_flags;                      // status flags
+  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 ) {

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 <inttypes.h>
+#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 );