Merge pull request #1496 from knmcguire/pocketdrone_stereocameradown_…

…edgeflow_test_151221 Stereocam updates and ins_int.c changes Add INS_INT_GPS_ID to configure/disable the GPS ABI callback
paparazzi · Dec 27, 2015 · a1a8941 · a1a8941
2 parents 2d20d6b + cb9f1cd
commit a1a8941
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 10 deletions.
diff --git a/conf/TUDELFT/tudelft_KM_control_panel.xml b/conf/TUDELFT/tudelft_KM_control_panel.xml
@@ -68,8 +68,8 @@
         <arg flag="-speech" />
       </program>
       <program name="NatNet">
-        <arg flag="-ac" constant="1"/>
-        <arg flag="12"/>
+        <arg flag="-ac" constant="7"/>
+        <arg flag="114"/>
         <arg flag="-small"/>
         <arg flag="-tf" constant="5"/>
       </program>

diff --git a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml
@@ -181,14 +181,14 @@
     <define name="H_X" value="0.39049610"/>
     <define name="H_Y" value="0.00278894"/>
     <define name="H_Z" value="0.92060036"/>
-    <define name="USE_GPS_HEADING" value="1"/>
+    <define name="USE_GPS_HEADING" value="0"/>
   </section>
-<define name="USE_GPS" value="FALSE"/>
- <define name="USE_GPS_HEADING" value="FALSE"/>
+
+
 
  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-   <define name="PGAIN" value="40"/>
-   <define name="DGAIN" value="10"/>
+   <define name="PGAIN" value="120"/>
+   <define name="DGAIN" value="40"/>
    <define name="IGAIN" value="0"/>
    <define name="REF_MAX_SPEED" value="0.5"/>
    <define name="USE_REF" value="FALSE"/>
@@ -252,6 +252,8 @@
     <subsystem name="gps" type="datalink"/>
     <subsystem name="stabilization" type="int_quat"/>
     <subsystem name="ahrs" type="int_cmpl_quat"/>
-    <subsystem name="ins" />
+    <subsystem name="ins">
+       <define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
+    </subsystem>
   </firmware>
 </airframe>
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
@@ -130,6 +130,9 @@ static void baro_cb(uint8_t sender_id, float pressure);
 #ifndef INS_INT_IMU_ID
 #define INS_INT_IMU_ID ABI_BROADCAST
 #endif
+#ifndef INS_INT_GPS_ID
+#define INS_INT_GPS_ID ABI_BROADCAST
+#endif
 static abi_event accel_ev;
 static abi_event gps_ev;
 
@@ -516,20 +519,30 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 }
 
 static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
-                       uint32_t stamp __attribute__((unused)),
+                       uint32_t stamp,
                        float x, float y, float z,
                        float noise __attribute__((unused)))
 {
 
   struct FloatVect3 vel_body = {x, y, z};
+  static uint32_t last_stamp = 0;
+  float dt = 0;
 
   /* rotate velocity estimate to nav/ltp frame */
   struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
   QUAT_INVERT(q_b2n, q_b2n);
   struct FloatVect3 vel_ned;
   float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
 
+  if (last_stamp > 0) {
+    dt = (float)(stamp - last_stamp) * 1e-6;
+  }
+
+  last_stamp = stamp;
+
 #if USE_HFF
+  (void)dt; //dt is unused variable in this define
+
   struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
   struct FloatVect2 Rvel = {noise, noise};
 
@@ -538,6 +551,10 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
 #else
   ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
   ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
+  if (last_stamp > 0) {
+    ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x);
+    ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y);
+  }
 #endif
 
   ins_ned_to_state();
@@ -554,6 +571,6 @@ void ins_int_register(void)
    * Subscribe to scaled IMU measurements and attach callbacks
    */
   AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
-  AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
+  AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
   AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
 }