moved booz to his own directory

sw/airborne/booz/ahrs_comp_filter.c

@@ -0,0 +1,117 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 "booz_ahrs.h"
+
+#include <math.h>
+
+#include "6dof.h"
+
+#define DT_PREDICT (1./250.)
+
+#define K1_accel 0.075
+#define K2_accel 0.00025
+#define K1_mag 0.1
+#define K2_mag 0.00025
+
+FLOAT_T comp_filter_int_phi;
+FLOAT_T comp_filter_int_theta;
+FLOAT_T comp_filter_int_psi;
+
+
+void booz_ahrs_init(void) {
+  booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
+  booz_ahrs_phi = 0.;
+  booz_ahrs_theta = 0.;
+  booz_ahrs_psi = 0.;
+
+  booz_ahrs_p = 0.;
+  booz_ahrs_q = 0.;
+  booz_ahrs_r = 0.;
+
+  booz_ahrs_bp = 0.;
+  booz_ahrs_bq = 0.;
+  booz_ahrs_br = 0.;
+  booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
+}
+
+void booz_ahrs_start(const float* accel, const float* gyro, const float* mag) {
+  booz_ahrs_p = 0.;
+  booz_ahrs_q = 0.;
+  booz_ahrs_r = 0.;
+  
+  booz_ahrs_bp = gyro[AXIS_P];
+  booz_ahrs_bq = gyro[AXIS_Q];
+  booz_ahrs_br = gyro[AXIS_R];
+
+  PhiOfAccel(booz_ahrs_phi, accel);
+  ThetaOfAccel(booz_ahrs_theta, accel);
+  PsiOfMag(booz_ahrs_psi, mag);
+
+  booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
+}
+
+void booz_ahrs_predict(const float* gyro ) {
+  /* unbias gyro */
+  booz_ahrs_p = gyro[AXIS_P] - booz_ahrs_bp;
+  booz_ahrs_q = gyro[AXIS_Q] - booz_ahrs_bq;
+  booz_ahrs_r = gyro[AXIS_R] - booz_ahrs_br; 
+  
+  /* update integrated angles */
+  float s_phi = sin(booz_ahrs_phi);
+  float c_phi = cos(booz_ahrs_phi);
+  float t_theta = tan(booz_ahrs_theta);
+  float c_theta = cos(booz_ahrs_theta);
+  float phi_dot = booz_ahrs_p + s_phi*t_theta*booz_ahrs_q + c_phi*t_theta*booz_ahrs_r;
+  float theta_dot = c_phi*booz_ahrs_q - s_phi*booz_ahrs_r;
+  float psi_dot = s_phi/c_theta*booz_ahrs_q + c_phi/c_theta*booz_ahrs_r;
+
+  comp_filter_int_phi = booz_ahrs_phi + phi_dot * DT_PREDICT;
+  comp_filter_int_theta = booz_ahrs_theta + theta_dot * DT_PREDICT;
+  comp_filter_int_psi = booz_ahrs_psi + psi_dot * DT_PREDICT;
+  
+}
+
+void booz_ahrs_update_accel( const float* accel) {
+  PhiOfAccel(booz_ahrs_measure_phi, accel);
+  ThetaOfAccel(booz_ahrs_measure_theta, accel);
+
+  float err_phi = booz_ahrs_measure_phi - comp_filter_int_phi;
+  float err_theta = booz_ahrs_measure_theta - comp_filter_int_theta;
+  booz_ahrs_phi = comp_filter_int_phi + err_phi * K1_accel;
+  booz_ahrs_theta = comp_filter_int_theta + err_theta * K1_accel;
+  
+  booz_ahrs_bp -= err_phi * K2_accel;
+  booz_ahrs_bq -= err_theta * K2_accel;
+
+}
+
+void booz_ahrs_update_mag( const float* mag ) {
+  PsiOfMag(booz_ahrs_measure_psi, mag);
+  float err_psi = booz_ahrs_measure_psi - comp_filter_int_psi;
+  
+  booz_ahrs_psi = comp_filter_int_psi + err_psi * K1_mag;
+  booz_ahrs_br -= err_psi * K2_mag;
+}
+

sw/airborne/booz/ahrs_comp_filter.h

@@ -0,0 +1,32 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 AHRS_COMP_FLT_H
+#define AHRS_COMP_FLT_H
+
+extern FLOAT_T comp_filter_int_phi;
+extern FLOAT_T comp_filter_int_theta;
+extern FLOAT_T comp_filter_int_psi;
+
+#endif /* AHRS_COMP_FLT_H */

sw/airborne/booz/ahrs_multitilt.c

@@ -0,0 +1,162 @@
+#include "booz_ahrs.h"
+
+#include <string.h>
+#include <math.h>
+
+#include "6dof.h"
+
+#define DT 4e-3
+
+/* covariance matrix */
+float mtt_P_phi[2][2];
+float mtt_P_theta[2][2];
+float mtt_P_psi[2][2];
+
+/* process covariance noise */
+//static const float Q_angle = 0.0005;
+static const float Q_angle = 0.00025;
+static const float Q_bias  = 0.0005;
+/* Measurement covariance */
+//static const float R_accel = 0.3;
+static const float R_accel = 0.5;
+
+static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias);
+static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]);
+
+void booz_ahrs_init(void) {
+  booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
+  booz_ahrs_phi = 0.;
+  booz_ahrs_theta = 0.;
+  booz_ahrs_psi = 0.;
+
+  booz_ahrs_p = 0.;
+  booz_ahrs_q = 0.;
+  booz_ahrs_r = 0.;
+
+  booz_ahrs_bp = 0.;
+  booz_ahrs_bq = 0.;
+  booz_ahrs_br = 0.;
+}
+
+void booz_ahrs_start(const float* accel, const float* gyro, const float* mag) {
+  /* reset covariance matrices */
+  const float cov_init[2][2] = {{1., 0.},
+				{0., 1.}};
+  memcpy(mtt_P_phi, cov_init, sizeof(cov_init));
+  memcpy(mtt_P_theta, cov_init, sizeof(cov_init));
+  memcpy(mtt_P_psi, cov_init, sizeof(cov_init));
+
+  /* initialise state */
+  booz_ahrs_p = 0.;
+  booz_ahrs_q = 0.;
+  booz_ahrs_r = 0.;
+  
+  booz_ahrs_bp = gyro[AXIS_P];
+  booz_ahrs_bq = gyro[AXIS_Q];
+  booz_ahrs_br = gyro[AXIS_R];
+
+  PhiOfAccel(booz_ahrs_phi, accel);
+  ThetaOfAccel(booz_ahrs_theta, accel);
+#ifndef DISABLE_MAGNETOMETER
+  PsiOfMag(booz_ahrs_psi, mag);
+#endif
+
+  booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
+}
+
+static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]) {
+
+  (*angle) += angle_dot * DT;
+
+  const float P_dot00 = Q_angle - P[0][1] - P[1][0];
+  const float P_dot01 = - P[1][1];
+  const float P_dot10 = - P[1][1];
+  const float P_dot11 = Q_bias;
+
+  /* P += Pdot * dt */
+  P[0][0] += P_dot00 * DT;
+  P[0][1] += P_dot01 * DT;
+  P[1][0] += P_dot10 * DT;
+  P[1][1] += P_dot11 * DT;
+
+}
+
+
+void booz_ahrs_predict(const float* gyro ) {
+  /* unbias gyro */
+  booz_ahrs_p = gyro[AXIS_P] - booz_ahrs_bp;
+  booz_ahrs_q = gyro[AXIS_Q] - booz_ahrs_bq;
+  booz_ahrs_r = gyro[AXIS_R] - booz_ahrs_br; 
+
+  /* update angles */
+  float s_phi = sin(booz_ahrs_phi);
+  float c_phi = cos(booz_ahrs_phi);
+  float t_theta = tan(booz_ahrs_theta);
+
+  float phi_dot = booz_ahrs_p + s_phi*t_theta*booz_ahrs_q + c_phi*t_theta*booz_ahrs_r;
+  float theta_dot = c_phi*booz_ahrs_q - s_phi*booz_ahrs_r;
+  mtt_predict_axis(&booz_ahrs_phi, phi_dot, mtt_P_phi);
+  mtt_predict_axis(&booz_ahrs_theta, theta_dot, mtt_P_theta);
+
+#ifndef DISABLE_MAGNETOMETER
+  float c_theta = cos(booz_ahrs_theta);
+  float psi_dot = s_phi/c_theta*booz_ahrs_q + c_phi/c_theta*booz_ahrs_r;
+  mtt_predict_axis(&booz_ahrs_psi, psi_dot, mtt_P_psi);
+#endif
+
+
+}
+
+static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias) {
+  const float Pct_0 = _P[0][0];
+  const float Pct_1 = _P[1][0];
+  /* E = C P C' + R */
+  const float E = R_accel + Pct_0;
+  /* K = P C' inv(E) */
+  const float K_0 = Pct_0 / E;
+  const float K_1 = Pct_1 / E;
+  /* P = P - K C P */
+  const float t_0 = Pct_0;
+  const float t_1 = _P[0][1];
+
+  _P[0][0] -= K_0 * t_0;
+  _P[0][1] -= K_0 * t_1;
+  _P[1][0] -= K_1 * t_0;
+  _P[1][1] -= K_1 * t_1;
+    
+  /*  X += K * err */
+  (*angle) += K_0 * _err;
+  (*bias)  += K_1 * _err;
+
+}
+
+void booz_ahrs_update_accel( const float* accel) {
+
+  PhiOfAccel(booz_ahrs_measure_phi, accel);
+  float err_phi = booz_ahrs_measure_phi - booz_ahrs_phi;
+  WRAP(err_phi, M_PI);
+  mtt_update_axis(err_phi, mtt_P_phi, &booz_ahrs_phi, &booz_ahrs_bp);
+  WRAP(booz_ahrs_phi, M_PI);
+
+  ThetaOfAccel(booz_ahrs_measure_theta, accel);
+  float err_theta = booz_ahrs_measure_theta - booz_ahrs_theta;
+  WRAP(err_theta, M_PI_2);
+  mtt_update_axis(err_theta, mtt_P_theta, &booz_ahrs_theta, &booz_ahrs_bq);
+  WRAP(booz_ahrs_theta, M_PI_2);
+
+}
+
+void booz_ahrs_update_mag( const float* mag ) {
+
+#ifndef DISABLE_MAGNETOMETER
+  PsiOfMag(booz_ahrs_measure_psi, mag);
+  float err_psi = booz_ahrs_measure_psi - booz_ahrs_psi;
+  WRAP(err_psi, M_PI);
+  mtt_update_axis(err_psi, mtt_P_psi, &booz_ahrs_psi, &booz_ahrs_br);
+  WRAP(booz_ahrs_psi, M_PI);
+#endif
+
+}
+
+
+

sw/airborne/booz/ahrs_multitilt.h

@@ -0,0 +1,38 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 AHRS_MULTITILT_H
+#define AHRS_MULTITILT_H
+
+#include "std.h"
+
+
+extern float mtt_P_phi[2][2];
+
+extern float mtt_P_theta[2][2];
+
+extern float mtt_P_psi[2][2];
+
+
+#endif /* AHRS_MULTITILT_H */

sw/airborne/booz/ahrs_quat_fast_ekf.h

@@ -0,0 +1,23 @@
+#ifndef AHRS_QUAT_FAST_EKF_H
+#define AHRS_QUAT_FAST_EKF_H
+
+#include <inttypes.h>
+#include "6dof.h"
+
+/* ekf state : quaternion and gyro biases */
+extern FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
+//extern FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
+/* we maintain unbiased rates */
+//extern FLOAT_T afe_p, afe_q, afe_r;
+/* we maintain eulers angles */
+//extern FLOAT_T afe_phi, afe_theta, afe_psi;
+
+extern FLOAT_T afe_P[7][7]; /* covariance */
+
+extern void afe_init( const int16_t *mag, const FLOAT_T* accel, const FLOAT_T* gyro );
+extern void afe_predict( const FLOAT_T* gyro );
+extern void afe_update_phi( const FLOAT_T* accel);
+extern void afe_update_theta( const FLOAT_T* accel);
+extern void afe_update_psi( const int16_t* mag);
+
+#endif /* AHRS_QUAT_FAST_EKF_H_H */

sw/airborne/booz/ahrs_quat_fast_ekf_utils.h

@@ -0,0 +1,172 @@
+#ifndef AHRS_QUAT_FAST_EKF_UTILS_H
+#define AHRS_QUAT__FAST_EKF_UTILS_H
+
+#include "ahrs_quat_fast_ekf.h"
+
+/*
+ * Compute the five elements of the DCM that we use for our
+ * rotations and Jacobians.  This is used by several other functions
+ * to speedup their computations.
+ */
+#define AFE_DCM_OF_QUAT() {				\
+    afe_dcm00 = 1.0-2*(afe_q2*afe_q2 + afe_q3*afe_q3);	\
+    afe_dcm01 =     2*(afe_q1*afe_q2 + afe_q0*afe_q3);	\
+    afe_dcm02 =     2*(afe_q1*afe_q3 - afe_q0*afe_q2);	\
+    afe_dcm12 =     2*(afe_q2*afe_q3 + afe_q0*afe_q1);	\
+    afe_dcm22 = 1.0-2*(afe_q1*afe_q1 + afe_q2*afe_q2);	\
+  }
+/*
+ * Compute Euler angles from our DCM.
+ */
+#define AFE_PHI_OF_DCM()   { booz_ahrs_phi = atan2( afe_dcm12, afe_dcm22 );}
+#define AFE_THETA_OF_DCM() { booz_ahrs_theta = -asin( afe_dcm02 );}
+#define AFE_PSI_OF_DCM()   { booz_ahrs_psi = atan2( afe_dcm01, afe_dcm00 );}
+#define AFE_EULER_OF_DCM() { AFE_PHI_OF_DCM(); AFE_THETA_OF_DCM(); AFE_PSI_OF_DCM()}
+
+/*
+ * initialise the quaternion from the set of eulers
+ */
+#define AFE_QUAT_OF_EULER() {						\
+    const FLOAT_T phi2     = booz_ahrs_phi / 2.0;			\
+    const FLOAT_T theta2   = booz_ahrs_theta / 2.0;			\
+    const FLOAT_T psi2     = booz_ahrs_psi / 2.0;			\
+    									\
+    const FLOAT_T sinphi2   = sin( phi2 );				\
+    const FLOAT_T cosphi2   = cos( phi2 );				\
+    									\
+    const FLOAT_T sintheta2 = sin( theta2 );				\
+    const FLOAT_T costheta2 = cos( theta2 );				\
+    									\
+    const FLOAT_T sinpsi2   = sin( psi2 );				\
+    const FLOAT_T cospsi2   = cos( psi2 );				\
+    									\
+    afe_q0 =  cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; \
+    afe_q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; \
+    afe_q2 =  cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; \
+    afe_q3 =  cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; \
+  }
+
+/*
+ * normalize quaternion
+ */
+#define AFE_NORM_QUAT() {			\
+    FLOAT_T  mag = afe_q0*afe_q0 + afe_q1*afe_q1	\
+      + afe_q2*afe_q2 + afe_q3*afe_q3;		\
+    mag = sqrt( mag );				\
+    afe_q0 /= mag;				\
+    afe_q1 /= mag;				\
+    afe_q2 /= mag;				\
+    afe_q3 /= mag;				\
+  }
+
+
+
+
+/*
+ * Compute the Jacobian of the measurements to the system states.
+ */
+#define AFE_COMPUTE_H_PHI() {          						\
+    const FLOAT_T phi_err =  2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12);     \
+    afe_H[0] = (afe_q1 * afe_dcm22) * phi_err;				        \
+    afe_H[1] = (afe_q0 * afe_dcm22 + 2 * afe_q1 * afe_dcm12) * phi_err;	        \
+    afe_H[2] = (afe_q3 * afe_dcm22 + 2 * afe_q2 * afe_dcm12) * phi_err;	        \
+    afe_H[3] = (afe_q2 * afe_dcm22) * phi_err;				        \
+}
+
+#define AFE_COMPUTE_H_THETA() {						        \
+    const FLOAT_T theta_err  = -2 / sqrt( 1 - afe_dcm02*afe_dcm02 );	        \
+    afe_H[0] = -afe_q2 * theta_err;						\
+    afe_H[1] =  afe_q3 * theta_err;						\
+    afe_H[2] = -afe_q0 * theta_err;						\
+    afe_H[3] =  afe_q1 * theta_err;						\
+  }
+
+#define AFE_COMPUTE_H_PSI() {					                \
+  const FLOAT_T psi_err =  2 / (afe_dcm00*afe_dcm00 + afe_dcm01*afe_dcm01);       \
+  afe_H[0] = (afe_q3 * afe_dcm00) * psi_err;					\
+  afe_H[1] = (afe_q2 * afe_dcm00) * psi_err;					\
+  afe_H[2] = (afe_q1 * afe_dcm00 + 2 * afe_q2 * afe_dcm01) * psi_err;	        \
+  afe_H[3] = (afe_q0 * afe_dcm00 + 2 * afe_q3 * afe_dcm01) * psi_err;	        \
+  }
+
+
+/* convert sensor reading into euler angle measurement */
+static inline FLOAT_T afe_phi_of_accel( const FLOAT_T* accel ) {
+  return atan2(accel[AXIS_Y], accel[AXIS_Z]);
+}
+
+static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
+  FLOAT_T g2 =					
+    accel[AXIS_X]*accel[AXIS_X] +	
+    accel[AXIS_Y]*accel[AXIS_Y] +	
+    accel[AXIS_Z]*accel[AXIS_Z];
+  return -asin( accel[AXIS_X] / sqrt( g2 ) );     
+}
+/*
+ * The rotation matrix to rotate from NED frame to body frame without
+ * rotating in the yaw axis is:
+ *
+ * [ 1      0         0    ] [ cos(Theta)  0  -sin(Theta) ]
+ * [ 0  cos(Phi)  sin(Phi) ] [      0      1       0      ]
+ * [ 0 -sin(Phi)  cos(Phi) ] [ sin(Theta)  0   cos(Theta) ]
+ *
+ * This expands to:
+ *
+ * [  cos(Theta)              0      -sin(Theta)         ]
+ * [  sin(Phi)*sin(Theta)  cos(Phi)   sin(Phi)*cos(Theta)]
+ * [  cos(Phi)*sin(Theta)  -sin(Phi)  cos(Phi)*cos(Theta)]
+ *
+ * However, to untilt the compass reading, we need to use the 
+ * transpose of this matrix.
+ *
+ * [  cos(Theta)  sin(Phi)*sin(Theta)  cos(Phi)*sin(Theta) ]
+ * [      0       cos(Phi)            -sin(Phi)            ]
+ * [ -sin(Theta)  sin(Phi)*cos(Theta)  cos(Phi)*cos(Theta) ]
+ *
+ * Additionally,
+ * since we already have the DCM computed for our current attitude,
+ * we can short cut all of the trig. substituting
+ * in from the definition of euler2quat and quat2euler, we have:
+ *
+ * [ cos(Theta)   -dcm12*dcm02   -dcm22*dcm02 ]
+ * [    0          dcm22         -dcm12       ]
+ * [ dcm02         dcm12          dcm22       ]
+ *
+ */
+static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) {
+  const FLOAT_T ctheta  = cos( booz_ahrs_theta );
+#if 0
+  const FLOAT_T    mn = ctheta * mag[0] 
+    - (afe_dcm12 * mag[1] + afe_dcm22 * mag[2]) * afe_dcm02 / ctheta;
+  
+  const FLOAT_T    me =
+    (afe_dcm22 * mag[1] - afe_dcm12 * mag[2]) / ctheta;
+#else
+  const FLOAT_T stheta  = sin( booz_ahrs_theta );
+  const FLOAT_T cphi  = cos( booz_ahrs_phi );
+  const FLOAT_T sphi  = sin( booz_ahrs_phi );
+
+  const FLOAT_T mn = 
+    ctheta*      mag[0]+
+    sphi*stheta* mag[1]+
+    cphi*stheta* mag[2];
+  const FLOAT_T me = 
+    0*     mag[0]+
+    cphi*  mag[1]+
+    -sphi* mag[2];
+#endif
+  
+  const FLOAT_T psi = -atan2( me, mn );
+  return psi;
+}
+
+#define AFE_WARP(x, b) {			\
+    while( x < -b )				\
+      x += 2 * b;				\
+    while( x > b )				\
+      x -= 2 * b;				\
+  }
+
+
+
+#endif /* AHRS_QUAT_FAST_EKF_UTILS_H */

sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.c

@@ -0,0 +1,18 @@
+#include "actuators.h"
+
+uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
+volatile bool_t  buss_twi_blmc_status;
+volatile uint8_t buss_twi_blmc_nb_err;
+volatile bool_t  buss_twi_blmc_i2c_done;
+volatile uint8_t buss_twi_blmc_idx;
+
+const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = { 0x52, 0x54, 0x56, 0x58 };
+
+void actuators_init ( void ) {
+  uint8_t i;
+  for (i=0; i<BUSS_TWI_BLMC_NB;i++)
+    buss_twi_blmc_motor_power[i] = 0;
+  buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
+  buss_twi_blmc_nb_err = 0;
+  buss_twi_blmc_i2c_done = TRUE;
+}

sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.h

@@ -0,0 +1,47 @@
+#ifndef ACTUATORS_BUSS_TWI_BLMC_HW_H
+#define ACTUATORS_BUSS_TWI_BLMC_HW_H
+
+#include <string.h>
+#include "std.h"
+#include "i2c.h"
+
+#define ChopServo(x,a,b) ((x)>(b)?(b):(x))
+#define Actuator(i) buss_twi_blmc_motor_power[i]
+#define ActuatorsCommit() {			                \
+    if ( buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) {	\
+      buss_twi_blmc_idx = 0;					\
+      buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY;		\
+      ActuatorsBussTwiBlmcSend();				\
+    }								\
+    else							\
+      buss_twi_blmc_nb_err++;					\
+  }
+
+#define SERVOS_TICS_OF_USEC(s) ((uint8_t)(s)) 
+
+#define BUSS_TWI_BLMC_STATUS_IDLE 0
+#define BUSS_TWI_BLMC_STATUS_BUSY 1
+#define BUSS_TWI_BLMC_NB 4
+extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
+extern volatile bool_t  buss_twi_blmc_status;
+extern volatile uint8_t buss_twi_blmc_nb_err;
+extern volatile bool_t  buss_twi_blmc_i2c_done;
+extern volatile uint8_t buss_twi_blmc_idx;
+extern const uint8_t buss_twi_blmc_addr[];
+
+#define ActuatorsBussTwiBlmcNext() {			\
+    buss_twi_blmc_idx++;				\
+    if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB) {		\
+      ActuatorsBussTwiBlmcSend();			\
+    }							\
+    else						\
+      buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;	\
+  }
+
+#define ActuatorsBussTwiBlmcSend() {					             \
+    i2c_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx];		             \
+    i2c_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); \
+  }
+
+
+#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */

sw/airborne/booz/arm7/booz_imu_hw.c

@@ -0,0 +1,105 @@
+#include "booz_imu.h"
+
+/* SSPCR0 settings */
+#define SSP_DDS  0x07 << 0  /* data size         : 8 bits        */
+#define SSP_FRF  0x00 << 4  /* frame format      : SPI           */
+#define SSP_CPOL 0x00 << 6  /* clock polarity    : data captured on first clock transition */  
+#define SSP_CPHA 0x00 << 7  /* clock phase       : SCK idles low */
+#define SSP_SCR  0x0F << 8  /* serial clock rate : divide by 16  */
+
+/* SSPCR1 settings */
+#define SSP_LBM  0x00 << 0  /* loopback mode     : disabled                  */
+#define SSP_SSE  0x00 << 1  /* SSP enable        : disabled                  */
+#define SSP_MS   0x00 << 2  /* master slave mode : master                    */
+#define SSP_SOD  0x00 << 3  /* slave output disable : don't care when master */
+
+static void SPI1_ISR(void) __attribute__((naked));
+
+void booz_imu_hw_init(void) {
+
+  /* setup pins for SSP (SCK, MISO, MOSI) */
+  PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
+  
+  /* setup SSP */
+  SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
+  SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
+  SSPCPSR = 0x2;
+  
+  /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT(VIC_SPI1);   // SPI1 selected as IRQ
+  VICIntEnable = VIC_BIT(VIC_SPI1);     // SPI1 interrupt enabled
+  VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
+  VICVectAddr7 = (uint32_t)SPI1_ISR;    // address of the ISR 
+
+}
+
+
+static inline bool_t isr_try_mag(void) {
+  switch (micromag_status) {
+  case MM_IDLE :
+    MmSendReq();
+    return TRUE;
+  case MM_GOT_EOC:
+    MmReadRes();
+    return TRUE;
+  }
+  return FALSE;
+}
+
+static inline bool_t isr_try_baro(void) {
+#ifndef SCP1000_NO_EINT
+  switch (scp1000_status) { 
+  case SCP1000_STA_STOPPED:
+    Scp1000SendConfig();
+    return TRUE;    
+  case SCP1000_STA_GOT_EOC:
+    Scp1000Read();
+    return TRUE;
+  }
+#else
+  if (scp1000_status == SCP1000_STA_STOPPED) {
+    Scp1000SendConfig();
+    return TRUE;
+  }
+  else if (scp1000_status == SCP1000_STA_WAIT_EOC && Scp1000DataReady()) {
+    scp1000_status = SCP1000_STA_GOT_EOC;
+    Scp1000Read();
+    return TRUE;
+  }
+#endif
+  return FALSE; 
+}
+
+static void SPI1_ISR(void) {
+ ISR_ENTRY();
+
+ switch (booz_imu_status) {
+ 
+ case BOOZ_IMU_STA_MEASURING_GYRO:
+   Max1167OnSpiInt();
+   /* we now have a gyro reading */
+   if (isr_try_mag())
+     booz_imu_status = BOOZ_IMU_STA_MEASURING_MAG;
+   else if (isr_try_baro())
+     booz_imu_status = BOOZ_IMU_STA_MEASURING_BARO;
+   else
+     booz_imu_status = BOOZ_IMU_STA_IDLE;
+   break;
+
+ case BOOZ_IMU_STA_MEASURING_MAG:
+   MmOnSpiIt();
+   if (isr_try_baro())
+     booz_imu_status = BOOZ_IMU_STA_MEASURING_BARO;
+   else
+     booz_imu_status = BOOZ_IMU_STA_IDLE;
+   break;
+   
+ case BOOZ_IMU_STA_MEASURING_BARO:
+   Scp1000OnSpiIt();
+   booz_imu_status = BOOZ_IMU_STA_IDLE;
+   break;
+ }
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}

sw/airborne/booz/arm7/booz_imu_hw.h

@@ -0,0 +1,30 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 BOOZ_IMU_HW_H
+#define BOOZ_IMU_HW_H
+
+
+
+#endif /* BOOZ_IMU_HW_H */

sw/airborne/booz/arm7/booz_link_mcu_hw.c

