From 4bfab9d05000e91c6a6a5a8d45f55e5ea6269543 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Tue, 1 Sep 2015 21:46:25 +0200
Subject: [PATCH] [fixedwing] energy_ctrl: use ABI instead of imu struct

---
 .../fixedwing/guidance/energy_ctrl.c          | 42 ++++++++++++++++---
 1 file changed, 36 insertions(+), 6 deletions(-)

diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
index ac53e7388f..4f052fa3d5 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
@@ -66,8 +66,7 @@
 #include "firmwares/fixedwing/nav.h"
 #include "generated/airframe.h"
 #include "firmwares/fixedwing/autopilot.h"
-#include "subsystems/ahrs.h"
-#include "subsystems/imu.h"
+#include "subsystems/abi.h"
 
 /////// DEFAULT GUIDANCE_V NECESSITIES //////
 
@@ -134,6 +133,13 @@ pprz_t v_ctl_throttle_slewed;
 float v_ctl_pitch_setpoint;
 
 
+static struct FloatQuat imu_to_body_quat;
+static struct Int32Vect3 accel_imu_meas;
+
+static abi_event accel_ev;
+static abi_event body_to_imu_ev;
+
+
 ///////////// DEFAULT SETTINGS ////////////////
 #ifndef V_CTL_ALTITUDE_MAX_CLIMB
 #define V_CTL_ALTITUDE_MAX_CLIMB 2;
@@ -153,6 +159,10 @@ INFO("V_CTL_GLIDE_RATIO not defined - default is 8.")
 #ifndef V_CTL_MAX_ACCELERATION
 #define V_CTL_MAX_ACCELERATION 0.5
 #endif
+
+#ifndef V_CTL_ENERGY_IMU_ID
+#define V_CTL_ENERGY_IMU_ID ABI_BROADCAST
+#endif
 /////////////////////////////////////////////////
 // Automatically found airplane characteristics
 
@@ -190,6 +200,18 @@ static void ac_char_update(float throttle, float pitch, float climb, float accel
   }
 }
 
+static void accel_cb(uint8_t sender_id __attribute__((unused)),
+                     uint32_t stamp __attribute__((unused)),
+                     struct Int32Vect3 *accel)
+{
+  accel_imu_meas = *accel;
+}
+
+static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
+                           struct FloatQuat *q_b2i_f)
+{
+  float_quat_invert(&imu_to_body_quat, q_b2i_f);
+}
 
 void v_ctl_init(void)
 {
@@ -256,6 +278,11 @@ void v_ctl_init(void)
 #endif
 
   v_ctl_throttle_setpoint = 0;
+
+  float_quat_identity(&imu_to_body_quat);
+
+  AbiBindMsgIMU_ACCEL_INT32(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
 }
 
 const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
@@ -340,10 +367,13 @@ void v_ctl_climb_loop(void)
 
   // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
 #ifndef SITL
-  struct Int32Vect3 accel_meas_body;
-  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
-  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
-  float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
+  /* convert last imu accel measurement to float */
+  struct FloatVect3 accel_imu_f;
+  ACCELS_BFP_OF_REAL(accel_imu_f, accel_imu_meas);
+  /* rotate from imu to body frame */
+  struct FloatVect3 accel_meas_body;
+  float_quat_vmult(&accel_meas_body, &imu_to_body_quat, &accel_imu_f);
+  float vdot = accel_meas_body.x / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
 #else
   float vdot = 0;
 #endif