fix sim with arduimu_basic; add a high_accel flag to prevent updates on

high acceleration

conf/modules/ins_arduimu_basic.xml

@@ -12,7 +12,7 @@
     <file name="ins_arduimu_basic.c"/>
   </makefile>
   <makefile target="sim">
-    <file_arch name="ins_arduimu.c"/>
+    <file_arch name="ins_arduimu_basic.c"/>
   </makefile>
 </module>
 

sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c

@@ -0,0 +1,47 @@
+/** ArduIMU simulation. OCaml binding.
+ *  Sim provides IR sensor reading and airspeed
+ */
+
+
+#include <inttypes.h>
+#include "generated/airframe.h"
+
+#include <caml/mlvalues.h>
+
+#include "estimator.h"
+
+// Prevent undefined reference (from estimator.c)
+#include "subsystems/sensors/infrared.h"
+struct Infrared infrared;
+
+// Arduimu empty implementation
+#include "modules/ins/ins_arduimu_basic.h"
+
+struct FloatEulers arduimu_eulers;
+struct FloatRates arduimu_rates;
+struct FloatVect3 arduimu_accel;
+
+float ins_roll_neutral;
+float ins_pitch_neutral;
+
+void ArduIMU_init( void ) {}
+void ArduIMU_periodic( void ) {}
+void ArduIMU_periodicGPS( void ) {}
+void ArduIMU_event( void ) {}
+
+// Updates from Ocaml sim
+float sim_air_speed;
+
+value set_ir_and_airspeed(
+    value roll __attribute__ ((unused)),
+    value front __attribute__ ((unused)),
+    value top __attribute__ ((unused)),
+    value air_speed
+    ) {
+  // Feed directly the estimator
+  estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral;
+  estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral;
+  sim_air_speed = Double_val(air_speed);
+  return Val_unit;
+}
+

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde

@@ -97,6 +97,8 @@ void Drift_correction(void)
   // Dynamic weighting of accelerometer info (reliability filter)
   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
   Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
+  // Set to 0 if high_accel_flag is true
+  if (high_accel_flag) Accel_weight = 0.;
 
 #if PERFORMANCE_REPORTING == 1
   tempfloat = ((Accel_weight - 0.5) * 256.0f);    //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde

@@ -25,6 +25,7 @@ void parse_pprz_gps() {
   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];
+  high_accel_flag = Paparazzi_GPS_buffer[14];
 
   if((stGpsFix >= 0x03) && (stFlags&0x01)) {
     gpsFix = 0; //valid position

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde

@@ -104,11 +104,7 @@ float Omega_P[3]= {0,0,0};//Omega Proportional correction
 float Omega_I[3]= {0,0,0};//Omega Integrator
 float Omega[3]= {0,0,0};
 
-//Magnetometer variables
-//int magnetom_x;
-//int magnetom_y;
-//int magnetom_z;
-//float MAG_Heading;
+boolean high_accel_flag = false; // disable update on accelerometers if true
 
 // Euler angles
 float roll;

sw/airborne/modules/ins/ins_arduimu_basic.c

@@ -13,9 +13,13 @@ Autoren@ZHAW:   schmiemi
 // test
 #include "estimator.h"
 
-// für das Senden von GPS-Daten an den ArduIMU
+// GPS data for ArduIMU
 #include "gps.h"
 
+// Command vector for thrust
+#include "generated/airframe.h"
+#include "inter_mcu.h"
+
 #define NB_DATA 9
 
 #ifndef ARDUIMU_I2C_DEV
@@ -24,7 +28,7 @@ Autoren@ZHAW:   schmiemi
 
 // Adresse des I2C Slaves:  0001 0110	letztes Bit ist für Read/Write
 // einzugebende Adresse im ArduIMU ist 0000 1011
-//da ArduIMU das Read/Write Bit selber anfügt.
+// da ArduIMU das Read/Write Bit selber anfügt.
 #define ArduIMU_SLAVE_ADDR 0x22
 
 #ifndef DOWNLINK_DEVICE
@@ -47,6 +51,11 @@ struct FloatVect3 arduimu_accel;
 float ins_roll_neutral;
 float ins_pitch_neutral;
 
+// High Accel Flag
+#define HIGH_ACCEL_LOW_SPEED 2.0
+#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
+bool_t high_accel_flag;
+
 void ArduIMU_init( void ) {
   FLOAT_EULERS_ZERO(arduimu_eulers);
   FLOAT_RATES_ZERO(arduimu_rates);
@@ -57,6 +66,8 @@ void ArduIMU_init( void ) {
 
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+
+  high_accel_flag = FALSE;
 }
 
 #define FillBufWith32bit(_buf, _index, _value) {  \
@@ -70,12 +81,22 @@ void ArduIMU_periodicGPS( void ) {
 
   if (ardu_gps_trans.status != I2CTransDone) { return; }
 
+  // Test for high acceleration:
+  //  - low speed
+  //  - high thrust
+  if (gps_speed_3d < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE]) {
+    high_accel_flag = TRUE;
+  } else {
+    high_accel_flag = FALSE;
+  }
+
   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)gps_course);   // course
   ardu_gps_trans.buf[12] = gps_mode;                              // status gps fix
   ardu_gps_trans.buf[13] = gps_status_flags;                      // status flags
-  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);
+  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);
 
 }