@@ -0,0 +1,261 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_link_mcu.h"
+
+
+uint8_t* link_mcu_tx_buf;
+uint8_t* link_mcu_rx_buf;
+
+
+
+
+
+#ifdef BOOZ_FILTER_MCU
+
+volatile uint8_t link_mcu_status;
+volatile uint8_t link_mcu_buf_idx;
+
+uint32_t it_count;
+uint16_t foo_debug;
+
+
+/*
+  wiring on IMU_V3
+  P0_4   SCK0
+  P0_5   MISO0
+  P0_6   MOSI0
+  P0_7   SSEL0
+  P1_24  DRDY
+*/
+
+#include "armVIC.h"
+
+
+void SPI0_ISR(void) __attribute__((naked));
+
+
+/*  */
+#define S0SPCR_BitEnable 0 << 2 /* enable more than 8 bits transferts */
+#define S0SPCR_CPHA      1 << 3 /* data sampled on first edge         */
+#define S0SPCR_CPOL      0 << 4 /* clock idles high                   */
+#define S0SPCR_MSTR      1 << 5 /* master mode                        */
+#define S0SPCR_LSBF      0 << 6 /* MSB first                          */
+#define S0SPCR_SPIE      0 << 7 /* SPI interrupt disabled             */
+#define S0SPCR_BITS      0 << 8 /* 16 bits transferts                 */
+
+#define S0SPCR_VAL ( S0SPCR_BitEnable |		\
+		     S0SPCR_CPHA |		\
+		     S0SPCR_CPOL |		\
+		     S0SPCR_MSTR |		\
+		     S0SPCR_LSBF |		\
+		     S0SPCR_SPIE |		\
+		     S0SPCR_BITS )
+
+
+
+#define SPI0_EnableInterrupt()  SetBit(S0SPCR, 7)
+#define SPI0_ClearInterrupt()   SetBit(S0SPINT, SPI0IF)
+
+void booz_link_mcu_hw_init ( void ) {
+
+  /* SS pin is output */
+  SetBit(IO0DIR, SS_PIN);
+  SPI0_UnselectSlave();
+
+  /* init SPI0 */
+  /* setup pins for sck, miso, mosi */
+  PINSEL0 |= 1<<8 | 1<<10 | 1<<12;
+  /* configure SPI : see above */
+  S0SPCR = S0SPCR_VAL;
+  /* setup SPI clock rate ~ 450Khz with 15MHz VPB */
+  S0SPCCR = 0x40;
+
+  /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT(VIC_SPI0);   // SPI0 selected as IRQ
+  VICIntEnable = VIC_BIT(VIC_SPI0);     // SPI0 interrupt enabled
+  VICVectCntl3 = VIC_ENABLE | VIC_SPI0;
+  VICVectAddr3 = (uint32_t)SPI0_ISR;    // address of the ISR
+
+  /* clear all potentially pending interrupts */
+  uint16_t foo1 __attribute__((unused)) = S0SPSR;
+  uint16_t foo2 __attribute__((unused)) = S0SPDR;
+  SPI0_ClearInterrupt();
+  SPI0_EnableInterrupt();
+
+  link_mcu_status = LINK_MCU_STATUS_IDLE;
+  link_mcu_buf_idx = 0;
+
+  link_mcu_tx_buf = (uint8_t*)&inter_mcu_state;
+  link_mcu_rx_buf = (uint8_t*)&booz_link_mcu_state_unused;
+
+}
+
+
+void SPI0_ISR(void) {
+  ISR_ENTRY();
+  /* read status register */
+  uint16_t foo __attribute__((unused)) = S0SPSR;
+  foo_debug = foo;
+  it_count++;
+
+  /* read_data_register   */
+  link_mcu_rx_buf[link_mcu_buf_idx] = S0SPDR;
+  link_mcu_buf_idx++;
+  if (link_mcu_buf_idx < LINK_MCU_BUF_LEN) {
+    S0SPDR = link_mcu_tx_buf[link_mcu_buf_idx];
+  }
+  else {
+    SPI0_UnselectSlave();
+    link_mcu_status = LINK_MCU_STATUS_IDLE;
+  }
+
+  SPI0_ClearInterrupt();
+  /* clear this interrupt from the VIC */
+  VICVectAddr = 0x00000000;
+  ISR_EXIT();
+}
+
+#endif /* BOOZ_FILTER_MCU */
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#ifdef BOOZ_CONTROLLER_MCU  
+
+/* 
+   IMU connected to SSP ( aka SPI1 ) as master
+   Controller is slave
+*/
+
+volatile uint32_t it_cnt;
+volatile uint32_t rx_it_cnt;
+volatile uint32_t ti_it_cnt;
+
+#include "LPC21xx.h"
+#include "interrupt_hw.h" 
+
+volatile uint8_t link_mcu_rx_buf_idx;
+volatile uint8_t link_mcu_tx_buf_idx;
+
+#define LinkMcuReceive() {	         				\
+    while (bit_is_set(SSPSR, RNE)) {					\
+      if (link_mcu_rx_buf_idx < LINK_MCU_BUF_LEN) {			\
+	link_mcu_rx_buf[link_mcu_rx_buf_idx] = SSPDR;			\
+	link_mcu_rx_buf_idx++;						\
+	if (link_mcu_rx_buf_idx == LINK_MCU_BUF_LEN)			\
+	  booz_link_mcu_status = BOOZ_LINK_MCU_DATA_AVAILABLE;		\
+      }                                                                 \
+      else {                                                            \
+	uint8_t foo __attribute__ ((unused));				\
+	foo = SSPDR;							\
+      }                                                                 \
+    }									\
+  }
+
+
+static void SSP_ISR(void) __attribute__((naked));
+
+/* SSPCR0 settings */
+#define SSP_DDS  0x07 << 0  /* data size         : 8 bits                    */
+#define SSP_FRF  0x00 << 4  /* frame format      : SPI                       */
+#define SSP_CPOL 0x00 << 6  /* clock polarity    : first clock transition    */
+#define SSP_CPHA 0x01 << 7  /* clock phase       : SCK idles high            */
+#define SSP_SCR  0x01 << 8  /* serial clock rate : divide by 2               */
+
+/* SSPCR1 settings */
+#define SSP_LBM  0x00 << 0  /* loopback mode     : disabled                  */
+#define SSP_SSE  0x00 << 1  /* SSP enable        : disabled                  */
+#define SSP_MS   0x01 << 2  /* master slave mode : slave                     */
+#define SSP_SOD  0x00 << 3  /* slave output disable : no                     */
+
+#define SSPCR0_VAL (SSP_DDS |  SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
+#define SSPCR1_VAL (SSP_LBM |  SSP_SSE | SSP_MS | SSP_SOD )
+
+#define SSP_PINSEL1_SCK  (2<<2)
+#define SSP_PINSEL1_MISO (2<<4)
+#define SSP_PINSEL1_MOSI (2<<6)
+#define SSP_PINSEL1_SSEL (2<<8)
+
+void booz_link_mcu_hw_init ( void ) {
+
+  /* setup pins for SSP (SCK, MISO, MOSI, SSEL) */
+  PINSEL1 |= SSP_PINSEL1_SCK  | SSP_PINSEL1_MISO | 
+             SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL ;
+
+  /* setup SSP */
+  SSPCR0 = SSPCR0_VAL;;
+  SSPCR1 = SSPCR1_VAL;
+  SSPCPSR = 0x10;
+  
+  /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT( VIC_SPI1 );  /* SPI1 selected as IRQ */
+  VICIntEnable = VIC_BIT( VIC_SPI1 );    /* enable it            */
+  VICVectCntl9 = VIC_ENABLE | VIC_SPI1;
+  VICVectAddr9 = (uint32_t)SSP_ISR;     /* address of the ISR   */
+
+  link_mcu_rx_buf  = (uint8_t*)&inter_mcu_state;
+  link_mcu_tx_buf = (uint8_t*)&booz_link_mcu_state_unused;
+
+  BoozLinkMcuHwRestart();
+}
+
+static void SSP_ISR(void) {
+ ISR_ENTRY();
+
+ it_cnt++;
+
+ /*  Rx half full  */
+ if (bit_is_set(SSPMIS, RXMIS)) { 
+   rx_it_cnt++;
+   /* LinkMcuTransmit(); */
+   LinkMcuReceive();
+   SSP_EnableRti();
+ }
+
+ /* Rx timeout     */ 
+ if ( bit_is_set(SSPMIS, RTMIS)) {
+   ti_it_cnt++;
+   LinkMcuReceive();
+   SSP_ClearRti();
+   SSP_DisableRti();
+   SSP_Disable();
+   booz_link_mcu_status = BOOZ_LINK_MCU_DATA_AVAILABLE;
+ }
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
+
+#endif /* BOOZ_CONTROLLER_MCU */

sw/airborne/booz/arm7/booz_link_mcu_hw.h

@@ -0,0 +1,99 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 BOOZ_LINK_MCU_HW_H
+#define BOOZ_LINK_MCU_HW_H
+
+#include "LPC21xx.h"
+#include "booz_debug.h"
+
+#define LINK_MCU_STATUS_IDLE 0
+#define LINK_MCU_STATUS_BUSY 1
+
+#define LINK_IMU_ERR_OVERRUN 0
+
+extern volatile uint8_t link_mcu_status;
+extern uint8_t* link_mcu_tx_buf;
+extern uint8_t* link_mcu_rx_buf;
+#define LINK_MCU_BUF_LEN (sizeof(struct booz_inter_mcu_state)/sizeof(uint8_t))
+
+#ifdef  BOOZ_FILTER_MCU
+
+extern volatile uint8_t link_mcu_buf_idx;
+
+#define SS_PIN 7
+#define SPI0_SelectSlave()   SetBit(IO0CLR, SS_PIN)
+#define SPI0_UnselectSlave() SetBit(IO0SET, SS_PIN)
+
+#define BoozLinkMcuHwSend() {						\
+    ASSERT((link_mcu_status == LINK_MCU_STATUS_IDLE),			\
+	   DEBUG_LINK_MCU_IMU, LINK_IMU_ERR_OVERRUN);			\
+    if (link_mcu_status != LINK_MCU_STATUS_IDLE) return;		\
+    SPI0_SelectSlave();							\
+    link_mcu_status = LINK_MCU_STATUS_BUSY;				\
+    link_mcu_buf_idx = 0;						\
+    S0SPDR = link_mcu_tx_buf[0];					\
+  }
+
+#endif /* BOOZ_FILTER_MCU */
+
+
+#ifdef  BOOZ_CONTROLLER_MCU
+
+extern volatile uint8_t link_mcu_rx_buf_idx;
+extern volatile uint8_t link_mcu_tx_buf_idx;
+
+#define SSP_Enable()     SetBit(SSPCR1, SSE);
+#define SSP_Disable()    ClearBit(SSPCR1, SSE);
+#define SSP_EnableRxi()  SetBit(SSPIMSC, RXIM)
+#define SSP_DisableRxi() ClearBit(SSPIMSC, RXIM)
+#define SSP_EnableTxi()  SetBit(SSPIMSC, TXIM)
+#define SSP_DisableTxi() ClearBit(SSPIMSC, TXIM)
+#define SSP_EnableRti()  SetBit(SSPIMSC, RTIM);
+#define SSP_DisableRti() ClearBit(SSPIMSC, RTIM);
+#define SSP_ClearRti()   SetBit(SSPICR, RTIC);
+
+#define LinkMcuTransmit() {						\
+    while (link_mcu_tx_buf_idx < LINK_MCU_BUF_LEN			\
+	   && bit_is_set(SSPSR, TNF)) {					\
+      SSPDR = link_mcu_tx_buf[link_mcu_tx_buf_idx];			\
+      link_mcu_tx_buf_idx++;						\
+    }			                                                \
+    if (link_mcu_tx_buf_idx == LINK_MCU_BUF_LEN)			\
+      SSP_DisableRxi();							\
+  }
+
+#define BoozLinkMcuHwRestart() { \
+    link_mcu_rx_buf_idx = 0;	 \
+    link_mcu_tx_buf_idx = 0;	 \
+    SSP_EnableRxi();		 \
+    SSP_Enable();		 \
+    /* LinkMcuTransmit(); */	 \
+  }
+
+
+
+#endif /* BOOZ_CONTROLLER_MCU */
+
+#endif /* BOOZ_LINK_MCU_HW_H */

sw/airborne/booz/booz_ahrs.c

@@ -0,0 +1,19 @@
+#include "booz_ahrs.h"
+
+uint8_t booz_ahrs_status;
+
+FLOAT_T booz_ahrs_phi; 
+FLOAT_T booz_ahrs_theta; 
+FLOAT_T booz_ahrs_psi;
+
+FLOAT_T booz_ahrs_p;
+FLOAT_T booz_ahrs_q;
+FLOAT_T booz_ahrs_r;
+
+FLOAT_T booz_ahrs_bp;
+FLOAT_T booz_ahrs_bq;
+FLOAT_T booz_ahrs_br;
+
+FLOAT_T booz_ahrs_measure_phi;
+FLOAT_T booz_ahrs_measure_theta;
+FLOAT_T booz_ahrs_measure_psi;

sw/airborne/booz/booz_ahrs.h

@@ -0,0 +1,114 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 BOOZ_AHRS_H
+#define BOOZ_AHRS_H
+
+#include "std.h"
+
+#define BOOZ_AHRS_STATUS_UNINIT  0
+#define BOOZ_AHRS_STATUS_RUNNING 1
+#define BOOZ_AHRS_STATUS_CRASHED 2
+
+extern uint8_t booz_ahrs_status;
+
+extern FLOAT_T booz_ahrs_phi; 
+extern FLOAT_T booz_ahrs_theta; 
+extern FLOAT_T booz_ahrs_psi;
+
+extern FLOAT_T booz_ahrs_p;
+extern FLOAT_T booz_ahrs_q;
+extern FLOAT_T booz_ahrs_r;
+
+extern FLOAT_T booz_ahrs_bp;
+extern FLOAT_T booz_ahrs_bq;
+extern FLOAT_T booz_ahrs_br;
+
+extern FLOAT_T booz_ahrs_measure_phi;
+extern FLOAT_T booz_ahrs_measure_theta;
+extern FLOAT_T booz_ahrs_measure_psi;
+
+extern void booz_ahrs_init(void);
+extern void booz_ahrs_start( const float* accel, const float* gyro, const float* mag);
+/*extern void booz_ahrs_run(const float* accel, const float* gyro, const float* mag);*/
+
+extern void booz_ahrs_predict( const float* gyro );
+extern void booz_ahrs_update_accel( const float* accel);
+extern void booz_ahrs_update_mag( const float* mag );
+
+
+//#define __AHRS_IMPL_HEADER(_typ, _ext) _t##_ext
+//#define _AHRS_IMPL_HEADER(_typ, _ext) __AHRS_IMPL_HEADER(_typ, _ext)
+//#define AHRS_IMPL_HEADER(_typ) _AHRS_IMPL_HEADER(_typ, ".h")
+//#error BOOZ_AHRS_TYPE
+//#include \"AHRS_IMPL_HEADER(BOOZ_AHRS_TYPE)\"
+
+#define BOOZ_AHRS_MULTITILT   0
+#define BOOZ_AHRS_QUATERNION  1
+#define BOOZ_AHRS_EULER       2
+#define BOOZ_AHRS_COMP_FILTER 3
+
+#if defined  BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT
+#include "ahrs_multitilt.h"
+#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION
+#include "ahrs_quat_fast_ekf.h"
+#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_EULER
+#include "ahrs_euler_fast_ekf.h"
+#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_COMP_FILTER
+#include "ahrs_comp_filter.h"
+#endif
+
+
+#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;}
+
+#define PhiOfAccel(_phi, _accel) {			\
+    _phi =  atan2(-_accel[AXIS_Y], -_accel[AXIS_Z]);	\
+  }
+
+#define ThetaOfAccel(_theta, _accel) {			\
+    const float g2 =					\
+      _accel[AXIS_X]*_accel[AXIS_X] +			\
+      _accel[AXIS_Y]*_accel[AXIS_Y] +			\
+      _accel[AXIS_Z]*_accel[AXIS_Z];			\
+    _theta = asin( accel[AXIS_X] / sqrt( g2 ) );	\
+  }
+
+#define PsiOfMag(_psi, _mag) {			        \
+    const float cphi   = cos( booz_ahrs_phi );		\
+    const float sphi   = sin( booz_ahrs_phi );		\
+    const float ctheta = cos( booz_ahrs_theta );	\
+    const float stheta = sin( booz_ahrs_theta );	\
+						        \
+    const float mn =					\
+      ctheta*      _mag[AXIS_X]+			\
+      sphi*stheta* _mag[AXIS_Y]+			\
+      cphi*stheta* _mag[AXIS_Z];			\
+    const float me =					\
+      /*    0*     mag[AXIS_X]+ */			\
+      cphi*  mag[AXIS_Y]+				\
+      -sphi* mag[AXIS_Z];				\
+    _psi = -atan2( me, mn );				\
+  }
+
+#endif /* BOOZ_AHRS_H */

sw/airborne/booz/booz_autopilot.c

@@ -0,0 +1,83 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_autopilot.h"
+
+#include "radio_control.h"
+#include "commands.h"
+#include "booz_control.h"
+#include "booz_nav.h"
+
+uint8_t booz_autopilot_mode;
+
+void booz_autopilot_init(void) {
+  booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE;
+}
+
+void booz_autopilot_periodic_task(void) {
+
+  switch (booz_autopilot_mode) {
+  case BOOZ_AP_MODE_FAILSAFE:
+  case BOOZ_AP_MODE_KILL:
+    SetCommands(commands_failsafe);
+    break;
+  case BOOZ_AP_MODE_RATE:
+    booz_control_rate_run();
+    SetCommands(booz_control_commands);
+    break;
+  case BOOZ_AP_MODE_ATTITUDE:
+  case BOOZ_AP_MODE_HEADING_HOLD:
+    booz_control_attitude_run();
+    SetCommands(booz_control_commands);
+    break;
+  case BOOZ_AP_MODE_NAV:
+    booz_nav_run();
+    booz_control_attitude_run();
+    SetCommands(booz_control_commands);
+    break;
+  }
+
+}
+
+
+void booz_autopilot_on_rc_event(void) {
+  /* I think this should be hidden in rc code */
+  /* the ap gets a mode everytime - the rc filters it */
+  if (rc_values_contains_avg_channels) {
+    booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
+    rc_values_contains_avg_channels = FALSE;
+  }
+  switch (booz_autopilot_mode) {
+  case BOOZ_AP_MODE_RATE:
+    booz_control_rate_read_setpoints_from_rc();
+    break;
+  case BOOZ_AP_MODE_ATTITUDE:
+  case BOOZ_AP_MODE_HEADING_HOLD:
+    booz_control_attitude_read_setpoints_from_rc();
+    break;
+  case BOOZ_AP_MODE_NAV:
+    booz_nav_read_setpoints_from_rc();
+    break;
+  }
+}

sw/airborne/booz/booz_autopilot.h

@@ -0,0 +1,50 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 BOOZ_AUTOPILOT_H
+#define BOOZ_AUTOPILOT_H
+
+#include "std.h"
+
+#define BOOZ_AP_MODE_FAILSAFE     0
+#define BOOZ_AP_MODE_KILL         1
+#define BOOZ_AP_MODE_RATE         2
+#define BOOZ_AP_MODE_ATTITUDE     3
+#define BOOZ_AP_MODE_HEADING_HOLD 4
+#define BOOZ_AP_MODE_NAV          5
+
+extern uint8_t booz_autopilot_mode;
+
+extern void booz_autopilot_init(void);
+extern void booz_autopilot_periodic_task(void);
+extern void booz_autopilot_on_rc_event(void);
+
+#define TRESHOLD_RATE_PPRZ (MIN_PPRZ / 2)
+#define TRESHOLD_ATTITUDE_PPRZ  (MAX_PPRZ/2)
+#define BOOZ_AP_MODE_OF_PPRZ(rc)					\
+  ((rc) < TRESHOLD_RATE_PPRZ ? BOOZ_AP_MODE_ATTITUDE :			\
+   (rc) < TRESHOLD_ATTITUDE_PPRZ ? BOOZ_AP_MODE_HEADING_HOLD :		\
+   BOOZ_AP_MODE_NAV )
+
+#endif /* BOOZ_AUTOPILOT_H */

sw/airborne/booz/booz_control.c

