vn100 module update the estimator

conf/modules/ins_vn100.xml

@@ -6,7 +6,7 @@
     <file name="ins_vn100.h"/>
   </header>
   <init fun="ins_init()"/>
-  <periodic fun="ins_periodic_task()" freq="4"/>
+  <periodic fun="ins_periodic_task()" freq="60"/>
   <periodic fun="ins_report_task()" freq="4"/>
   <event fun="ins_event_task()"/>
   <makefile>

sw/airborne/modules/ins/fw_ins_vn100.c

@@ -28,9 +28,19 @@
 
 #include "modules/ins/ins_vn100.h"
 #include "mcu_periph/spi.h"
+#include "estimator.h"
+#include "generated/airframe.h"
+
+#ifndef INS_YAW_NEUTRAL_DEFAULT
+#define INS_YAW_NEUTRAL_DEFAULT 0.
+#endif
 
 void ins_init( void ) {
 
+  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+  ins_yaw_neutral = INS_YAW_NEUTRAL_DEFAULT;
+
   /* SPI polarity = 1 - data sampled on rising edge */
   SpiSetCPOL();
   /* SPI phase = 1 - SCK idle high */
@@ -100,6 +110,11 @@ void ins_event_task( void ) {
   if (spi_message_received) {
     spi_message_received = FALSE;
     parse_ins_msg();
+#ifndef INS_VN100_READ_ONLY
+    // Update estimator
+    // FIXME Use a proper rotation matrix here
+    EstimatorSetAtt((ins_eulers.phi - ins_roll_neutral), ins_eulers.psi, (ins_eulers.theta - ins_pitch_neutral));
+#endif
     //uint8_t s = 4+VN100_REG_QMR_SIZE;
     //DOWNLINK_SEND_DEBUG(DefaultChannel,s,spi_buffer_input);
   }

sw/airborne/modules/ins/ins_vn100.c

@@ -34,6 +34,11 @@
 #include "downlink.h"
 #include "messages.h"
 
+/* neutrals */
+float ins_roll_neutral;
+float ins_pitch_neutral;
+float ins_yaw_neutral;
+
 struct FloatEulers ins_eulers;
 struct FloatQuat ins_quat;
 struct FloatRates ins_rates;
@@ -176,6 +181,9 @@ void parse_ins_msg( void ) {
 
 }
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
 #include "downlink.h"

sw/airborne/modules/ins/ins_vn100.h

@@ -35,6 +35,11 @@
 #include "VN100.h"
 #include "math/pprz_algebra_float.h"
 
+/* neutrals */
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+extern float ins_yaw_neutral;
+
 /* state */
 extern struct FloatEulers ins_eulers;
 extern struct FloatQuat ins_quat;