[math] add double_quat_vmult, fixes for nps

paparazzi · Aug 2, 2014 · 0f71187 · 0f71187
1 parent 062d322
commit 0f71187
Show file tree

Hide file tree

Showing 6 changed files with 29 additions and 5 deletions.
diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h
@@ -173,6 +173,30 @@ static inline void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQu
   e->psi = atan2(dcm01, dcm00);
 }
 
+#define DOUBLE_QUAT_VMULT(v_out, q, v_in) double_quat_vmult(&(v_out), &(q), &(v_in))
+static inline void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q,struct DoubleVect3 * v_in) {
+  const double qi2_M1_2  = q->qi*q->qi - 0.5;
+  const double qiqx = q->qi*q->qx;
+  const double qiqy = q->qi*q->qy;
+  const double qiqz = q->qi*q->qz;
+  double m01  = q->qx*q->qy;   /* aka qxqy */
+  double m02  = q->qx*q->qz;   /* aka qxqz */
+  double m12  = q->qy*q->qz;   /* aka qyqz */
+
+  const double m00  = qi2_M1_2 + q->qx*q->qx;
+  const double m10  = m01 - qiqz;
+  const double m20  = m02 + qiqy;
+  const double m21  = m12 - qiqx;
+  m01 += qiqz;
+  m02 -= qiqy;
+  m12 += qiqx;
+  const double m11  = qi2_M1_2 + q->qy*q->qy;
+  const double m22  = qi2_M1_2 + q->qz*q->qz;
+  v_out->x = 2*(m00 * v_in->x + m01 * v_in->y + m02 * v_in->z);
+  v_out->y = 2*(m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
+  v_out->z = 2*(m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
+}
+
 /* multiply _vin by _mat, store in _vout */
 #define DOUBLE_MAT33_VECT3_MUL(_vout, _mat, _vin) {     \
     (_vout).x = (_mat)[0]*(_vin).x + (_mat)[1]*(_vin).y + (_mat)[2]*(_vin).z;   \

diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
@@ -727,7 +727,7 @@ static inline void float_quat_integrate(struct FloatQuat *q, struct FloatRates *
 }
 
 #define FLOAT_QUAT_VMULT(v_out, q, v_in) float_quat_vmult(&(v_out), &(q), &(v_in))
-static inline void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q,struct FloatVect3 * v_in) {
+static inline void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, struct FloatVect3 *v_in) {
   const float qi2_M1_2  = q->qi*q->qi - 0.5;
   const float qiqx = q->qi*q->qx;
   const float qiqy = q->qi*q->qy;

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
@@ -218,7 +218,7 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
 
   /* converted expected measurement from inertial to body frame */
   struct FloatVect3 b_expected;
-  FLOAT_QUAT_VMULT(b_expected, ahrs_impl.ltp_to_imu_quat, *i_expected);
+  float_quat_vmult(&b_expected, &ahrs_impl.ltp_to_imu_quat, (struct FloatVect3*)i_expected);
 
   // S = HPH' + JRJ
   float H[3][6] = {{           0., -b_expected.z,  b_expected.y, 0., 0., 0.},

diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c
@@ -189,7 +189,7 @@ void nps_ivy_display(void) {
 
   /* transform magnetic field to body frame */
   struct DoubleVect3 h_body;
-  FLOAT_QUAT_VMULT(h_body, fdm.ltp_to_body_quat, fdm.ltp_h);
+  double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h);
 
   IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
          AC_ID,

diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c
@@ -32,7 +32,7 @@ void   nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
 
   /* transform gravity to body frame */
   struct DoubleVect3 g_body;
-  FLOAT_QUAT_VMULT(g_body, fdm.ltp_to_body_quat, fdm.ltp_g);
+  double_quat_vmult(&g_body, &fdm.ltp_to_body_quat, &fdm.ltp_g);
   //  printf(" g_body %f %f %f\n", g_body.x, g_body.y, g_body.z);
 
   //  printf(" accel_fdm %f %f %f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);

diff --git a/sw/simulator/nps/nps_sensor_mag.c b/sw/simulator/nps/nps_sensor_mag.c
@@ -29,7 +29,7 @@ void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct Doubl
 
   /* transform magnetic field to body frame */
   struct DoubleVect3 h_body;
-  FLOAT_QUAT_VMULT(h_body, fdm.ltp_to_body_quat, fdm.ltp_h);
+  double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h);
 
   /* transform to imu frame */
   struct DoubleVect3 h_imu;