@@ -0,0 +1,205 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_control.h"
+
+#include "booz_estimator.h"
+#include "booz_autopilot.h"
+#include "radio_control.h"
+
+#define BOOZ_CONTROL_MIN_THROTTLE 0.05
+
+float booz_control_p_sp;
+float booz_control_q_sp;
+float booz_control_r_sp;
+float booz_control_power_sp;
+
+float booz_control_rate_pq_pgain;
+float booz_control_rate_pq_dgain;
+float booz_control_rate_r_pgain;
+float booz_control_rate_r_dgain;
+float booz_control_rate_last_err_p;
+float booz_control_rate_last_err_q;
+float booz_control_rate_last_err_r;
+
+pprz_t booz_control_commands[COMMANDS_NB];
+
+#define BOOZ_CONTROL_RATE_PQ_PGAIN -700.
+#define BOOZ_CONTROL_RATE_PQ_DGAIN   15.
+
+#define BOOZ_CONTROL_RATE_R_PGAIN -600.
+#define BOOZ_CONTROL_RATE_R_DGAIN    5.
+
+/* setpoints for max stick throw in degres per second */
+#define BOOZ_CONTROL_RATE_PQ_MAX_SP 120.
+#define BOOZ_CONTROL_RATE_R_MAX_SP  100.
+
+
+float booz_control_attitude_phi_sp;
+float booz_control_attitude_theta_sp;
+float booz_control_attitude_psi_sp;
+float booz_control_attitude_phi_theta_pgain;
+float booz_control_attitude_phi_theta_dgain;
+float booz_control_attitude_psi_pgain;
+float booz_control_attitude_psi_dgain;
+
+#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN  -1250.
+#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN   -700.
+
+#define BOOZ_CONTROL_ATTITUDE_PSI_PGAIN  -1050.
+#define BOOZ_CONTROL_ATTITUDE_PSI_DGAIN   -850.
+
+/* setpoints for max stick throw in degres */
+#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30.
+#define BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP 45.
+
+#define BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP (1./50.)
+
+void booz_control_init(void) {
+
+  booz_control_p_sp = 0.;
+  booz_control_q_sp = 0.;
+  booz_control_r_sp = 0.;
+  booz_control_power_sp = 0.;
+
+  booz_control_rate_last_err_p = 0.;
+  booz_control_rate_last_err_q = 0.;
+  booz_control_rate_last_err_r = 0.;
+
+  booz_control_rate_pq_pgain = BOOZ_CONTROL_RATE_PQ_PGAIN;
+  booz_control_rate_pq_dgain = BOOZ_CONTROL_RATE_PQ_DGAIN;
+  booz_control_rate_r_pgain = BOOZ_CONTROL_RATE_R_PGAIN;
+  booz_control_rate_r_dgain = BOOZ_CONTROL_RATE_R_DGAIN;
+
+
+  booz_control_attitude_phi_sp = 0.;
+  booz_control_attitude_theta_sp =0.;
+  booz_control_attitude_psi_sp =0.;
+  booz_control_attitude_phi_theta_pgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN;
+  booz_control_attitude_phi_theta_dgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN;
+  booz_control_attitude_psi_pgain = BOOZ_CONTROL_ATTITUDE_PSI_PGAIN;
+  booz_control_attitude_psi_dgain = BOOZ_CONTROL_ATTITUDE_PSI_DGAIN;
+
+}
+
+
+void booz_control_rate_read_setpoints_from_rc(void) {
+
+  booz_control_p_sp = -rc_values[RADIO_ROLL]  * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
+  booz_control_q_sp =  rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
+  booz_control_r_sp = -rc_values[RADIO_YAW]   * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
+  booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
+
+}
+
+
+void booz_control_rate_run(void) {
+
+  if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
+    booz_control_commands[COMMAND_P] = 0;
+    booz_control_commands[COMMAND_Q] = 0;
+    booz_control_commands[COMMAND_R] = 0;
+    booz_control_commands[COMMAND_THROTTLE] = 0;
+  }
+  else {
+    const float rate_err_p = booz_estimator_uf_p - booz_control_p_sp;
+    const float rate_d_err_p = rate_err_p - booz_control_rate_last_err_p;
+    booz_control_rate_last_err_p = rate_err_p;
+    const float cmd_p = booz_control_rate_pq_pgain * ( rate_err_p + booz_control_rate_pq_dgain * rate_d_err_p );
+    
+    const float rate_err_q = booz_estimator_uf_q - booz_control_q_sp;
+    const float rate_d_err_q = rate_err_q - booz_control_rate_last_err_q;
+    booz_control_rate_last_err_q = rate_err_q;
+    const float cmd_q = booz_control_rate_pq_pgain * ( rate_err_q + booz_control_rate_pq_dgain * rate_d_err_q );
+    
+    const float rate_err_r = booz_estimator_uf_r - booz_control_r_sp;
+    const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
+    booz_control_rate_last_err_r = rate_err_r;
+    const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
+
+    booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
+    booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
+    booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
+    booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
+  }
+
+}
+
+void booz_control_attitude_read_setpoints_from_rc(void) {
+
+  booz_control_attitude_phi_sp = -rc_values[RADIO_ROLL]  * 
+                                  RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
+  booz_control_attitude_theta_sp =  rc_values[RADIO_PITCH] * 
+                                  RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
+  if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
+    booz_control_r_sp = -rc_values[RADIO_YAW]   * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
+  }
+  else { /* BOOZ_AP_MODE_HEADING_HOLD */
+    if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE)
+      booz_control_attitude_psi_sp = booz_estimator_psi;
+    else {
+      booz_control_attitude_psi_sp +=  -rc_values[RADIO_YAW] * 
+	RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)*BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP/MAX_PPRZ;
+    }
+  }
+  booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
+}
+
+void booz_control_attitude_run(void) {
+
+  if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
+    booz_control_commands[COMMAND_P] = 0;
+    booz_control_commands[COMMAND_Q] = 0;
+    booz_control_commands[COMMAND_R] = 0;
+    booz_control_commands[COMMAND_THROTTLE] = 0;
+  }
+  else {
+    const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp;
+    const float cmd_p = booz_control_attitude_phi_theta_pgain *  att_err_phi + 
+      booz_control_attitude_phi_theta_dgain * booz_estimator_p ;
+    
+    const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp;
+    const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta + 
+      booz_control_attitude_phi_theta_dgain * booz_estimator_q;
+    
+    float cmd_r;
+    if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
+      const float rate_err_r = booz_estimator_r - booz_control_r_sp;
+      const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
+      booz_control_rate_last_err_r = rate_err_r;
+      cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
+    }
+    else {
+      float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
+      NormRadAngle(att_err_psi);
+      cmd_r = booz_control_attitude_psi_pgain * att_err_psi + 
+	booz_control_attitude_psi_dgain * booz_estimator_r;
+    }
+
+    booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
+    booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
+    booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
+    booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
+  }
+}

sw/airborne/booz/booz_control.h

@@ -0,0 +1,69 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 BOOZ_CONTROL_H
+#define BOOZ_CONTROL_H
+
+#include "std.h"
+#include "paparazzi.h"
+
+extern void booz_control_init(void);
+
+extern void booz_control_rate_read_setpoints_from_rc(void);
+extern void booz_control_rate_run(void);
+
+extern void booz_control_attitude_read_setpoints_from_rc(void);
+extern void booz_control_attitude_run(void);
+
+extern float booz_control_p_sp;
+extern float booz_control_q_sp;
+extern float booz_control_r_sp;
+extern float booz_control_power_sp;
+
+extern float booz_control_rate_pq_pgain;
+extern float booz_control_rate_pq_dgain;
+extern float booz_control_rate_r_pgain;
+extern float booz_control_rate_r_dgain;
+
+extern float booz_control_attitude_phi_sp;
+extern float booz_control_attitude_theta_sp;
+extern float booz_control_attitude_psi_sp;
+
+extern float booz_control_attitude_phi_theta_pgain;
+extern float booz_control_attitude_phi_theta_dgain;
+extern float booz_control_attitude_psi_pgain;
+extern float booz_control_attitude_psi_dgain;
+
+
+#define BoozControlAttitudeSetSetPoints(_phi_sp, _theta_sp, _psi_sp, _power_sp) { \
+    booz_control_attitude_phi_sp = _phi_sp;				          \
+    booz_control_attitude_theta_sp = _theta_sp;				          \
+    booz_control_attitude_psi_sp = _psi_sp;				          \
+    booz_control_power_sp = _power_sp;					          \
+  }
+
+#include "airframe.h"
+extern pprz_t booz_control_commands[COMMANDS_NB];
+
+#endif /* BOOZ_CONTROL_H */

sw/airborne/booz/booz_controller_main.c

@@ -0,0 +1,172 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 "booz_controller_main.h"
+
+#include "std.h"
+#include "init_hw.h"
+#include "interrupt_hw.h"
+#include "sys_time.h"
+#include "led.h"
+
+#include "booz_energy.h"
+
+#include "commands.h"
+#include "i2c.h"
+#include "actuators.h"
+#include "radio_control.h"
+
+#include "adc.h"
+#include "booz_link_mcu.h"
+
+#include "booz_estimator.h"
+#include "booz_control.h"
+#include "booz_nav.h"
+#include "booz_autopilot.h"
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include "booz_controller_telemetry.h"
+#include "datalink.h"
+
+
+
+int16_t trim_p = 0;
+int16_t trim_q = 0;
+int16_t trim_r = 0;
+uint8_t vbat = 0;
+
+#ifndef SITL
+int main( void ) {
+  booz_controller_main_init();
+  while(1) {
+    if (sys_time_periodic())
+      booz_controller_main_periodic_task();
+    booz_controller_main_event_task();
+  }
+  return 0;
+}
+#endif
+
+STATIC_INLINE void booz_controller_main_init( void ) {
+
+  hw_init();
+  led_init();
+  sys_time_init();
+
+  adc_init();
+  booz_energy_init();
+
+  i2c_init();
+  actuators_init();
+  SetCommands(commands_failsafe);
+
+  ppm_init();
+  radio_control_init();
+
+  booz_link_mcu_init();
+  
+  booz_estimator_init();
+  booz_control_init();
+  booz_nav_init();
+  booz_autopilot_init();
+
+  //FIXME
+#ifndef SITL
+  uart1_init_tx();
+#endif
+
+  int_enable();
+
+  DOWNLINK_SEND_BOOT(&cpu_time_sec);
+}
+
+
+#define PeriodicPrescaleBy5( _code_0, _code_1, _code_2, _code_3, _code_4) { \
+    static uint8_t _50hz = 0;						\
+    _50hz++;								\
+    if (_50hz >= 5) _50hz = 0;						\
+    switch (_50hz) {							\
+    case 0:								\
+      _code_0;								\
+      break;								\
+    case 1:								\
+      _code_1;								\
+      break;								\
+    case 2:								\
+      _code_2;								\
+      break;								\
+    case 3:								\
+      _code_3;								\
+      break;								\
+    case 4:								\
+      _code_4;								\
+      break;								\
+    }									\
+  }
+
+STATIC_INLINE void booz_controller_main_periodic_task( void ) {
+  
+  /* check for timeout */
+  booz_link_mcu_periodic();
+  /* run control loops */
+  booz_autopilot_periodic_task();
+
+  //  commands[COMMAND_P] = 0;
+  //  commands[COMMAND_Q] = 0;
+  //  commands[COMMAND_R] = 0;
+  //  commands[COMMAND_THROTTLE] = MAX_PPRZ/3;
+
+  SetActuatorsFromCommands(commands);
+
+  PeriodicPrescaleBy5(							\
+    {							                \
+      radio_control_periodic_task();					\
+      if (rc_status != RC_OK)						\
+	booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE;			\
+    },									\
+    {									\
+      booz_controller_telemetry_periodic_task();			\
+    },									\
+    {									\
+      booz_energy_periodic();						\
+    },									\
+    {},									\
+    {}									\
+    );									\
+}
+
+STATIC_INLINE void booz_controller_main_event_task( void ) {
+  
+  // FIXME
+#ifndef SITL
+  DlEventCheckAndHandle();
+#endif
+
+  BoozLinkMcuEventCheckAndHandle(booz_estimator_read_inter_mcu_state);
+
+  RadioControlEventCheckAndHandle(booz_autopilot_on_rc_event);
+ 
+}

sw/airborne/booz/booz_controller_main.h

@@ -0,0 +1,14 @@
+#ifndef BOOZ_CONTROLLER_MAIN_H
+#define BOOZ_CONTROLLER_MAIN_H
+
+#ifdef SITL
+#define STATIC_INLINE
+#else
+#define STATIC_INLINE static inline
+#endif
+
+STATIC_INLINE void booz_controller_main_init( void );
+STATIC_INLINE void booz_controller_main_periodic_task( void );
+STATIC_INLINE void booz_controller_main_event_task( void );
+
+#endif /* BOOZ_CONTROLLER_MAIN_H */

sw/airborne/booz/booz_controller_telemetry.c

@@ -0,0 +1,5 @@
+#include "booz_controller_telemetry.h"
+
+
+uint8_t telemetry_mode_Controller;
+

sw/airborne/booz/booz_controller_telemetry.h

@@ -0,0 +1,106 @@
+#ifndef BOOZ_CONTROLLER_TELEMETRY_H
+#define BOOZ_CONTROLLER_TELEMETRY_H
+
+#include "std.h"
+#include "messages.h"
+#include "periodic.h"
+#include "uart.h"
+
+#include "booz_energy.h"
+#include "radio_control.h"
+#include "actuators.h"
+#include "booz_link_mcu.h"
+#include "booz_estimator.h"
+#include "booz_autopilot.h"
+#include "booz_control.h"
+#include "booz_nav.h"
+
+#include "actuators_buss_twi_blmc_hw.h"
+
+#include "settings.h"
+
+#include "downlink.h"
+
+#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE(16, MD5SUM)
+
+#define PERIODIC_SEND_BOOZ_STATUS()					\
+  DOWNLINK_SEND_BOOZ_STATUS(&booz_link_mcu_nb_err,			\
+			    &booz_link_mcu_status,			\
+			    &rc_status,					\
+			    &booz_autopilot_mode,			\
+			    &booz_energy_bat,				\
+			    &cpu_time_sec,				\
+			    &buss_twi_blmc_nb_err)
+
+
+#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values)
+
+#define PERIODIC_SEND_BOOZ_FD()						\
+  DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p,				\
+			&booz_estimator_q,				\
+			&booz_estimator_r,				\
+			&booz_estimator_phi,				\
+			&booz_estimator_theta,				\
+			&booz_estimator_psi); 
+
+#define PERIODIC_SEND_ACTUATORS()			\
+  DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
+
+#define PERIODIC_SEND_BOOZ_CONTROL() {					\
+    switch (booz_autopilot_mode) {					\
+    case BOOZ_AP_MODE_RATE:						\
+      DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \
+				   &booz_estimator_uf_q, &booz_control_q_sp, \
+				   &booz_estimator_uf_r, &booz_control_r_sp, \
+				   &booz_control_power_sp);		\
+      break;								\
+    case BOOZ_AP_MODE_ATTITUDE:						\
+    case BOOZ_AP_MODE_HEADING_HOLD:					\
+    case BOOZ_AP_MODE_NAV:						\
+      DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \
+				  &booz_estimator_theta, &booz_control_attitude_theta_sp, \
+				  &booz_estimator_psi, &booz_control_attitude_psi_sp, \
+				  &booz_control_power_sp);		\
+      break;								\
+    }									\
+  }
+
+#define PERIODIC_SEND_BOOZ_UF_RATES()               \
+  DOWNLINK_SEND_BOOZ_UF_RATES(&booz_estimator_uf_p, \
+			      &booz_estimator_uf_q, \
+			      &booz_estimator_uf_r); 
+
+#define PERIODIC_SEND_BOOZ_VERT_LOOP() {				\
+    DOWNLINK_SEND_BOOZ_VERT_LOOP(&booz_nav_hover_z_sp,			\
+				 &booz_estimator_w,			\
+				 &booz_estimator_z,			\
+				 &booz_nav_hover_power_command);	\
+  }
+#define PERIODIC_SEND_BOOZ_HOV_LOOP() {					\
+    DOWNLINK_SEND_BOOZ_HOV_LOOP(&booz_nav_hover_x_sp,			\
+				&booz_nav_hover_y_sp,			\
+				&booz_estimator_u,			\
+				&booz_estimator_x,			\
+				&booz_estimator_v,			\
+				&booz_estimator_y,			\
+				&booz_nav_hover_phi_command,		\
+				&booz_nav_hover_theta_command);		\
+  }
+
+#define PERIODIC_SEND_BOOZ_CMDS() DOWNLINK_SEND_BOOZ_CMDS(&buss_twi_blmc_motor_power[SERVO_MOTOR_FRONT],\
+							  &buss_twi_blmc_motor_power[SERVO_MOTOR_BACK],	\
+							  &buss_twi_blmc_motor_power[SERVO_MOTOR_LEFT],	\
+							  &buss_twi_blmc_motor_power[SERVO_MOTOR_RIGHT]); 
+
+
+
+#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue()
+
+extern uint8_t telemetry_mode_Controller;
+
+static inline void booz_controller_telemetry_periodic_task(void) {
+  PeriodicSendController()
+}
+
+
+#endif /* BOOZ_CONTROLLER_TELEMETRY_H */

sw/airborne/booz/booz_datalink.c

@@ -0,0 +1,24 @@
+#define DATALINK_C
+#include "datalink.h"
+
+#include "settings.h"
+#include "downlink.h"
+#include "messages.h"
+#include "dl_protocol.h"
+#include "uart.h"
+
+#define IdOfMsg(x) (x[1])
+
+void dl_parse_msg(void) {
+  uint8_t msg_id = IdOfMsg(dl_buffer);
+  switch (msg_id) {
+  
+  case DL_SETTING : {
+    uint8_t i = DL_SETTING_index(dl_buffer);
+    float var = DL_SETTING_value(dl_buffer);
+    DlSetting(i, var);
+    DOWNLINK_SEND_DL_VALUE(&i, &var);
+    break;
+  }
+  }
+}

sw/airborne/booz/booz_debug.c

@@ -0,0 +1,4 @@
+#include "booz_debug.h"
+
+uint8_t booz_debug_mod;
+uint8_t booz_debug_err;

sw/airborne/booz/booz_debug.h

