From 861986abae7c7d6be1f5a89361d9aa9b8b86e44e Mon Sep 17 00:00:00 2001
From: Michal Podhradsky <podhrmic@users.noreply.github.com>
Date: Thu, 16 Feb 2017 14:53:48 -0800
Subject: [PATCH] Vectornav as AHRS, minor fix in the float_euler stabilization
 settings (#2013)

---
 conf/modules/stabilization_float_euler.xml        |  2 +-
 sw/airborne/subsystems/ahrs/ahrs_vectornav.c      | 15 +++++++++------
 .../subsystems/ahrs/ahrs_vectornav_wrapper.c      |  4 ++++
 .../subsystems/ahrs/ahrs_vectornav_wrapper.h      |  1 +
 4 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/conf/modules/stabilization_float_euler.xml b/conf/modules/stabilization_float_euler.xml
index 78b691b0fb..ca0bfc16b1 100644
--- a/conf/modules/stabilization_float_euler.xml
+++ b/conf/modules/stabilization_float_euler.xml
@@ -49,7 +49,7 @@
         <dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN" persistent="true"/>
         <dl_setting var="stabilization_gains.rates_d.y" min="0" step="1" max="500" shortname="dgaind q" param="STABILIZATION_ATTITUDE_THETA_DGAIN_D" persistent="true"/>
         <dl_setting var="stabilization_gains.dd.y" min="0"    step="1" max="1000" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN" persistent="true"/>
-        <dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN" persistent="true"/>
+        <dl_setting var="stabilization_gains.p.z" min="1" step="1" max="8000" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN" persistent="true"/>
         <dl_setting var="stabilization_gains.i.z" min="0"  step="1" max="400" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN" persistent="true"/>
         <dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN" persistent="true"/>
         <dl_setting var="stabilization_gains.rates_d.z" min="0" step="1" max="500" shortname="dgaind r" param="STABILIZATION_ATTITUDE_PHI_DGAIN_D" persistent="true"/>
diff --git a/sw/airborne/subsystems/ahrs/ahrs_vectornav.c b/sw/airborne/subsystems/ahrs/ahrs_vectornav.c
index e3ff966cfc..7f5ed5bc8d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_vectornav.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_vectornav.c
@@ -72,11 +72,17 @@ void ahrs_vectornav_event(void)
   if (ahrs_vn.vn_packet.msg_available) {
     vn200_read_message(&(ahrs_vn.vn_packet),&(ahrs_vn.vn_data));
     ahrs_vn.vn_packet.msg_available = false;
-    ahrs_vectornav_propagate();
+
+    if (ahrs_vectornav_is_enabled()) {
+      ahrs_vectornav_propagate();
+    }
 
     // send ABI messages
     uint32_t now_ts = get_sys_time_usec();
+    // in fixed point for sending as ABI and telemetry msgs
+    RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro);
     AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i);
+    ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel);
     AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i);
   }
 }
@@ -124,11 +130,8 @@ void ahrs_vectornav_propagate(void)
   // Attitude [deg]
   static struct FloatQuat imu_quat; // convert from euler to quat
   float_quat_of_eulers(&imu_quat, &ahrs_vn.vn_data.attitude);
-  static struct FloatRMat imu_rmat; // convert from quat to rmat
-  float_rmat_of_quat(&imu_rmat, &imu_quat);
-  static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
-  float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ahrs_vn.body_to_imu));
-  stateSetNedToBodyRMat_f(&ltp_to_body_rmat); // set body states [rad]
+  stateSetNedToBodyQuat_f(&imu_quat);
+
 }
 
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.c
index ed2c482a03..b0e5c43e70 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.c
@@ -59,6 +59,10 @@ static bool ahrs_vectornav_enable_output(bool enable)
   return ahrs_vectornav_output_enabled;
 }
 
+bool ahrs_vectornav_is_enabled(void){
+  return ahrs_vectornav_output_enabled;
+}
+
 void ahrs_vectornav_register(void)
 {
   ahrs_vectornav_output_enabled = AHRS_VECTORNAV_OUTPUT_ENABLED;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.h
index 2f0f888cb0..caeb8c3dd7 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.h
@@ -35,5 +35,6 @@
 #endif
 
 extern void ahrs_vectornav_register(void);
+extern bool ahrs_vectornav_is_enabled(void);
 
 #endif /* AHRS_VECTORNAV_WRAPPER_H */