Vectornav as AHRS, minor fix in the float_euler stabilization settings (

#2013)
paparazzi · Feb 16, 2017 · 861986a · 861986a
1 parent 15d00ad
commit 861986a
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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
@@ -35,5 +35,6 @@
 #endif
 
 extern void ahrs_vectornav_register(void);
+extern bool ahrs_vectornav_is_enabled(void);
 
 #endif /* AHRS_VECTORNAV_WRAPPER_H */