@@ -0,0 +1,31 @@
+#ifndef BOOZ_DEBUG_H
+#define BOOZ_DEBUG_H
+
+#ifdef BOOZ_DEBUG
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+extern uint8_t booz_debug_mod;
+extern uint8_t booz_debug_err;
+
+#define DEBUG_IMU          0
+#define DEBUG_MAX_1117     1
+#define DEBUG_SCP1000      2
+#define DEBUG_LINK_MCU_IMU 3
+
+
+#define ASSERT(cond, mod, err) {					\
+    if (!(cond)) {							\
+      booz_debug_mod = mod;						\
+      booz_debug_err = err;						\
+      DOWNLINK_SEND_BOOZ_ERROR(&booz_debug_mod, &booz_debug_err);	\
+    }									\
+  }
+#else
+#define ASSERT(cond, mod, err) {}
+#endif
+
+
+#endif /* BOOZ_DEBUG_H */

sw/airborne/booz/booz_energy.c

@@ -0,0 +1,16 @@
+#include "booz_energy.h"
+#include CONFIG
+#include "adc.h"
+
+
+
+uint8_t booz_energy_bat;
+static struct adc_buf bat_adc_buf;
+
+void booz_energy_init( void ) {
+  adc_buf_channel(ADC_BAT, &bat_adc_buf, DEFAULT_AV_NB_SAMPLE);
+}
+
+void booz_energy_periodic( void ) {
+  booz_energy_bat =  bat_adc_buf.sum * 0.004295;
+}

sw/airborne/booz/booz_energy.h

@@ -0,0 +1,14 @@
+#ifndef BOOZ_ENERGY_H
+#define BOOZ_ENERGY_H
+
+#include "std.h"
+
+extern uint8_t booz_energy_bat;
+
+extern void booz_energy_init( void );
+extern void booz_energy_periodic( void );
+
+
+
+
+#endif /* BOOZ_ENERGY_H */

sw/airborne/booz/booz_estimator.c

@@ -0,0 +1,128 @@
+#include "booz_estimator.h"
+
+#include "booz_inter_mcu.h"
+
+
+float booz_estimator_uf_p;
+float booz_estimator_uf_q;
+float booz_estimator_uf_r;
+
+float booz_estimator_p;
+float booz_estimator_q;
+float booz_estimator_r;
+
+float booz_estimator_phi;
+float booz_estimator_theta;
+float booz_estimator_psi;
+
+float booz_estimator_dcm[AXIS_NB][AXIS_NB];
+
+float booz_estimator_x;
+float booz_estimator_y;
+float booz_estimator_z;
+
+float booz_estimator_vx;
+float booz_estimator_vy;
+float booz_estimator_vz;
+
+float booz_estimator_u;
+float booz_estimator_v;
+float booz_estimator_w;
+
+void booz_estimator_init( void ) {
+
+  booz_estimator_uf_p = 0.;
+  booz_estimator_uf_q = 0.;
+  booz_estimator_uf_r = 0.;
+
+  booz_estimator_p = 0.;
+  booz_estimator_q = 0.;
+  booz_estimator_r = 0.;
+
+  booz_estimator_phi = 0.;
+  booz_estimator_theta = 0.;
+  booz_estimator_psi = 0.;
+
+  booz_estimator_x = 0.;
+  booz_estimator_y = 0.;
+  booz_estimator_z = 0.;
+
+  booz_estimator_vx = 0.;
+  booz_estimator_vy = 0.;
+  booz_estimator_vz = 0.;
+
+  booz_estimator_u = 0.;
+  booz_estimator_v = 0.;
+  booz_estimator_w = 0.;
+}
+
+
+#define BoozEstimatorComputeDCM() {			                     \
+							                     \
+    float sinPHI   = sin( booz_estimator_phi );		                     \
+    float cosPHI   = cos( booz_estimator_phi );		                     \
+    float sinTHETA = sin( booz_estimator_theta);	                     \
+    float cosTHETA = cos( booz_estimator_theta);	                     \
+    float sinPSI   = sin( booz_estimator_psi);		                     \
+    float cosPSI   = cos( booz_estimator_psi);		                     \
+    							                     \
+    booz_estimator_dcm[0][0] = cosTHETA * cosPSI;			     \
+    booz_estimator_dcm[0][1] = cosTHETA * sinPSI;			     \
+    booz_estimator_dcm[0][2] = -sinTHETA;				     \
+    booz_estimator_dcm[1][0] = sinPHI * sinTHETA * cosPSI - cosPHI * sinPSI; \
+    booz_estimator_dcm[1][1] = sinPHI * sinTHETA * sinPSI + cosPHI * cosPSI; \
+    booz_estimator_dcm[1][2] = sinPHI * cosTHETA;			     \
+    booz_estimator_dcm[2][0] = cosPHI * sinTHETA * cosPSI + sinPHI * sinPSI; \
+    booz_estimator_dcm[2][1] = cosPHI * sinTHETA * sinPSI - sinPHI * cosPSI; \
+    booz_estimator_dcm[2][2] = cosPHI * cosTHETA;			     \
+									     \
+}
+
+#define BoozEstimatorUpdateBodySpeed() {				\
+    						                        \
+    booz_estimator_u =							\
+      booz_estimator_dcm[AXIS_U][AXIS_X] * booz_estimator_vx +		\
+      booz_estimator_dcm[AXIS_U][AXIS_Y] * booz_estimator_vy +		\
+      booz_estimator_dcm[AXIS_U][AXIS_Z] * booz_estimator_vz ;		\
+    									\
+    booz_estimator_v =							\
+      booz_estimator_dcm[AXIS_V][AXIS_X] * booz_estimator_vx +		\
+      booz_estimator_dcm[AXIS_V][AXIS_Y] * booz_estimator_vy +		\
+      booz_estimator_dcm[AXIS_V][AXIS_Z] * booz_estimator_vz ;		\
+    									\
+    booz_estimator_w =							\
+      booz_estimator_dcm[AXIS_W][AXIS_X] * booz_estimator_vx +		\
+      booz_estimator_dcm[AXIS_W][AXIS_Y] * booz_estimator_vy +		\
+      booz_estimator_dcm[AXIS_W][AXIS_Z] * booz_estimator_vz ;		\
+    									\
+  }
+    
+void booz_estimator_read_inter_mcu_state( void ) {
+
+  booz_estimator_uf_p  = inter_mcu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
+  booz_estimator_uf_q  = inter_mcu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
+  booz_estimator_uf_r  = inter_mcu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
+
+  booz_estimator_p     = inter_mcu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
+  booz_estimator_q     = inter_mcu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
+  booz_estimator_r     = inter_mcu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
+
+  booz_estimator_phi   = inter_mcu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
+  booz_estimator_theta = inter_mcu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
+  booz_estimator_psi   = inter_mcu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
+
+  booz_estimator_x = inter_mcu_state.pos[AXIS_X]; 
+  booz_estimator_y = inter_mcu_state.pos[AXIS_Y]; 
+  booz_estimator_z = inter_mcu_state.pos[AXIS_Z]; 
+
+  booz_estimator_vx = inter_mcu_state.speed[AXIS_X]; 
+  booz_estimator_vy = inter_mcu_state.speed[AXIS_Y]; 
+  booz_estimator_vz = inter_mcu_state.speed[AXIS_Z]; 
+
+  BoozEstimatorComputeDCM();
+
+  BoozEstimatorUpdateBodySpeed();
+  
+}
+
+

sw/airborne/booz/booz_estimator.h

@@ -0,0 +1,41 @@
+#ifndef BOOZ_ESTIMATOR_H
+#define BOOZ_ESTIMATOR_H
+
+#include "6dof.h"
+
+/* unfiltered rates available when filter crashed or uninitialed */
+extern float booz_estimator_uf_p;
+extern float booz_estimator_uf_q;
+extern float booz_estimator_uf_r;
+
+extern float booz_estimator_p;
+extern float booz_estimator_q;
+extern float booz_estimator_r;
+
+extern float booz_estimator_phi;
+extern float booz_estimator_theta;
+extern float booz_estimator_psi;
+
+extern float booz_estimator_dcm[AXIS_NB][AXIS_NB];
+
+/* position in earth frame : not yet available - sim only */
+extern float booz_estimator_x;
+extern float booz_estimator_y;
+extern float booz_estimator_z;
+
+/* speed in earth frame : not yet available - sim only */
+extern float booz_estimator_vx;
+extern float booz_estimator_vy;
+extern float booz_estimator_vz;
+
+/* speed in body frame : not yet available - sim only */
+extern float booz_estimator_u;
+extern float booz_estimator_v;
+extern float booz_estimator_w;
+
+
+extern void booz_estimator_init( void );
+extern void booz_estimator_read_inter_mcu_state( void );
+
+
+#endif /* BOOZ_ESTIMATOR_H */

sw/airborne/booz/booz_filter_main.c

@@ -0,0 +1,159 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_filter_main.h"
+
+#include "std.h"
+#include "init_hw.h"
+#include "interrupt_hw.h"
+#include "sys_time.h"
+
+#include "booz_imu.h"
+#include "booz_still_detection.h"
+#include "booz_ahrs.h"
+
+#include "gps.h"
+#include "booz_ins.h"
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include "booz_filter_telemetry.h"
+
+#include "booz_link_mcu.h"
+
+static inline void on_imu_accel( void );
+static inline void on_imu_gyro( void );
+static inline void on_imu_mag( void );
+static inline void on_imu_baro( void );
+static inline void on_gps( void );
+
+#ifndef SITL
+int main( void ) {
+  { uint32_t foo = 0; while (foo < 1e5) foo++;}
+
+  booz_filter_main_init();
+
+  while (1) {
+    if (sys_time_periodic())
+      booz_filter_main_periodic_task();
+    booz_filter_main_event_task();
+  }
+  return 0;
+}
+#endif
+
+STATIC_INLINE void booz_filter_main_init( void ) {
+
+  hw_init();
+
+  sys_time_init();
+  //FIXME
+#ifndef SITL
+  uart0_init_tx();
+  uart1_init_tx();
+#endif
+  booz_imu_init();
+
+  booz_still_detection_init();
+
+  booz_ahrs_init();
+
+  booz_ins_init();
+
+  booz_link_mcu_init();
+
+  int_enable();
+
+}
+
+// static uint32_t t0, t1, diff;
+
+STATIC_INLINE void booz_filter_main_event_task( void ) {
+  /* check if measurements are available */
+  BoozImuEvent(on_imu_accel, on_imu_gyro, on_imu_mag, on_imu_baro);
+  
+  GpsEventCheckAndHandle(on_gps, FALSE);
+}
+
+STATIC_INLINE void booz_filter_main_periodic_task( void ) {
+
+  booz_imu_periodic();
+  RunOnceEvery(4, {booz_filter_telemetry_periodic_task();})
+
+}
+
+static inline void on_imu_accel( void ) {
+  if (booz_ahrs_status == BOOZ_AHRS_STATUS_RUNNING) {
+    RunOnceEvery(4, {booz_ahrs_update_accel(imu_accel);}); 
+  }
+  if (booz_ins_status == BOOZ_INS_STATUS_RUNNING) {
+    booz_ins_predict(imu_accel); 
+  }
+}
+
+static inline void on_imu_gyro( void ) {
+
+  switch (booz_ahrs_status) {
+
+  case BOOZ_AHRS_STATUS_UNINIT :
+    booz_still_detection_run();
+    if (booz_still_detection_status == BSD_STATUS_LOCKED) {
+      booz_ahrs_start(booz_still_detection_accel, 
+		      booz_still_detection_gyro, 
+		      booz_still_detection_mag);
+      booz_ins_start(booz_still_detection_accel, 
+		     booz_still_detection_pressure);
+    }
+    break;
+
+  case BOOZ_AHRS_STATUS_RUNNING :
+    // t0 = T0TC;
+    booz_ahrs_predict(imu_gyro);
+    // t1 = T0TC;
+    // diff = t1 - t0;
+    // DOWNLINK_SEND_TIME(&diff);
+    break;
+
+  }
+  //  DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_P], &imu_gyro[AXIS_Q], &imu_gyro[AXIS_R]);
+  booz_link_mcu_send();
+}
+
+static inline void on_imu_mag( void ) {
+  //  DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
+  if (booz_ahrs_status == BOOZ_AHRS_STATUS_RUNNING) {
+    booz_ahrs_update_mag(imu_mag);
+  }
+}
+
+static inline void on_imu_baro( void ) {
+  //  DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure);
+  booz_ins_update_pressure(imu_pressure);
+}
+
+static inline void on_gps( void ) {
+
+
+}

sw/airborne/booz/booz_filter_main.h

@@ -0,0 +1,14 @@
+#ifndef BOOZ_FILTER_MAIN_H
+#define BOOZ_FILTER_MAIN_H
+
+#ifdef SITL
+#define STATIC_INLINE
+#else
+#define STATIC_INLINE static inline
+#endif
+
+STATIC_INLINE void booz_filter_main_init( void );
+STATIC_INLINE void booz_filter_main_periodic_task( void );
+STATIC_INLINE void booz_filter_main_event_task( void );
+
+#endif /* BOOZ_FILTER_MAIN_H */

sw/airborne/booz/booz_filter_telemetry.c

@@ -0,0 +1,3 @@
+#include "booz_filter_telemetry.h"
+
+uint8_t telemetry_mode_Filter;

sw/airborne/booz/booz_filter_telemetry.h

@@ -0,0 +1,180 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 BOOZ_FILTER_TELEMETRY_H
+#define BOOZ_FILTER_TELEMETRY_H
+#include "std.h"
+#include "messages.h"
+#include "periodic.h"
+#include "uart.h"
+
+#include "booz_imu.h"
+#include "booz_ahrs.h"
+#include "booz_ins.h"
+
+#include "settings.h"
+
+#include "downlink.h"
+
+
+
+/*
+ *
+ * IMU
+ *
+ */
+#define PERIODIC_SEND_IMU_GYRO()		\
+  DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_X],	\
+			 &imu_gyro[AXIS_Y],	\
+			 &imu_gyro[AXIS_Z]);
+
+#define PERIODIC_SEND_IMU_GYRO_RAW()			\
+  DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_raw[AXIS_X],	\
+			     &imu_gyro_raw[AXIS_Y],	\
+			     &imu_gyro_raw[AXIS_Z]);
+
+#if 0
+#define PERIODIC_SEND_IMU_GYRO_LP()			\
+  DOWNLINK_SEND_IMU_GYRO_LP(&imu_gyro_lp[AXIS_X],	\
+			    &imu_gyro_lp[AXIS_Y],	\
+			    &imu_gyro_lp[AXIS_Z]);
+#endif
+
+#define PERIODIC_SEND_IMU_ACCEL()		\
+  DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X],	\
+			  &imu_accel[AXIS_Y],	\
+			  &imu_accel[AXIS_Z]);
+
+#define PERIODIC_SEND_IMU_ACCEL_RAW()			\
+  DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X],	\
+			      &imu_accel_raw[AXIS_Y],	\
+			      &imu_accel_raw[AXIS_Z]);
+
+
+#define PERIODIC_SEND_IMU_GYRO_RAW_AVG()			\
+  DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X],	\
+				 &imu_vs_gyro_raw_avg[AXIS_Y],	\
+				 &imu_vs_gyro_raw_avg[AXIS_Z],	\
+				 &imu_vs_gyro_raw_var[AXIS_X],	\
+				 &imu_vs_gyro_raw_var[AXIS_Y],	\
+				 &imu_vs_gyro_raw_var[AXIS_Z]);
+
+#define PERIODIC_SEND_IMU_ACCEL_RAW_AVG()				\
+  DOWNLINK_SEND_IMU_ACCEL_RAW_AVG(&imu_vs_accel_raw_avg[AXIS_X],	\
+				  &imu_vs_accel_raw_avg[AXIS_Y],	\
+				  &imu_vs_accel_raw_avg[AXIS_Z],	\
+				  &imu_vs_accel_raw_var[AXIS_X],	\
+				  &imu_vs_accel_raw_var[AXIS_Y],	\
+				  &imu_vs_accel_raw_var[AXIS_Z]);
+
+#define PERIODIC_SEND_IMU_MAG()			\
+  DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X],	\
+			&imu_mag[AXIS_Y],	\
+			&imu_mag[AXIS_Z]);
+
+#define PERIODIC_SEND_IMU_MAG_RAW()			\
+  DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X],	\
+			    &imu_mag_raw[AXIS_Y],	\
+			    &imu_mag_raw[AXIS_Z]);
+
+#define PERIODIC_SEND_IMU_PRESSURE()		\
+  DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure);
+
+/*
+ *
+ * AHRS
+ *
+ */
+#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT
+
+#define PERIODIC_SEND_AHRS_COV()					\
+  DOWNLINK_SEND_AHRS_EULER_COV(&mtt_P_phi[0][0],   &mtt_P_phi[0][1],	\
+			                           &mtt_P_phi[1][1],	\
+			       &mtt_P_theta[0][0], &mtt_P_theta[0][1],	\
+			                           &mtt_P_theta[1][1],  \
+			       &mtt_P_psi[0][0],   &mtt_P_psi[0][1],	\
+			                           &mtt_P_psi[1][1]);	\
+
+#define PERIODIC_SEND_AHRS_STATE()					\
+  DOWNLINK_SEND_AHRS_EULER_STATE(&booz_ahrs_phi, &booz_ahrs_theta, &booz_ahrs_psi, \
+				 &booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br);
+
+
+#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION
+
+#define PERIODIC_SEND_AHRS_COV() {					\
+    DOWNLINK_SEND_AHRS_QUAT_COV(&afe_P[0][0], &afe_P[1][1],		\
+				&afe_P[2][2], &afe_P[3][3],		\
+				&afe_P[4][4], &afe_P[5][5],		\
+				&afe_P[6][6]);				\
+  }
+
+#define PERIODIC_SEND_AHRS_STATE()					\
+  DOWNLINK_SEND_AHRS_QUAT_STATE(&afe_q0, &afe_q1, &afe_q2, &afe_q3,	\
+				&booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br);
+
+#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_COMP_FILTER
+
+#define PERIODIC_SEND_AHRS_STATE()					\
+  DOWNLINK_SEND_AHRS_EULER_STATE(&booz_ahrs_phi, &booz_ahrs_theta, &booz_ahrs_psi, \
+				 &booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br);
+
+#define PERIODIC_SEND_AHRS_COV() {}
+
+
+
+#endif /* BOOZ_AHRS_TYPE */
+
+#define PERIODIC_SEND_AHRS_MEASURE()					\
+  DOWNLINK_SEND_AHRS_MEASURE(&booz_ahrs_measure_phi,			\
+			     &booz_ahrs_measure_theta,			\
+			     &booz_ahrs_measure_psi);
+
+
+
+#define PERIODIC_SEND_INS_STATE()		\
+  DOWNLINK_SEND_BOOZ_INS_STATE(&booz_ins_z, &booz_ins_z_meas, &booz_ins_zdot, & booz_ins_baz); 
+
+
+#if 0
+#define PERIODIC_SEND_BOOZ_DEBUG() {					\
+    float m_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]);		\
+    const float g2 =							\
+      imu_accel[AXIS_X]*imu_accel[AXIS_X] +				\
+      imu_accel[AXIS_Y]*imu_accel[AXIS_Y] +				\
+      imu_accel[AXIS_Z]*imu_accel[AXIS_Z];				\
+    float m_theta = -asin( imu_accel[AXIS_X] / sqrt( g2 ) );		\
+    DOWNLINK_SEND_BOOZ_DEBUG(&m_phi, &m_theta);		\
+}
+#endif
+
+#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue()
+
+extern uint8_t telemetry_mode_Filter;
+
+static inline void booz_filter_telemetry_periodic_task(void) {
+  PeriodicSendFilter()
+}
+
+#endif /* BOOZ_FILTER_TELEMETRY_H */

