Merge pull request #499 from TobiasMue/ins

fixes to use ins_alt_dot with baro wen USE_BAROMETER is defined

sw/airborne/modules/sensors/baro_MS5534A.h

@@ -54,7 +54,7 @@ void baro_MS5534A_event_task( void );
 
 void baro_MS5534A_event( void );
 
-#define BaroMS5534AUpdate(_b) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; baro_MS5534A_available = FALSE; } }
+#define BaroMS5534AUpdate(_b, _h) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; _h(); baro_MS5534A_available = FALSE; } }
 
 #endif // USE_BARO_MS5534A
 

sw/airborne/modules/sensors/baro_amsys.h

@@ -28,7 +28,8 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 
-#define BARO_AMSYS_DT 0.05
+/// new measurement every baro_amsys_read_periodic
+#define BARO_AMSYS_DT BARO_AMSYS_READ_PERIODIC_PERIOD
 
 extern uint16_t baro_amsys_adc;
 // extern float baro_amsys_offset;
@@ -47,6 +48,6 @@ extern void baro_amsys_read_event( void );
 
 #define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); }
 
-#define BaroAmsysUpdate(_b) { if (baro_amsys_valid) { _b = baro_amsys_adc; baro_amsys_valid = FALSE; } }
+#define BaroAmsysUpdate(_b, _h) { if (baro_amsys_valid) { _b = baro_amsys_adc; _h(); baro_amsys_valid = FALSE; } }
 
 #endif // BARO_AMSYS_H

sw/airborne/modules/sensors/baro_bmp.h

@@ -34,7 +34,13 @@
 #define BARO_BMP_START_PRESS  4
 #define BARO_BMP_READ_PRESS   5
 
-#define BARO_BMP_DT 0.05
+/// new measurement every 3rd baro_bmp_periodic
+#ifndef SITL
+#define BARO_BMP_DT (BARO_BMP_PERIODIC_PERIOID / 3)
+#else
+#define BARO_BMP_DT BARO_BMP_PERIODIC_PERIOID
+#endif
+
 extern bool_t baro_bmp_enabled;
 extern float baro_bmp_r;
 extern float baro_bmp_sigma2;
@@ -51,6 +57,6 @@ void baro_bmp_init(void);
 void baro_bmp_periodic(void);
 void baro_bmp_event(void);
 
-#define BaroBmpUpdate(_b) { if (baro_bmp_valid) { _b = baro_bmp_pressure; baro_bmp_valid = FALSE; } }
+#define BaroBmpUpdate(_b, _h) { if (baro_bmp_valid) { _b = baro_bmp_pressure; _h(); baro_bmp_valid = FALSE; } }
 
 #endif

sw/airborne/modules/sensors/baro_board_module.h

@@ -50,16 +50,14 @@
 #define BARO_DIFF_EVENT NoBaro
 #endif
 
-#define NoBaro(_b) {}
+#define NoBaro(_b, _h) {}
 
 /** BaroEvent macro.
  *  Need to be maped to one the external baro running has a module
  */
 #define BaroEvent(_b_abs_handler, _b_diff_handler) {  \
-  BARO_ABS_EVENT(baro.absolute);                      \
+    BARO_ABS_EVENT(baro.absolute, _b_abs_handler);       \
-  BARO_DIFF_EVENT(baro.differential);                 \
+    BARO_DIFF_EVENT(baro.differential, _b_diff_handler); \
-  _b_abs_handler();                                   \
-  _b_diff_handler();                                  \
 }
 
 

sw/airborne/modules/sensors/baro_ets.h

@@ -45,7 +45,8 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 
-#define BARO_ETS_DT 0.05
+/// new measurement every baro_ets_read_periodic
+#define BARO_ETS_DT BARO_ETS_READ_PERIODIC_PERIOD
 
 extern uint16_t baro_ets_adc;
 extern uint16_t baro_ets_offset;
@@ -63,6 +64,6 @@ extern void baro_ets_read_event( void );
 
 #define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); }
 
-#define BaroEtsUpdate(_b) { if (baro_ets_valid) { _b = baro_ets_adc; baro_ets_valid = FALSE; } }
+#define BaroEtsUpdate(_b, _h) { if (baro_ets_valid) { _b = baro_ets_adc; _h(); baro_ets_valid = FALSE; } }
 
 #endif // BARO_ETS_H

sw/airborne/modules/sensors/baro_ms5611_i2c.h

@@ -12,7 +12,9 @@
 
 #define PROM_NB                 8
 
-#define BARO_MS5611_DT 0.05
+/// new measurement every baro_ms5611_periodic
+#define BARO_MS5611_DT BARO_MS5611_PERIODIC_PERIOID
+
 #define BARO_MS5611_R 20
 #define BARO_MS5611_SIGMA2 1
 extern float baro_ms5611_alt;
@@ -42,6 +44,6 @@ extern void baro_ms5611_d1(void);
 extern void baro_ms5611_d2(void);
 extern void baro_ms5611_event(void);
 
-#define BaroMs5611Update(_b) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; baro_ms5611_valid = FALSE; } }
+#define BaroMs5611Update(_b, _h) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; _h(); baro_ms5611_valid = FALSE; } }
 
 #endif

sw/airborne/modules/sensors/baro_scp.h

@@ -20,6 +20,6 @@ void baro_scp_init(void);
 void baro_scp_periodic(void);
 void baro_scp_event(void);
 
-#define BaroScpUpdate(_b) { if (baro_scp_available) { _b = baro_scp_pressure; baro_scp_available = FALSE; } }
+#define BaroScpUpdate(_b, _h) { if (baro_scp_available) { _b = baro_scp_pressure; _h(); baro_scp_available = FALSE; } }
 
 #endif

sw/airborne/subsystems/ins/ins_alt_float.c

@@ -105,6 +105,10 @@ void ins_update_baro() {
       UTM_COPY(utm, *stateGetPositionUtm_f());
       utm.alt = ins_alt;
       stateSetPositionUtm_f(&utm);
+      struct NedCoor_f ned_vel;
+      memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f));
+      ned_vel.z = -ins_alt_dot;
+      stateSetSpeedNed_f(&ned_vel);
     }
   }
 #endif
@@ -129,7 +133,7 @@ void ins_update_gps(void) {
   struct NedCoor_f ned_vel = {
     gps.ned_vel.x / 100.,
     gps.ned_vel.y / 100.,
-    gps.ned_vel.z / 100.
+    -ins_alt_dot
   };
   // set velocity
   stateSetSpeedNed_f(&ned_vel);

sw/airborne/subsystems/ins/ins_alt_float.h

@@ -33,7 +33,7 @@
 #include <inttypes.h>
 #include "std.h"
 #include "state.h"
-
+#include "generated/modules.h"
 
 #if USE_BAROMETER
 #ifdef BARO_MS5534A