[paparazzi-commits] [5889] now building libeknav and paparazzi together

[antoine drouin]

Revision: 5889
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5889
Author:   poine
Date:     2010-09-16 08:19:27 +0000 (Thu, 16 Sep 2010)
Log Message:
-----------
now building libeknav and paparazzi together

Modified Paths:
--------------
    paparazzi3/trunk/conf/Makefile.omap
    paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml
    paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/fms/libeknav/assertions.hpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_1.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_3.cpp

Removed Paths:
-------------
    paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c

Modified: paparazzi3/trunk/conf/Makefile.omap
===================================================================
--- paparazzi3/trunk/conf/Makefile.omap 2010-09-15 23:43:45 UTC (rev 5888)
+++ paparazzi3/trunk/conf/Makefile.omap 2010-09-16 08:19:27 UTC (rev 5889)
@@ -62,8 +62,9 @@
 
 
 CXX = 
/opt/paparazzi/omap/overo-oe/tmp/sysroots/i686-linux/usr/armv7a/bin/arm-angstrom-linux-gnueabi-g++
-CXXFLAGS = -pipe -O3 -fshow-column -ffast-math -DEIGEN_DONT_VECTORIZE -fPIC
-CXXFLAGS += -DEIGEN_DONT_ALIGN -DNDEBUG -g -ffunction-sections -fdata-sections 
-DTIME_OPS
+CXXFLAGS = -pipe -O3 -fshow-column -ffast-math -fPIC
+CXXFLAGS += -g -ffunction-sections -fdata-sections 
+CXXFLAGS += -mfloat-abi=softfp -mtune=cortex-a8 -mfpu=vfp -march=armv7-a
 CXXFLAGS += -Wall -Wextra
 CXXFLAGS += $($(TARGET).CXXFLAGS)      
 
@@ -90,7 +91,7 @@
 %.elf:   $(OBJ_C_OMAP) $(OBJ_CPP_OMAP)
        @echo LD $@
        $(Q)if (expr "$($(TARGET).cpp_srcs)"); \
-          then $(CXX) $(CXXFLAGS) $(OBJ_CPP_OMAP) --output $@ $(LDFLAGS) 
$($(TARGET).LDFLAGS) ; \
+          then $(CXX) $(CXXFLAGS) $(OBJ_CPP_OMAP) $(OBJ_C_OMAP) --output $@ 
$(LDFLAGS) $($(TARGET).LDFLAGS); \
           else $(CC) $(CFLAGS) $(OBJ_C_OMAP) --output $@ $(LDFLAGS) 
$($(TARGET).LDFLAGS); fi
 
 # Compile: create object files from C source files

Modified: paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml     2010-09-15 
23:43:45 UTC (rev 5888)
+++ paparazzi3/trunk/conf/airframes/Poine/test_libeknav.xml     2010-09-16 
08:19:27 UTC (rev 5889)
@@ -5,11 +5,44 @@
 
     HOST=auto3
 
-    # test 1
+    LIB_EIGEN_DIR = 
/opt/paparazzi/omap/overo-oe/tmp/sysroots/armv7a-angstrom-linux-gnueabi/usr/include/eigen2
+    LIB_EIGEN_CFLAGS =  -I$(LIB_EIGEN_DIR) -DEIGEN_DONT_VECTORIZE  
-DEIGEN_DONT_ALIGN -DNDEBUG 
+
+    LIBEKNAV_SRCS = fms/libeknav/basic_ins_qkf.cpp           \
+                    fms/libeknav/ins_qkf_predict.cpp         \
+                    fms/libeknav/ins_qkf_observe_vector.cpp  \
+                    fms/libeknav/ins_qkf_observe_gps_pvt.cpp \
+                    fms/libeknav/ins_qkf_observe_gps_p.cpp
+#    LIBEKNAV_CFLAGS = -DTIME_OPS
+    LIBEKNAV_CFLAGS =
+
+    # test 1: how do I build cpp for using libeigen ?
     test1.ARCHDIR = omap