sw/airborne/booz/booz_imu.c

@@ -0,0 +1,78 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_imu.h"
+
+#include "booz_debug.h"
+
+/* calibrated sensors */
+float   imu_accel[AXIS_NB]; /* accelerometers in m/s2           */
+float   imu_gyro[AXIS_NB];  /* gyros in rad/s                   */
+float   imu_mag[AXIS_NB];   /* magnetometer in arbitrary unit   */
+float   imu_pressure;       /* static pressure in pascals       */
+
+/* raw sensors */
+uint16_t imu_accel_raw[AXIS_NB];
+uint16_t imu_gyro_raw[AXIS_NB];
+int16_t  imu_mag_raw[AXIS_NB];
+uint32_t imu_pressure_raw;
+
+/* internal ADCs */
+struct adc_buf buf_ax;
+struct adc_buf buf_ay;
+struct adc_buf buf_az;
+
+uint8_t booz_imu_status;
+
+#define IMU_ERR_OVERUN 0
+
+void booz_imu_init(void) {
+  uint8_t i;
+  for (i=0; i<AXIS_NB; i++) {
+    imu_accel_raw[i] = 0;
+    imu_gyro_raw[i] = 0;
+    imu_mag_raw[i] = 0;
+    imu_accel[i] = 0.;
+    imu_gyro[i] = 0.;
+    imu_mag[i] = 0.;
+  }
+  imu_pressure_raw = 0;
+  imu_pressure = 0.;
+  booz_imu_status = BOOZ_IMU_STA_IDLE;
+
+  adc_buf_channel(IMU_ACCEL_X_CHAN, &buf_ax, DEFAULT_AV_NB_SAMPLE);
+  adc_buf_channel(IMU_ACCEL_Y_CHAN, &buf_ay, DEFAULT_AV_NB_SAMPLE);
+  adc_buf_channel(IMU_ACCEL_Z_CHAN, &buf_az, DEFAULT_AV_NB_SAMPLE);
+  adc_init();
+  max1167_init(); 
+  micromag_init();
+  scp1000_init();
+  booz_imu_hw_init();
+}
+
+void booz_imu_periodic(void) {
+  ASSERT((booz_imu_status == BOOZ_IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN);
+  booz_imu_status = BOOZ_IMU_STA_MEASURING_GYRO;
+  max1167_read();
+}

sw/airborne/booz/booz_imu.h

@@ -0,0 +1,116 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 BOOZ_IMU_H
+#define BOOZ_IMU_H
+
+#include "std.h"
+#include "6dof.h"
+
+#include "adc.h"
+#include "max1167.h"
+#include "micromag.h"
+#include "scp1000.h"
+
+#include "airframe.h"
+
+#define BOOZ_IMU_STA_IDLE           0
+#define BOOZ_IMU_STA_MEASURING_GYRO 1
+#define BOOZ_IMU_STA_MEASURING_MAG  2
+#define BOOZ_IMU_STA_MEASURING_BARO 3
+
+extern uint8_t booz_imu_status;
+/* calibrated sensors */
+extern float   imu_accel[AXIS_NB]; /* accelerometers in m/s2           */
+extern float   imu_gyro[AXIS_NB];  /* gyros in rad/s                   */
+extern float   imu_mag[AXIS_NB];   /* magnetometer in arbitrary unit   */
+extern float   imu_pressure;       /* static pressure in pascals       */
+
+/* raw sensors */
+extern uint16_t imu_accel_raw[AXIS_NB];
+extern uint16_t imu_gyro_raw[AXIS_NB];
+extern int16_t  imu_mag_raw[AXIS_NB];
+extern uint32_t imu_pressure_raw;
+
+/* internal ADCs */
+extern struct adc_buf buf_ax;
+extern struct adc_buf buf_ay;
+extern struct adc_buf buf_az;
+
+extern void booz_imu_init(void);
+extern void booz_imu_periodic(void);
+
+
+#define BoozImuScaleSensor3(_s_cal, _s_name, _s_raw) {			\
+    _s_cal[AXIS_X] = _s_name##_X_GAIN *					\
+      (float)((int32_t)_s_raw[AXIS_X] - _s_name##_X_NEUTRAL);		\
+    _s_cal[AXIS_Y] = _s_name##_Y_GAIN *					\
+      (float)((int32_t)_s_raw[AXIS_Y] - _s_name##_Y_NEUTRAL);		\
+    _s_cal[AXIS_Z] = _s_name##_Z_GAIN *					\
+      (float)((int32_t)_s_raw[AXIS_Z] - _s_name##_Z_NEUTRAL);		\
+  }
+
+#define BoozImuScaleSensor1(_s_cal, _s_name, _s_raw) {			\
+    _s_cal = _s_name##_GAIN *						\
+      (float)((int32_t)_s_raw - _s_name##_NEUTRAL);			\
+  }
+
+
+#define BoozImuEvent(accel_handler, gyro_handler, mag_handler, baro_handler) { \
+    if (max1167_status == STA_MAX1167_DATA_AVAILABLE) {			\
+      max1167_status = STA_MAX1167_IDLE;				\
+      imu_gyro_raw[AXIS_P] = max1167_values[IMU_GYRO_X_CHAN];		\
+      imu_gyro_raw[AXIS_Q] = max1167_values[IMU_GYRO_Y_CHAN];		\
+      imu_gyro_raw[AXIS_R] = max1167_values[IMU_GYRO_Z_CHAN];		\
+      BoozImuScaleSensor3(imu_gyro, IMU_GYRO, imu_gyro_raw);		\
+      									\
+      imu_accel_raw[AXIS_X] = buf_ax.sum;				\
+      imu_accel_raw[AXIS_Y] = buf_ay.sum;				\
+      imu_accel_raw[AXIS_Z] = buf_az.sum;				\
+      BoozImuScaleSensor3(imu_accel, IMU_ACCEL, imu_accel_raw);		\
+      gyro_handler();							\
+      accel_handler();							\
+    }									\
+    else if (micromag_status == MM_DATA_AVAILABLE) {			\
+      micromag_status = MM_IDLE;					\
+      imu_mag_raw[AXIS_X] = micromag_values[IMU_MAG_X_CHAN];		\
+      imu_mag_raw[AXIS_Y] = micromag_values[IMU_MAG_Y_CHAN];		\
+      imu_mag_raw[AXIS_Z] = micromag_values[IMU_MAG_Z_CHAN];		\
+      BoozImuScaleSensor3(imu_mag, IMU_MAG, imu_mag_raw);		\
+      mag_handler();							\
+    }									\
+    else if (scp1000_status == SCP1000_STA_DATA_AVAILABLE) {		\
+      scp1000_status = SCP1000_STA_WAIT_EOC;				\
+      imu_pressure_raw = scp1000_pressure;				\
+      BoozImuScaleSensor1(imu_pressure, IMU_PRESSURE, imu_pressure_raw); \
+      baro_handler();							\
+    }									\
+  }
+
+
+extern void booz_imu_hw_init(void);
+
+#include "booz_imu_hw.h"
+
+#endif /* BOOZ_IMU_H */

sw/airborne/booz/booz_ins.c

@@ -0,0 +1,69 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 "booz_ins.h"
+#include "6dof.h"
+#include "tl_vfilter.h"
+
+FLOAT_T booz_ins_x;
+FLOAT_T booz_ins_y;
+FLOAT_T booz_ins_z;
+
+FLOAT_T booz_ins_xdot;
+FLOAT_T booz_ins_ydot;
+FLOAT_T booz_ins_zdot;
+
+FLOAT_T booz_ins_qnh;
+FLOAT_T booz_ins_x_meas;
+FLOAT_T booz_ins_y_meas;
+FLOAT_T booz_ins_z_meas;
+FLOAT_T booz_ins_bax;
+FLOAT_T booz_ins_bay;
+FLOAT_T booz_ins_baz;
+
+uint8_t booz_ins_status;
+
+void booz_ins_init( void ) {
+  booz_ins_status = BOOZ_INS_STATUS_UNINIT;
+  
+
+}
+
+void booz_ins_start( const float* accel, const float pressure) {
+  booz_ins_qnh = pressure;
+  booz_ins_status = BOOZ_INS_STATUS_RUNNING;
+  tl_vf_init(0., 0., 0.);
+}
+
+void booz_ins_predict( const float* accel) {
+  tl_vf_predict(accel[AXIS_Z]);
+  booz_ins_z = tl_vf_z;
+  booz_ins_zdot = tl_vf_zdot;
+  booz_ins_baz = tl_vf_bias;
+}
+
+void booz_ins_update_pressure( const float pressure ) {
+  booz_ins_z_meas = (pressure - booz_ins_qnh)*0.084;
+  tl_vf_update(booz_ins_z_meas);
+}

sw/airborne/booz/booz_ins.h

@@ -0,0 +1,55 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008 Antoine Drouin
+ *
+ * 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 BOOZ_INS_H
+#define BOOZ_INS_H
+
+#include "std.h"
+
+extern FLOAT_T booz_ins_x;
+extern FLOAT_T booz_ins_y;
+extern FLOAT_T booz_ins_z;
+
+extern FLOAT_T booz_ins_xdot;
+extern FLOAT_T booz_ins_ydot;
+extern FLOAT_T booz_ins_zdot;
+
+extern FLOAT_T booz_ins_qnh;
+extern FLOAT_T booz_ins_x_meas;
+extern FLOAT_T booz_ins_y_meas;
+extern FLOAT_T booz_ins_z_meas;
+extern FLOAT_T booz_ins_bax;
+extern FLOAT_T booz_ins_bay;
+extern FLOAT_T booz_ins_baz;
+
+#define BOOZ_INS_STATUS_UNINIT  0
+#define BOOZ_INS_STATUS_RUNNING 1
+extern uint8_t booz_ins_status;
+
+extern void booz_ins_init( void );
+extern void booz_ins_start( const float* accel, const float pressure);
+extern void booz_ins_predict( const float* accel);
+extern void booz_ins_update_pressure( const float pressure );
+
+#endif /* BOOZ_INS_H */

sw/airborne/booz/booz_inter_mcu.c

@@ -0,0 +1,75 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_inter_mcu.h"
+
+#include "booz_imu.h"
+#include "booz_ahrs.h"
+#include "booz_ins.h"
+
+
+struct booz_inter_mcu_state inter_mcu_state;
+
+#ifdef BOOZ_FILTER_MCU
+
+//#define LP_GYROS
+
+void inter_mcu_fill_state() {
+#if 1
+#ifndef LP_GYROS
+  inter_mcu_state.r_rates[AXIS_P]  = imu_gyro[AXIS_P] * RATE_PI_S/M_PI;
+  inter_mcu_state.r_rates[AXIS_Q]  = imu_gyro[AXIS_Q] * RATE_PI_S/M_PI;
+  inter_mcu_state.r_rates[AXIS_R]  = imu_gyro[AXIS_R] * RATE_PI_S/M_PI;
+#else
+  inter_mcu_state.r_rates[AXIS_P]  = imu_gyro_lp[AXIS_P] * RATE_PI_S/M_PI;
+  inter_mcu_state.r_rates[AXIS_Q]  = imu_gyro_lp[AXIS_Q] * RATE_PI_S/M_PI;
+  inter_mcu_state.r_rates[AXIS_R]  = imu_gyro_lp[AXIS_R] * RATE_PI_S/M_PI;
+#endif
+  inter_mcu_state.f_rates[AXIS_P]  = booz_ahrs_p * RATE_PI_S/M_PI;
+  inter_mcu_state.f_rates[AXIS_Q]  = booz_ahrs_q * RATE_PI_S/M_PI;
+  inter_mcu_state.f_rates[AXIS_R]  = booz_ahrs_r * RATE_PI_S/M_PI;
+  inter_mcu_state.f_eulers[AXIS_X] = booz_ahrs_phi   * ANGLE_PI/M_PI;
+  inter_mcu_state.f_eulers[AXIS_Y] = booz_ahrs_theta * ANGLE_PI/M_PI;
+  inter_mcu_state.f_eulers[AXIS_Z] = booz_ahrs_psi   * ANGLE_PI/M_PI;
+  inter_mcu_state.pos[AXIS_X] = booz_ins_x;
+  inter_mcu_state.pos[AXIS_Y] = booz_ins_y;
+  inter_mcu_state.pos[AXIS_Z] = booz_ins_z;
+  inter_mcu_state.speed[AXIS_X] = booz_ins_xdot;
+  inter_mcu_state.speed[AXIS_Y] = booz_ins_ydot;
+  inter_mcu_state.speed[AXIS_Z] = booz_ins_zdot;
+  inter_mcu_state.status =  booz_ahrs_status;
+#else
+  inter_mcu_state.r_rates[AXIS_P]  = RATE_PI_S/M_PI;
+  inter_mcu_state.r_rates[AXIS_Q]  = RATE_PI_S/M_PI;
+  inter_mcu_state.r_rates[AXIS_R]  = RATE_PI_S/M_PI;
+  inter_mcu_state.f_rates[AXIS_P]  = RATE_PI_S/M_PI;
+  inter_mcu_state.f_rates[AXIS_Q]  = RATE_PI_S/M_PI;
+  inter_mcu_state.f_rates[AXIS_R]  = RATE_PI_S/M_PI;
+  inter_mcu_state.f_eulers[AXIS_X] = ANGLE_PI/M_PI;
+  inter_mcu_state.f_eulers[AXIS_Y] = ANGLE_PI/M_PI;
+  inter_mcu_state.f_eulers[AXIS_Z] = ANGLE_PI/M_PI;
+  inter_mcu_state.status =  1;
+#endif /* 0 */
+}
+#endif

sw/airborne/booz/booz_inter_mcu.h

@@ -0,0 +1,54 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 BOOZ_INTER_MCU_H
+#define BOOZ_INTER_MCU_H
+
+#include "std.h"
+#include "6dof.h"
+
+/* angles are transmitted as int16 : PI -> 16384 */
+/* rates are transmitted as int16 : 1 PI s-1 -> 16384 */
+#define ANGLE_PI 0X3FFF
+#define RATE_PI_S 0X3FFF
+
+struct booz_inter_mcu_state {
+  int16_t r_rates [AXIS_NB];
+  int16_t f_rates [AXIS_NB];
+  int16_t f_eulers[AXIS_NB];
+  int16_t pad0;
+  float   pos[AXIS_NB];
+  float   speed[AXIS_NB];
+  uint8_t status;
+  uint8_t pad1;
+  uint16_t crc;
+};
+
+extern struct booz_inter_mcu_state inter_mcu_state;
+
+#ifdef BOOZ_FILTER_MCU
+extern void inter_mcu_fill_state(void);
+#endif
+
+#endif /* BOOZ_INTER_MCU_H */

sw/airborne/booz/booz_link_mcu.c

@@ -0,0 +1,84 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_link_mcu.h"
+
+#include "std.h"
+
+struct booz_inter_mcu_state booz_link_mcu_state_unused;
+uint16_t booz_link_mcu_crc;
+
+
+
+#ifdef BOOZ_FILTER_MCU  /* FILTER LPC code */
+
+/* FIXME!!!! two function with same name in single MCU configuration */
+/* by chance  booz_link_mcu_hw_init does nothing in sim              */
+#ifndef SITL
+void booz_link_mcu_init ( void ) {
+  booz_link_mcu_hw_init();
+}
+#endif
+
+void booz_link_mcu_send ( void ) {
+  inter_mcu_fill_state();
+  BoozLinkMcuComputeCRC();
+  inter_mcu_state.crc = booz_link_mcu_crc;
+  BoozLinkMcuHwSend();
+}
+
+
+
+#endif /* BOOZ_FILTER_MCU */
+
+
+
+
+
+#ifdef BOOZ_CONTROLLER_MCU  /* lpc controller board */
+
+#include "spi.h"
+
+#include "booz_estimator.h"
+
+volatile uint8_t  booz_link_mcu_status;
+uint32_t booz_link_mcu_nb_err;
+uint32_t booz_link_mcu_timeout;
+#define BOOZ_LINK_MCU_TIMEOUT 100
+
+void booz_link_mcu_init ( void ) {
+
+  booz_link_mcu_hw_init();
+  booz_link_mcu_status = BOOZ_LINK_MCU_IDLE;
+  booz_link_mcu_nb_err = 0;
+  booz_link_mcu_timeout = BOOZ_LINK_MCU_TIMEOUT;
+
+}
+
+extern void booz_link_mcu_periodic( void ) {
+  if (booz_link_mcu_timeout < BOOZ_LINK_MCU_TIMEOUT)
+    booz_link_mcu_timeout++;
+}
+
+#endif /* BOOZ_CONTROLLER_MCU */

sw/airborne/booz/booz_link_mcu.h

@@ -0,0 +1,95 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 BOOZ_LINK_LINK_MCU_H
+#define BOOZ_LINK_LINK_MCU_H
+
+#include <inttypes.h>
+
+#include "6dof.h"
+
+#include "booz_link_mcu_hw.h"
+
+#include "booz_inter_mcu.h"
+
+extern void booz_link_mcu_init ( void );
+extern void booz_link_mcu_hw_init ( void );
+extern struct booz_inter_mcu_state booz_link_mcu_state_unused; /* single dir only */
+extern uint16_t booz_link_mcu_crc;
+
+#define BOOZ_LINK_MCU_CRC_INIT 0x0
+#define BOOZ_LINK_MCU_PAYLOAD_LENGTH (sizeof(struct booz_inter_mcu_state) - 2)
+#define BoozLinkMcuCrcLow(x) (((x)&0xff))
+#define BoozLinkMcuCrcHigh(x) (((x)>>8))
+#define BoozLinkMcuComputeCRC() {					\
+    uint8_t i;								\
+    booz_link_mcu_crc = BOOZ_LINK_MCU_CRC_INIT;				\
+    for(i = 0; i < BOOZ_LINK_MCU_PAYLOAD_LENGTH; i++) {			\
+      uint8_t _byte = ((uint8_t*)&inter_mcu_state)[i];			\
+      uint8_t a = ((uint8_t)BoozLinkMcuCrcHigh(booz_link_mcu_crc)) + _byte; \
+      uint8_t b = ((uint8_t)BoozLinkMcuCrcLow(booz_link_mcu_crc)) + a;	\
+      booz_link_mcu_crc = b | a << 8;					\
+    }									\
+  }
+
+#ifdef BOOZ_FILTER_MCU
+
+extern void booz_link_mcu_send( void );
+
+#endif /* BOOZ_FILTER_MCU */
+
+
+
+
+#ifdef BOOZ_CONTROLLER_MCU
+
+#define BOOZ_LINK_MCU_IDLE           0
+#define BOOZ_LINK_MCU_DATA_AVAILABLE 1
+#define BOOZ_LINK_MCU_SHORT_READ     2
+#define BOOZ_LINK_MCU_OVER_READ      3
+
+extern volatile uint8_t  booz_link_mcu_status;
+extern uint32_t booz_link_mcu_nb_err;
+extern uint32_t booz_link_mcu_timeout;
+extern void booz_link_mcu_periodic( void );
+
+#define BoozLinkMcuEventCheckAndHandle(handler) {		\
+    if (booz_link_mcu_status == BOOZ_LINK_MCU_DATA_AVAILABLE) {	\
+      BoozLinkMcuComputeCRC();					\
+      if (booz_link_mcu_crc == inter_mcu_state.crc) {		\
+	booz_link_mcu_timeout = 0;				\
+	handler();						\
+      }								\
+      else {							\
+	booz_link_mcu_nb_err++;					\
+      }								\
+      BoozLinkMcuHwRestart();					\
+      booz_link_mcu_status = BOOZ_LINK_MCU_IDLE;		\
+    }								\
+  }								\
+
+#endif /* BOOZ_CONTROLLER_MCU */
+
+
+#endif /* BOOZ_LINK_LINK_MCU_H */

