Merge branch 'v7' of github.com:tudelft/obc2016 into v7

paparazzi · Sep 24, 2016 · e2a1d03 · e2a1d03
2 parents bb3d886 + 0638740
commit e2a1d03
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 18 deletions.
diff --git a/conf/flight_plans/TUDELFT/outback_dalby.xml b/conf/flight_plans/TUDELFT/outback_dalby.xml
@@ -6,6 +6,7 @@
 #include "guidance/guidance_hybrid.h"
 #include "guidance/guidance_v.h"
 #include "guidance/guidance_h.h"
+#include "subsystems/electrical.h"
 
 // States
 #define AircraftIsBooting()  LessThan(nav_block,4)
@@ -177,19 +178,21 @@
       <stay wp="LANDING" vmode="climb" climb="land_cmd.z"/>
     </block>
 
-    <block name="land">
-      <go forward="false" wp="TD"/>
-    </block>
-    <block name="flare">
-      <exception cond="NavDetectGround()" deroute="HoldingPoint"/>
-      <exception cond="!nav_is_in_flight()" deroute="landed"/>
-      <call fun="NavStartDetectGround()"/>
-      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
-    </block>
     <block name="landed">
       <call fun="NavKillThrottle()"/>
       <call fun="nav_throttle_curve_set(0)" loop="false"/>
-      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+      <attitude pitch="0" roll="0" throttle="0" until="stage_time>2" vmode="throttle"/>
+    </block>
+    <block name="inert">
+      <call fun="NavKillThrottle()"/>
+      <call fun="NavOpaDisarm(true)"/>
+      <attitude pitch="0" roll="0" throttle="0" until="stage_time>2" vmode="throttle"/>
+      <attitude pitch="0" roll="0" throttle="0" until="electrical.vsupply>220" vmode="throttle"/>
+    </block>
+    <block name="wait_minute">
+      <call fun="NavKillThrottle()"/>
+      <attitude pitch="0" roll="0" throttle="0" until="stage_time>60" vmode="throttle"/>
+      <deroute block="Start Engine"/>
     </block>
     <block name="GpsLoss">
       <exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="landed"/>

diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -329,8 +329,8 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
   /* compute distance of estimated/current pos to target wp
    * distance with half metric precision (6.25 cm)
    */
-  INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
-  dist_to_point = int32_vect2_norm(&diff);
+  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
+  dist_to_point = float_vect2_norm(&diff_f);
 
   /* return TRUE if we have arrived */
   if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
@@ -343,7 +343,7 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
     struct Int32Vect2 from_diff;
     VECT2_DIFF(from_diff, *wp, *from);
     INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2);
-    return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
+    return (diff_f.x * from_diff.x + diff_f.y * from_diff.y < 0);
   }
 
   return false;
@@ -356,16 +356,16 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
   static uint16_t wp_entry_time = 0;
   static bool wp_reached = false;
   static struct EnuCoor_i wp_last = { 0, 0, 0 };
-  struct Int32Vect2 diff;
+  struct FloatVect2 diff_f;
 
   if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
     wp_reached = false;
     wp_last = *wp;
   }
-  VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
-  INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
-  dist_to_point = int32_vect2_norm(&diff);
-  if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
+
+  VECT2_DIFF(diff_f, *wp, *stateGetPositionEnu_f());
+  dist_to_point = float_vect2_norm(&diff_f);
+  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
     if (!wp_reached) {
       wp_reached = true;
       wp_entry_time = autopilot_flight_time;

diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -110,6 +110,7 @@ extern bool nav_set_failsafe(void);
 
 #define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } false; })
 #define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } false; })
+#define NavOpaDisarm(_true_or_false) ({ opa_controller_ap_disarm(_true_or_false); false; })
 
 
 #define NavSetGroundReferenceHere() ({ nav_reset_reference(); false; })