-    test1.CXXFLAGS += -I 
/opt/paparazzi/omap/overo-oe/tmp/sysroots/armv7a-angstrom-linux-gnueabi/usr/include/eigen2
+    test1.CXXFLAGS += $(LIB_EIGEN_CFLAGS)
     test1.cpp_srcs  = fms/libeknav/hello_world.cpp
 
+    # test 2: now build with libeknav
+    test2.ARCHDIR = omap
+    test2.CXXFLAGS += $(LIB_EIGEN_CFLAGS)
+    test2.cpp_srcs  = fms/libeknav/test_libeknav_2.cpp
+    test2.CXXFLAGS += $(LIBEKNAV_CFLAGS)
+    test2.cpp_srcs += $(LIBEKNAV_SRCS)
+
+    # test 3: now try to add Paparazzi's C
+    test3.ARCHDIR = omap
+    test3.CXXFLAGS += $(LIB_EIGEN_CFLAGS)
+    test3.CXXFLAGS += -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT)
+    test3.CXXFLAGS += -I$(PAPARAZZI_SRC)/sw/airborne
+    test3.CXXFLAGS += -I$(PAPARAZZI_SRC)/sw/include
+    test3.cpp_srcs  = fms/libeknav/test_libeknav_3.cpp
+    test3.CXXFLAGS += $(LIBEKNAV_CFLAGS)
+    test3.cpp_srcs += $(LIBEKNAV_SRCS)
+    test3.LDFLAGS  += -levent -lm
+    test3.CFLAGS   += -DFMS_PERIODIC_FREQ=512
+    test3.srcs     += fms/fms_periodic.c
+    test3.CXXFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp 
-DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown
+    test3.srcs     += fms/fms_spi_link.c
+
     # test network based telemetry on overo (using udp_transport2/messages2)
     overo_test_telemetry2.ARCHDIR  = omap
     overo_test_telemetry2.CFLAGS  += -I$(ACINCLUDE) -I. 
-I$(PAPARAZZI_HOME)/var/include

Added: paparazzi3/trunk/sw/airborne/fms/libeknav/assertions.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/assertions.hpp                    
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/assertions.hpp    2010-09-16 
08:19:27 UTC (rev 5889)
@@ -0,0 +1,65 @@
+/*
+ * assertions.hpp
+ *
+ *  Created on: Aug 6, 2009
+ *      Author: Jonathan Brandmeyer
+ *
+ *          This file is part of libeknav.
+ *
+ *  Libeknav 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, version 3.
+ *
+ *  Libeknav 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 libeknav.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef AHRS_ASSERTIONS_HPP
+#define AHRS_ASSERTIONS_HPP
+
+#include <Eigen/Core>
+#include <cmath>
+
+template<typename MatrixBase>
+bool hasNaN(const MatrixBase& expr);
+
+template<typename MatrixBase>
+bool hasInf(const MatrixBase& expr);
+
+template<typename MatrixBase>
+bool hasNaN(const MatrixBase& expr)
+{
+       for (int j = 0; j != expr.cols(); ++j) {
+               for (int i = 0; i != expr.rows(); ++i) {
+                       if (std::isnan(expr.coeff(i, j)))
+                               return true;
+               }
+       }
+       return false;
+}
+
+template<typename MatrixBase>
+bool hasInf(const MatrixBase& expr)
+{
+       for (int i = 0; i != expr.cols(); ++i) {
+               for (int j = 0; j != expr.rows(); ++j) {
+                       if (std::isinf(expr.coeff(j, i)))
+                               return true;
+               }
+       }
+       return false;
+}
+
+template<typename MatrixBase>
+bool isReal(const MatrixBase& exp)
+{
+       return !hasNaN(exp) && ! hasInf(exp);
+}
+
+#endif /* ASSERTIONS_HPP_ */