sw/airborne/booz/booz_nav.c

@@ -0,0 +1,57 @@
+#include "booz_nav.h"
+
+#include <math.h>
+
+#include "booz_estimator.h"
+#include "booz_control.h"
+#include "radio_control.h"
+
+
+#define Block(x) case x: nav_block=x;
+#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); }
+#define NextBlock() { nav_block++; InitBlock(); }
+#define GotoBlock(b) { nav_block=b; InitBlock(); }
+
+#define Stage(s) case s: nav_stage=s;
+#define NextStage() { nav_stage++; InitStage() }
+#define NextStageFrom(wp) { last_wp = wp; NextStage() }
+#define GotoStage(s) { nav_stage = s; InitStage() }
+
+#define NavGotoWaypoint(_wp) {}
+#define NavVerticalAutoThrottleMode(_foo) {}
+#define NavVerticalAltitudeMode(_foo, _bar) {}
+
+#define NavCircleWaypoint(_foo, _bar) {}
+
+extern void nav_home(void);
+#define NAV_C
+#include "flight_plan.h"
+
+const uint8_t nb_waypoint;
+struct point waypoints[NB_WAYPOINT] = WAYPOINTS; 
+
+uint8_t nav_stage, nav_block;
+uint16_t stage_time, block_time;
+
+
+#include "booz_nav_hover.h"
+
+void booz_nav_init(void) {
+  nav_block = 0;
+  nav_stage = 0;
+  booz_nav_hover_init();
+}
+
+
+void booz_nav_run(void) {
+  booz_nav_hover_run();
+  BoozControlAttitudeSetSetPoints(booz_nav_hover_phi_command, booz_nav_hover_theta_command,
+				  booz_nav_hover_psi_sp, booz_nav_hover_power_command);
+}
+
+void booz_nav_read_setpoints_from_rc(void) {
+  booz_nav_hover_read_setpoints_from_rc();
+}
+
+
+

sw/airborne/booz/booz_nav.h

@@ -0,0 +1,35 @@
+#ifndef BOOZ_NAV_H
+#define BOOZ_NAV_H
+
+#include "std.h"
+
+#include "booz_nav_hover.h"
+
+extern void booz_nav_init(void);
+extern void booz_nav_run(void);
+extern void booz_nav_read_setpoints_from_rc(void);
+
+
+struct point {
+  float x;
+  float y;
+  float a;
+};
+
+#define WaypointX(_wp) (waypoints[_wp].x)
+#define WaypointY(_wp) (waypoints[_wp].y)
+#define WaypointAlt(_wp) (waypoints[_wp].a)
+
+extern const uint8_t nb_waypoint;
+extern struct point waypoints[]; 
+
+extern uint16_t stage_time, block_time;
+extern uint8_t nav_stage, nav_block;
+
+extern void nav_init_stage( void );
+#define InitStage() { nav_init_stage(); return; }
+
+
+
+
+#endif /* BOOZ_NAV_H */

sw/airborne/booz/booz_nav_hover.c

@@ -0,0 +1,185 @@
+#include "booz_nav.h"
+
+#include <math.h>
+
+#include "booz_estimator.h"
+#include "booz_control.h"
+#include "radio_control.h"
+
+float booz_nav_hover_x_sp;
+float booz_nav_hover_y_sp;
+float booz_nav_hover_z_sp;
+float booz_nav_hover_psi_sp;
+
+float booz_nav_hover_h_max_err;
+float booz_nav_hover_h_pgain;
+float booz_nav_hover_h_dgain;
+float booz_nav_hover_v_max_err;
+float booz_nav_hover_v_pgain;
+float booz_nav_hover_v_dgain;
+
+float booz_nav_hover_phi_command;
+float booz_nav_hover_theta_command;
+float booz_nav_hover_power_command;
+
+#define BOOZ_NAV_HOVER_H_MAX_ERR 10.
+/* rad / m */
+#define BOOZ_NAV_HOVER_H_PGAIN (RadOfDeg(20.) / BOOZ_NAV_HOVER_H_MAX_ERR)
+/* rad / (ms-1) */
+#define BOOZ_NAV_HOVER_H_DGAIN (RadOfDeg(5.))
+
+#define BOOZ_NAV_HOVER_MAX_PHI_COMMAND   RadOfDeg(30.)
+#define BOOZ_NAV_HOVER_MAX_THETA_COMMAND RadOfDeg(30.)
+
+#define BOOZ_NAV_HOVER_V_MAX_ERR 10.
+#define BOOZ_NAV_HOVER_V_PGAIN 0.01
+#define BOOZ_NAV_HOVER_V_DGAIN 0.015
+#define BOOZ_NAV_HOVER_V_HOVER_POWER 0.318
+
+#ifdef NAV_VERTICAL
+static void booz_nav_hover_vertical_loop_run(void);
+#endif
+#ifdef NAV_HORIZONTAL
+static void booz_nav_hover_horizontal_loop_run(void);
+#endif
+
+void booz_nav_hover_init(void) {
+
+  booz_nav_hover_x_sp = 0.;
+  booz_nav_hover_y_sp = 0.;
+  booz_nav_hover_z_sp = 0.;
+  booz_nav_hover_psi_sp = 0.;
+
+  booz_nav_hover_h_max_err =  BOOZ_NAV_HOVER_H_MAX_ERR;
+  booz_nav_hover_h_pgain = BOOZ_NAV_HOVER_H_PGAIN;
+  booz_nav_hover_h_dgain = BOOZ_NAV_HOVER_H_DGAIN;
+  booz_nav_hover_v_max_err = BOOZ_NAV_HOVER_V_MAX_ERR;
+  booz_nav_hover_v_pgain = BOOZ_NAV_HOVER_V_PGAIN;
+  booz_nav_hover_v_dgain = BOOZ_NAV_HOVER_V_DGAIN;
+
+  booz_nav_hover_phi_command = 0.;
+  booz_nav_hover_theta_command = 0.;
+  booz_nav_hover_power_command = 0.;
+
+}
+
+void booz_nav_hover_run(void) {
+#ifdef NAV_VERTICAL
+  booz_nav_hover_vertical_loop_run();
+#endif
+#ifdef NAV_HORIZONTAL
+  booz_nav_hover_horizontal_loop_run();
+#endif
+}
+
+void booz_nav_hover_read_setpoints_from_rc(void) {
+#ifdef NAV_VERTICAL
+  /* direct altitude between 1 and -9 meters */
+  booz_nav_hover_z_sp = 1. - 10. / MAX_PPRZ * (float)rc_values[RADIO_THROTTLE];
+#else
+  booz_nav_hover_power_command = (float)rc_values[RADIO_THROTTLE] / MAX_PPRZ;
+#endif
+
+#ifdef NAV_HORIZONTAL
+  booz_nav_hover_x_sp = 0.;
+  booz_nav_hover_y_sp = 0.;
+  booz_nav_hover_psi_sp = 0.;
+#else
+  booz_nav_hover_phi_command = (float)-rc_values[RADIO_ROLL]   / MAX_PPRZ;
+  booz_nav_hover_theta_command = (float)rc_values[RADIO_PITCH] / MAX_PPRZ;;
+  booz_nav_hover_psi_sp = 0.;
+#endif
+}
+
+
+
+
+
+#ifdef NAV_VERTICAL
+static void booz_nav_hover_vertical_loop_run(void) {
+  float vertical_err = booz_estimator_z - booz_nav_hover_z_sp;
+  Bound(vertical_err, -booz_nav_hover_v_max_err, booz_nav_hover_v_max_err);
+  float bank_coef = cos(booz_estimator_phi)*cos(booz_estimator_theta);
+  if (bank_coef < 1e-1) bank_coef = 1e-1;
+  float adjusted_hover_power  = BOOZ_NAV_HOVER_V_HOVER_POWER / bank_coef;
+  booz_nav_hover_power_command = adjusted_hover_power +
+                                 booz_nav_hover_v_pgain * vertical_err +
+                                 booz_nav_hover_v_dgain * booz_estimator_vz;
+
+  Bound(booz_nav_hover_power_command, 0., 1.);
+
+}
+#endif
+
+
+
+
+
+#ifdef NAV_HORIZONTAL
+static void booz_nav_hover_horizontal_loop_run(void) {
+  
+  /* get a position error vector              */
+  float x_error = booz_estimator_x - booz_nav_hover_x_sp;
+  float y_error = booz_estimator_y - booz_nav_hover_y_sp;
+  //  printf("earth pos error %f %f\n", x_error, y_error); 
+  /* get norm and unit position error vector  */  
+  float norm_error = sqrt(x_error*x_error+ y_error*y_error);
+  float unit_x_error = x_error / norm_error;
+  float unit_y_error = y_error / norm_error;
+  //  printf("earth pos error %f %f %f\n", unit_x_error, unit_y_error, norm_error); 
+  /* bound the error in amplitude             */
+  Bound(norm_error, 0., booz_nav_hover_h_max_err);
+  /* convert unit error vector to body frame       */
+  float x_unit_err_body = unit_x_error * booz_estimator_dcm[AXIS_X][AXIS_X] +
+                          unit_y_error * booz_estimator_dcm[AXIS_X][AXIS_Y];
+  float y_unit_err_body = unit_x_error * booz_estimator_dcm[AXIS_Y][AXIS_X] +
+                          unit_y_error * booz_estimator_dcm[AXIS_Y][AXIS_Y];
+  //  printf("body  pos error %f %f\n", x_unit_err_body, y_unit_err_body); 
+  /* */
+
+  booz_nav_hover_phi_command   =  - booz_nav_hover_h_pgain * y_unit_err_body * norm_error +
+                                  - booz_nav_hover_h_dgain *  booz_estimator_v;
+  booz_nav_hover_theta_command =    booz_nav_hover_h_pgain * x_unit_err_body * norm_error +
+                                    booz_nav_hover_h_dgain *  booz_estimator_u;
+  
+  Bound(booz_nav_hover_phi_command, -BOOZ_NAV_HOVER_MAX_PHI_COMMAND, BOOZ_NAV_HOVER_MAX_PHI_COMMAND);
+  Bound(booz_nav_hover_theta_command, -BOOZ_NAV_HOVER_MAX_THETA_COMMAND, BOOZ_NAV_HOVER_MAX_THETA_COMMAND);
+
+}
+#endif
+
+
+
+
+# if 0
+#define STICK_ABSOLUTE_POS   0
+#define STICK_ABSOLUTE_SPEED 1
+#define STICK_RELATIVE_SPEED 2
+#define STICK_MODE STICK_RELATIVE_SPEED
+#define DT_READ_SETPOINTS 0.004
+void booz_nav_hover_read_setpoints_from_rc(void) {
+#ifndef DISABLE_NAV
+  booz_nav_hover_z_sp   = -1. - 10. / MAX_PPRZ * (float)rc_values[RADIO_THROTTLE];
+#if defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_POS
+  booz_nav_horizontal_x_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/- 20m */
+  booz_nav_horizontal_y_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_ROLL];  /* warning RC roll negativ to the right !! +/- 20m */
+#elif defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_SPEED
+  booz_nav_horizontal_x_sp += -5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/-5 m/s */
+  booz_nav_horizontal_y_sp +=  5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_ROLL];  /* +/-5 m/s */
+#elif defined STICK_MODE && STICK_MODE == STICK_RELATIVE_SPEED
+  float booz_nav_hover_dx_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_PITCH];
+  float booz_nav_hover_dy_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* warning RC roll negativ to the right !! +/- 5m/s */
+  /* convert vector to earth frame ( use transpose of DCM )  */
+  float booz_nav_hover_dx_earth = booz_nav_hover_dx_bod * booz_estimator_dcm[AXIS_X][AXIS_X] +
+                                  booz_nav_hover_dy_bod * booz_estimator_dcm[AXIS_Y][AXIS_X];
+  float booz_nav_hover_dy_earth = booz_nav_hover_dx_bod * booz_estimator_dcm[AXIS_X][AXIS_Y] +
+                                  booz_nav_hover_dy_bod * booz_estimator_dcm[AXIS_Y][AXIS_Y];
+  booz_nav_hover_x_sp += (booz_nav_hover_dx_earth * DT_READ_SETPOINTS);
+  booz_nav_hover_y_sp += (booz_nav_hover_dy_earth * DT_READ_SETPOINTS);
+  booz_nav_hover_psi_sp += (-RadOfDeg(60.) * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_YAW]);
+  booz_nav_hover_psi_sp = RadOfDeg(0.);
+  NormRadAngle(booz_nav_hover_psi_sp);
+#endif /* STICK_MODE  */
+#endif /* DISABLE_NAV */
+}
+#endif /* 0 */

sw/airborne/booz/booz_nav_hover.h

@@ -0,0 +1,28 @@
+#ifndef BOOZ_NAV_HOVER_H
+#define BOOZ_NAV_HOVER_H
+
+
+#ifndef DISABLE_NAV
+extern float booz_nav_hover_x_sp;
+extern float booz_nav_hover_y_sp;
+extern float booz_nav_hover_z_sp;
+extern float booz_nav_hover_psi_sp;
+
+extern float booz_nav_hover_h_max_err;
+extern float booz_nav_hover_h_pgain;
+extern float booz_nav_hover_h_dgain;
+extern float booz_nav_hover_v_max_err;
+extern float booz_nav_hover_v_pgain;
+extern float booz_nav_hover_v_dgain;
+
+extern float booz_nav_hover_phi_command;
+extern float booz_nav_hover_theta_command;
+extern float booz_nav_hover_power_command;
+#endif
+
+extern void booz_nav_hover_init(void);
+extern void booz_nav_hover_run(void);
+extern void booz_nav_hover_read_setpoints_from_rc(void);
+
+
+#endif /* BOOZ_NAV_HOVER_H */

sw/airborne/booz/booz_still_detection.c

@@ -0,0 +1,136 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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 "booz_still_detection.h"
+#include "booz_imu.h"
+#include "sys_time.h"
+
+uint8_t booz_still_detection_status;
+float booz_still_detection_accel[AXIS_NB];
+float booz_still_detection_gyro[AXIS_NB];
+float booz_still_detection_mag[AXIS_NB];
+float booz_still_detection_pressure;
+
+#define BSD_BUF_LEN 128
+
+static uint16_t bsd_accel_raw[AXIS_NB][BSD_BUF_LEN];
+static uint32_t bsd_accel_raw_sum[AXIS_NB];
+uint32_t bsd_accel_raw_avg[AXIS_NB];
+float    bsd_accel_raw_var[AXIS_NB];
+
+static uint16_t bsd_gyro_raw[AXIS_NB][BSD_BUF_LEN];
+static uint32_t bsd_gyro_raw_sum[AXIS_NB];
+static uint32_t bsd_gyro_raw_avg[AXIS_NB];
+static float    bsd_gyro_raw_var[AXIS_NB];
+
+static int16_t  bsd_mag_raw[AXIS_NB][BSD_BUF_LEN];
+static int32_t  bsd_mag_raw_sum[AXIS_NB];
+static int32_t  bsd_mag_raw_avg[AXIS_NB];
+static float    bsd_mag_raw_var[AXIS_NB];
+
+static uint32_t bsd_pressure_raw[BSD_BUF_LEN];
+static uint32_t bsd_pressure_raw_sum;
+static uint32_t bsd_pressure_raw_avg;
+//static float    bsd_pressure_raw_var;
+
+static int16_t  bsd_buf_head;
+static bool_t bsd_buf_filled;
+
+static inline void bsd_set_initial_values(void);
+
+#define BsdUpdateAverageArray(_sensor) {				         \
+    bsd_##_sensor##_raw_sum[axis] -= bsd_##_sensor##_raw[axis][bsd_buf_head];    \
+    bsd_##_sensor##_raw[axis][bsd_buf_head] = imu_##_sensor##_raw[axis];         \
+    bsd_##_sensor##_raw_sum[axis] += imu_##_sensor##_raw[axis];	                 \
+    bsd_##_sensor##_raw_avg[axis] = bsd_##_sensor##_raw_sum[axis] / BSD_BUF_LEN; \
+  }
+
+#define BsdUpdateAverageScal(_sensor) {				        \
+    bsd_##_sensor##_raw_sum -= bsd_##_sensor##_raw[bsd_buf_head];	\
+    bsd_##_sensor##_raw[bsd_buf_head] = imu_##_sensor##_raw;		\
+    bsd_##_sensor##_raw_sum += imu_##_sensor##_raw;			\
+    bsd_##_sensor##_raw_avg = bsd_##_sensor##_raw_sum / BSD_BUF_LEN;    \
+  }
+
+void booz_still_detection_init(void) {
+  bsd_buf_filled = FALSE;
+  bsd_buf_head = -1;
+  booz_still_detection_status = BSD_STATUS_UNINIT;
+}
+
+void booz_still_detection_run(void) {
+  if (booz_still_detection_status == BSD_STATUS_LOCKED)
+    return;
+  /* update sliding average of sensors */
+  bsd_buf_head++;
+  if (bsd_buf_head >= BSD_BUF_LEN) {
+    bsd_buf_filled = TRUE;
+    bsd_buf_head = 0;
+  } 
+  
+  uint8_t axis, j;
+  for (axis=0; axis<AXIS_NB; axis++) {
+    BsdUpdateAverageArray(accel);
+    BsdUpdateAverageArray(gyro);
+    BsdUpdateAverageArray(mag);
+  }
+  BsdUpdateAverageScal(pressure);
+
+  /* update sensors variance */
+  if (bsd_buf_filled) {
+    for (axis=0; axis<AXIS_NB; axis++) {  
+
+      bsd_accel_raw_var[axis] = 0.;
+      bsd_gyro_raw_var[axis] = 0.;
+      bsd_mag_raw_var[axis] = 0.;
+
+      for (j=0; j<BSD_BUF_LEN; j++) {
+	int32_t diff;
+	diff = bsd_gyro_raw[axis][j] - bsd_gyro_raw_avg[axis];
+	bsd_gyro_raw_var[axis] += (float)(diff*diff);
+	diff = bsd_accel_raw[axis][j] - bsd_accel_raw_avg[axis];
+	bsd_accel_raw_var[axis] += (float)(diff*diff);
+	diff = bsd_mag_raw[axis][j] - bsd_mag_raw_avg[axis];
+	bsd_mag_raw_var[axis] += (float)(diff*diff);
+      }
+      bsd_gyro_raw_var[axis] /= (float)BSD_BUF_LEN;
+      bsd_accel_raw_var[axis] /= (float)BSD_BUF_LEN;
+      bsd_mag_raw_var[axis] /= (float)BSD_BUF_LEN;
+    }
+    /* check vehicle still */
+    if (cpu_time_sec > 2 &&
+        bsd_accel_raw_var[AXIS_X] < BSD_ACCEL_RAW_MAX_VAR &&
+	bsd_accel_raw_var[AXIS_Y] < BSD_ACCEL_RAW_MAX_VAR &&
+	bsd_accel_raw_var[AXIS_Z] < BSD_ACCEL_RAW_MAX_VAR )
+      bsd_set_initial_values();
+  }
+}
+
+static inline void bsd_set_initial_values(void) {
+      booz_still_detection_status = BSD_STATUS_LOCKED;
+      BoozImuScaleSensor3(booz_still_detection_accel, IMU_ACCEL, bsd_accel_raw_avg);
+      BoozImuScaleSensor3(booz_still_detection_gyro,  IMU_GYRO,  bsd_gyro_raw_avg);
+      BoozImuScaleSensor3(booz_still_detection_mag,   IMU_MAG,   bsd_mag_raw_avg);
+      BoozImuScaleSensor1(booz_still_detection_pressure, IMU_PRESSURE, bsd_pressure_raw_avg);
+}

sw/airborne/booz/booz_still_detection.h

@@ -0,0 +1,53 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2008  Antoine Drouin
+ *
+ * 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. 
+ *
+ */
+
+/* 
+   detect a still vehicle by monitoring variance of imu sensors 
+*/ 
+
+
+#ifndef BOOZ_STILL_DETECTION_H
+#define BOOZ_STILL_DETECTION_H
+
+#include "std.h"
+#include "6dof.h"
+
+extern float booz_still_detection_accel[AXIS_NB];
+extern float booz_still_detection_gyro[AXIS_NB];
+extern float booz_still_detection_mag[AXIS_NB];
+extern float booz_still_detection_pressure;
+
+extern uint32_t bsd_accel_raw_avg[AXIS_NB];
+extern float    bsd_accel_raw_var[AXIS_NB];
+
+extern uint8_t booz_still_detection_status;
+#define BSD_STATUS_UNINIT 0
+#define BSD_STATUS_LOCKED 1
+
+extern void booz_still_detection_init(void);
+extern void booz_still_detection_run(void);
+
+#define BSD_ACCEL_RAW_MAX_VAR 7000.
+
+#endif /* BOOZ_STILL_DETECTION_H */

sw/airborne/booz/test/booz_test_imu.c

@@ -0,0 +1,90 @@
+#include "std.h"
+#include "init_hw.h"
+#include "interrupt_hw.h"
+#include "sys_time.h"
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "booz_imu.h"
+
+static inline void main_init(void);
+static inline void main_periodic(void);
+static inline void main_event(void);
+
+uint32_t t0, t1, diff;
+
+int main( void ) {
+  main_init();
+  while (1) {
+    if (sys_time_periodic())
+      main_periodic();
+    main_event();
+  }
+  return 0;
+}
+
+
+static inline void main_init(void) {
+  hw_init();
+
+  sys_time_init();
+
+  uart1_init_tx();
+
+  booz_imu_init();
+
+  int_enable();
+}
+
+
+static inline void main_periodic(void) {
+  //  t0 = T0TC;
+  booz_imu_periodic();
+  RunOnceEvery(250,DOWNLINK_SEND_BOOT(&cpu_time_sec)); 
+}
+
+
+static inline void on_imu_accel_available(void) {
+  RunOnceEvery(4, {				\
+      DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], &imu_accel_raw[AXIS_Y], &imu_accel_raw[AXIS_Z]); \
+      DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]); \
+    });
+}
+
+static inline void on_imu_gyro_available(void) {
+  //DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_raw[AXIS_P], &imu_gyro_raw[AXIS_Q], &imu_gyro_raw[AXIS_R]);
+  //DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_P], &imu_gyro[AXIS_Q], &imu_gyro[AXIS_R]);
+  //  t1 = T0TC;
+  //  diff = t1 - t0;
+  //  DOWNLINK_SEND_TIME(&diff);
+}
+
+static inline void on_imu_mag_available(void) {
+  DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], &imu_mag_raw[AXIS_Y], &imu_mag_raw[AXIS_Z]);
+  DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
+  //  t1 = T0TC;
+  //  diff = t1 - t0;
+  //  DOWNLINK_SEND_TIME(&diff);
+  //  t0 = t1;
+}
+
+static inline void on_imu_baro_available(void) {
+  float pressure = 0.25 * imu_pressure_raw;
+  static float ground_pressure = 0.;
+  if (ground_pressure == 0) ground_pressure = pressure;
+  float z_baro = (ground_pressure - pressure)*0.084;
+  DOWNLINK_SEND_TL_IMU_PRESSURE(&pressure);
+  DOWNLINK_SEND_TL_IMU_RANGEMETER(&z_baro);
+    t1 = T0TC;
+    diff = t1 - t0;
+    DOWNLINK_SEND_TIME(&diff);
+    t0 = t1;
+}
+
+static inline void main_event(void) {
+  BoozImuEvent(on_imu_accel_available, on_imu_gyro_available, on_imu_mag_available, on_imu_baro_available);
+
+}
+

sw/airborne/booz/test/booz_test_link_mcu_ctl.c

@@ -0,0 +1,77 @@
+#include "std.h"
+#include "init_hw.h"
+#include "interrupt_hw.h"
+#include "sys_time.h"
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "booz_link_mcu.h"
+
+static inline void main_init(void);
+static inline void main_periodic(void);
+static inline void main_event(void);
+
+int main( void ) {
+  main_init();
+  while (1) {
+    if (sys_time_periodic())
+      main_periodic();
+    main_event();
+  }
+  return 0;
+}
+
+
+static inline void main_init(void) {
+  hw_init();
+
+  sys_time_init();
+
+  uart1_init_tx();
+
+  booz_link_mcu_init();
+
+  int_enable();
+}
+
+extern volatile uint32_t it_cnt;
+extern volatile uint32_t rx_it_cnt;
+extern volatile uint32_t ti_it_cnt;
+
+static inline void main_periodic(void) {
+  RunOnceEvery(250, {					\
+      static uint32_t it_last;				\
+      uint16_t it_nb = it_cnt - it_last;		\
+      it_last = it_cnt;					\
+      DOWNLINK_SEND_TAKEOFF(&it_nb);			\
+      static float rx_it_last;				\      
+      float diff_rx = rx_it_cnt - rx_it_last;		\
+      rx_it_last = rx_it_cnt;				\
+      static float ti_it_last;				\
+      float diff_ti = ti_it_cnt - ti_it_last;		\
+      ti_it_last = ti_it_cnt;				\
+      DOWNLINK_SEND_BOOZ_DEBUG_BAR(&diff_rx, &diff_ti);	\
+    });
+}
+
+
+static inline void on_link_event(void) {
+  RunOnceEvery(250, {					\
+
+      uint8_t f0 = link_mcu_rx_buf[0];			\
+      uint8_t f1 = link_mcu_rx_buf[1];			\
+      uint8_t f2 = link_mcu_rx_buf[2];			\
+      uint8_t f3 = link_mcu_rx_buf[3];			\
+      DOWNLINK_SEND_BOOZ_CMDS(&f0, &f1, &f2, &f3);	\
+    });
+
+
+
+}
+
+static inline void main_event(void) {
+  BoozLinkMcuEventCheckAndHandle(on_link_event);
+}
+

sw/airborne/booz/test/booz_test_link_mcu_imu.c

@@ -0,0 +1,57 @@
+#include "std.h"
+#include "init_hw.h"
+#include "interrupt_hw.h"
+#include "sys_time.h"
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "booz_link_mcu.h"
+
+static inline void main_init(void);
+static inline void main_periodic(void);
+static inline void main_event(void);
+
+int main( void ) {
+  main_init();
+  while (1) {
+    if (sys_time_periodic())
+      main_periodic();
+    main_event();
+  }
+  return 0;
+}
+
+
+static inline void main_init(void) {
+  hw_init();
+
+  sys_time_init();
+
+  uart1_init_tx();
+
+  booz_link_mcu_init();
+
+  int_enable();
+}
+
+extern uint8_t foo_debug;
+extern uint32_t it_count;
+
+static inline void main_periodic(void) {
+  RunOnceEvery(250, {				\
+      static uint32_t it_last;			\
+      uint16_t it_nb = it_count - it_last;	\
+      it_last = it_count;			\
+      DOWNLINK_SEND_BOOT(&it_nb)		\
+	});
+  //  RunOnceEvery(250,DOWNLINK_SEND_TAKEOFF(&foo_debug)); 
+  booz_link_mcu_send();
+}
+
+
+static inline void main_event(void) {
+
+}
+