Deleted: paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c     2010-09-15 
23:43:45 UTC (rev 5888)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c     2010-09-16 
08:19:27 UTC (rev 5889)
@@ -1,12 +0,0 @@
-
-
-#include <stdio.h>
-
-
-int main(int argc, char** argv) {
-
-  printf("hello world\n");
-
-  return 0;
-}
-

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp       2010-09-15 
23:43:45 UTC (rev 5888)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp       2010-09-16 
08:19:27 UTC (rev 5889)
@@ -20,7 +20,7 @@
  *  along with libeknav.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "sigma_points.hpp"
+//#include "sigma_points.hpp"
 #include "quaternions.hpp"
 #include <Eigen/StdVector>
 

Added: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp         
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp 
2010-09-16 08:19:27 UTC (rev 5889)
@@ -0,0 +1,64 @@
+/*
+ * ins_qkf_observe_gps_p.cpp
+ *
+ *  Created on: Jun 10, 2010
+ *      Author: Jonathan Brandmeyer
+ *
+ *          This file is part of libeknav.
+ *
+ *  Libeknav 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, version 3.
+ *
+ *  Libeknav 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 libeknav.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "ins_qkf.hpp"
+#include "assertions.hpp"
+
+#define RANK_ONE_UPDATES
+
+#ifndef RANK_ONE_UPDATES
+#include "timer.hpp"
+#include <Eigen/LU>
+#endif
+
+using namespace Eigen;
+
+void
+basic_ins_qkf::obs_gps_p_report(const Vector3d& pos, const Vector3d& p_error)
+{
+       Matrix<double, 3, 1> residual = pos - avg_state.position;       
+       std::cout << "diff_p(" <<residual(0) << ", " << residual(1) << ", " << 
residual(2) <<")\n";
+
+
+#ifdef RANK_ONE_UPDATES
+       Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
+       for (int i = 0; i < 3; ++i) {
+               double innovation_cov_inv = 1.0/(cov(6+i, 6+i) + p_error[i]);
+               Matrix<double, 12, 1> gain = cov.block<12, 1>(0, 6+i) * 
innovation_cov_inv;
+               update += gain * (residual[i] - update[6+i]);
+               cov -= gain * cov.block<1, 12>(6+i, 0);
+       }
+
+#else
+       Matrix<double, 3, 3> innovation_cov = cov.block<3, 3>(6, 6);
+       innovation_cov += p_error.asDiagonal();
+
+       Matrix<double, 12, 3> kalman_gain = cov.block<12, 3>(0, 6)
+               * innovation_cov.part<Eigen::SelfAdjoint>().inverse();
+       Matrix<double, 12, 1> update = kalman_gain * residual;
+       cov.part<Eigen::SelfAdjoint>() -= kalman_gain * cov.block<3, 12>(6, 0);
+#endif
+       Quaterniond rotor = avg_state.apply_kalman_vec_update(update);
+       std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << 
"\t" << update.segment<6>(6).transpose() << ")\n";
+       counter_rotate_cov(rotor);
+       assert(is_real());
+}

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp       
2010-09-15 23:43:45 UTC (rev 5888)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp       
2010-09-16 08:19:27 UTC (rev 5889)
@@ -22,9 +22,9 @@
 #include "ins_qkf.hpp"
 #include "assertions.hpp"
 #include <Eigen/LU>
-#include "timer.hpp"
 
 #ifdef TIME_OPS
+#include "timer.hpp"
 #include <iostream>
 #endif
 

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp        
2010-09-15 23:43:45 UTC (rev 5888)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp        
2010-09-16 08:19:27 UTC (rev 5889)
@@ -21,15 +21,17 @@
 
 #include "ins_qkf.hpp"
 #include "assertions.hpp"
-#include "timer.hpp"
 
 #ifdef TIME_OPS
+#include "timer.hpp"
 #include <iostream>
 #endif
 
 using namespace Eigen;
 #define RANK_ONE_UPDATES
 
+#include <Eigen/LU>
+
 void
 basic_ins_qkf::obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error)
 {

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp       
2010-09-15 23:43:45 UTC (rev 5888)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp       
2010-09-16 08:19:27 UTC (rev 5889)
@@ -21,9 +21,9 @@
 
 #include "ins_qkf.hpp"
 #include "assertions.hpp"
-#include "timer.hpp"
 
 #ifdef TIME_OPS
+#include "timer.hpp"
 # include <iostream>
 #endif
 

Added: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_1.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_1.cpp               
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_1.cpp       
2010-09-16 08:19:27 UTC (rev 5889)
@@ -0,0 +1,21 @@
+
+
+#include <iostream>
+#include <iomanip>
+
+#include <Eigen/Core>
+
+// import most common Eigen types 
+USING_PART_OF_NAMESPACE_EIGEN
+
+int main(int, char *[]) {
+
+  std::cout << "hello world" << std::endl;
+
+  Matrix3f m3;
+  m3 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+
+  return 0;
+
+}
+

Added: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp               
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp       
2010-09-16 08:19:27 UTC (rev 5889)
@@ -0,0 +1,62 @@
+
+
+#include <iostream>
+#include <iomanip>
+
+#include <Eigen/Core>
+
+#include "ins_qkf.hpp"
+
+//#include "std.h"
+#define RadOfDeg(x) ((x) * (M_PI/180.))
+
+// import most common Eigen types 
+USING_PART_OF_NAMESPACE_EIGEN
+
+int main(int, char *[]) {
+
+  std::cout << "test libeknav 1" << std::endl;
+
+  /* initial state */
+  Vector3d pos_0_ecef(1017.67e3, -5079.282e3, 3709.041e3);
+  Vector3d speed_0_ecef(0., 0., 0.);
+  //  Vector3d orientation(0., 0., 0.);
+  Vector3d bias_0(0., 0., 0.);
+
+  /* initial covariance */
+  const double pos_cov_0 =  1e2*1e2;
+  const double speed_cov_0 =  3.*3.;
+  const double orientation_cov_0 =  RadOfDeg(5.)*RadOfDeg(5.);
+  const double bias_cov_0 =  0.447;
+  
+  /* system noise      */
+  const Vector3d gyro_white_noise = Vector3d::Ones()*0.1*0.1;
+  const Vector3d gyro_stability_noise = Vector3d::Ones()*0.00001;
+  const Vector3d accel_white_noise = Vector3d::Ones()* 0.04*0.04;
+
+  /* measurement noise */
+  const double mag_noise = std::pow(5 / 180.0 * M_PI, 2);
+  const Vector3d gps_pos_noise = Vector3d::Ones()  *10*10;
+  const Vector3d gps_speed_noise = Vector3d::Ones()*0.1*0.1;
+
+  /* sensors */
+  Vector3d gyro(0., 0., 0.);
+  Vector3d accelerometer(0., 0., 9.81);
+  Vector3d magnetometer = Vector3d::UnitZ();
+
+  basic_ins_qkf ins(pos_0_ecef, pos_cov_0, bias_cov_0, speed_cov_0,
+                   gyro_white_noise, gyro_stability_noise, accel_white_noise);
+
+  const double dt = 1./512.; /* predict at 512Hz */
+  for (int i=1; i<5000; i++) {
+    ins.predict(gyro, accelerometer, dt);
+    if (i % 10 == 0)  /* update mag at 50Hz */
+      ins.obs_vector(magnetometer, magnetometer, mag_noise);
+    if (i % 128 == 0) /* update gps at 4 Hz */
+      ins.obs_gps_pv_report(pos_0_ecef, speed_0_ecef, gps_pos_noise, 
gps_speed_noise);
+  }
+
+  return 0;
+
+}
+

Added: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_3.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_3.cpp               
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_3.cpp       
2010-09-16 08:19:27 UTC (rev 5889)
@@ -0,0 +1,130 @@
+
+#include <iostream>
+#include <iomanip>
+
+#include <Eigen/Core>
+
+#include "ins_qkf.hpp"
+
+
+#include <event.h>
+extern "C" {
+#include "std.h"
+#include "fms/fms_debug.h"
+#include "fms/fms_periodic.h"
+#include "fms/fms_spi_link.h"
+#include "fms/fms_autopilot_msg.h"
+#include "booz/booz_imu.h"
+  /* our sensors            */
+  struct BoozImuFloat imu;
+}
+
+static void main_init(void);
+static void main_periodic(int my_sig_num);
+static void main_dialog_with_io_proc(void);
+
+
+/* initial state */
+Vector3d pos_0_ecef(1017.67e3, -5079.282e3, 3709.041e3);
+Vector3d speed_0_ecef(0., 0., 0.);
+//  Vector3d orientation(0., 0., 0.);
+Vector3d bias_0(0., 0., 0.);
+
+/* initial covariance */
+const double pos_cov_0 =  1e2*1e2;
+const double speed_cov_0 =  3.*3.;
+//  const double orientation_cov_0 =  RadOfDeg(5.)*RadOfDeg(5.);
+const double bias_cov_0 =  0.447;
+
+/* system noise      */
+const Vector3d gyro_white_noise = Vector3d::Ones()*0.1*0.1;
+const Vector3d gyro_stability_noise = Vector3d::Ones()*0.00001;
+const Vector3d accel_white_noise = Vector3d::Ones()* 0.04*0.04;
+
+static basic_ins_qkf ins = basic_ins_qkf(pos_0_ecef, pos_cov_0, bias_cov_0, 
speed_cov_0,
+                                        gyro_white_noise, 
gyro_stability_noise, accel_white_noise);
+
+
+// import most common Eigen types 
+USING_PART_OF_NAMESPACE_EIGEN
+
+int main(int, char *[]) {
+
+  main_init();
+
+  std::cout << "test libeknav 1" << std::endl;
+
+  /* Enter our mainloop */
+  event_dispatch();
+  
+  TRACE(TRACE_DEBUG, "%s", "leaving mainloop... goodbye!\n");
+
+  return 0;
+
+}
+
+
+static void main_init(void) {
+
+  TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
+
+  /* Initalize our SPI link to IO processor */
+  if (spi_link_init()) {
+    TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
+    return;
+  }
+  
+  /* Initalize the event library */
+  event_init();
+  
+  /* Initalize our \xF4 so accurate periodic timer */
+  if (fms_periodic_init(main_periodic)) {
+    TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+    return; 
+  }
+ 
+}
+
+
+static void main_periodic(int my_sig_num __attribute__ ((unused))) {
+
+  static uint32_t cnt;
+  cnt++;
+
+  main_dialog_with_io_proc();
+
+  const double dt = 1./512.;
+  Vector3d gyro(0., 0., 0.);
+  Vector3d accelerometer(0., 0., 9.81);
+  ins.predict(gyro, accelerometer, dt);
+
+  if (cnt % 10 == 0) { /* update mag at 50Hz */
+    Vector3d magnetometer = Vector3d::UnitZ();
+    const double   mag_noise = std::pow(5 / 180.0 * M_PI, 2);
+    ins.obs_vector(magnetometer, magnetometer, mag_noise);
+  }
+  if (cnt % 128 == 0) /* update gps at 4 Hz */ {
+    const Vector3d gps_pos_noise = Vector3d::Ones()  *10*10;
+    const Vector3d gps_speed_noise = Vector3d::Ones()*0.1*0.1;
+    ins.obs_gps_pv_report(pos_0_ecef, speed_0_ecef, gps_pos_noise, 
gps_speed_noise);
+  }
+
+}
+
+
+static void main_dialog_with_io_proc() {
+
+  struct AutopilotMessageCRCFrame msg_in;
+  struct AutopilotMessageCRCFrame msg_out;
+  uint8_t crc_valid; 
+  
+  //  for (uint8_t i=0; i<6; i++) 
msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
+  
+  spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, 
&crc_valid);
+  
+  struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up; 
+  RATES_FLOAT_OF_BFP(imu.gyro, in->gyro);
+  ACCELS_FLOAT_OF_BFP(imu.accel, in->accel); 
+  MAGS_FLOAT_OF_BFP(imu.mag, in->mag); 
+
+}