navigation on Ovals (Anton's code)

conf/flight_plans/flight_plan.dtd

@@ -12,14 +12,14 @@
 <!ELEMENT exceptions (exception*)>
 
 <!ELEMENT blocks (block+)>
-<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
+<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval)*>
 
 <!ELEMENT include (arg|with)*>
 <!ELEMENT arg EMPTY>
 <!ELEMENT with EMPTY>
 
-<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
+<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval)*>
-<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
+<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval)*>
 <!ELEMENT exception EMPTY>
 <!ELEMENT heading EMPTY>
 <!ELEMENT attitude EMPTY>
@@ -29,6 +29,7 @@
 <!ELEMENT call EMPTY>
 <!ELEMENT circle EMPTY>
 <!ELEMENT eight EMPTY>
+<!ELEMENT oval EMPTY>
 <!ELEMENT survey_rectangle EMPTY>
 <!ELEMENT deroute EMPTY>
 <!ELEMENT stay EMPTY>
@@ -155,6 +156,11 @@ center CDATA #REQUIRED
 turn_around CDATA #REQUIRED
 radius CDATA #REQUIRED>
 
+<!ATTLIST oval
+p1 CDATA #REQUIRED
+p2 CDATA #REQUIRED
+radius CDATA #REQUIRED>
+
 <!ATTLIST survey_rectangle
 grid CDATA #REQUIRED
 wp1 CDATA #REQUIRED

conf/flight_plans/versatile.xml

@@ -27,6 +27,10 @@
       <eight radius="75" center="1" turn_around="2"/>
     </block>
 
+    <block name="oval" strip_button="Oval">
+      <oval p1="1" p2="2" radius="80"/>
+    </block>
+
     <block name="circle 1 auto pitch">
       <circle radius="75" wp="1" pitch="auto" throttle="0.7"/>
     </block>

sw/airborne/nav.c

@@ -237,7 +237,7 @@ static inline void nav_circle_XY(float x, float y, float radius) {
 #define Or(x, y) ((x) || (y))
 #define Min(x,y) (x < y ? x : y)
 #define Max(x,y) (x > y ? x : y)
-#define NavQdrCloseTo(x) ({ float circle_qdr = NavCircleQdr(); (Min(x, 350) < circle_qdr && circle_qdr < x+10); })
+#define NavQdrCloseTo(x) ({ float _course = x; NormCourse(_course); float circle_qdr = NavCircleQdr(); (Min(_course, 350) < circle_qdr && circle_qdr < _course+10); })
 #define NavBlockTime() (block_time)
 #define LessThan(_x, _y) ((_x) < (_y))
 
@@ -637,3 +637,87 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
     return;
   }
 }
+
+/************** Oval Navigation **********************************************/
+
+/** Navigation along a figure O. One side leg is defined by waypoints [p1] and
+    [p2]. 
+    The navigation goes through 4 states: OC1 (half circle next to [p1]),
+    OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 
+    (opposite leg).
+
+    Initial state is the route along the desired segment (OC2).
+*/
+
+enum oval_status { OR12, OC2, OR21, OC1 };
+
+static enum oval_status oval_status;
+void nav_oval_init( void ) {
+  oval_status = OC2;
+}
+
+void nav_oval(uint8_t p1, uint8_t p2, float radius) {
+  float alt = waypoints[p1].a;
+  waypoints[p1].a = alt;
+
+  float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
+  float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
+  float d = sqrt(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y);
+
+  /* Unit vector from p1 to p2 */
+  float u_x = p2_p1_x / d;
+  float u_y = p2_p1_y / d;
+
+  /* The half circle centers and the other leg */
+  struct point p1_center = { waypoints[p1].x + radius * -u_y,
+			     waypoints[p1].y + radius * u_x,
+			     alt  };
+  struct point p1_out = { waypoints[p1].x + 2*radius * -u_y,
+			  waypoints[p1].y + 2*radius * u_x,
+			  alt  };
+  
+  struct point p2_in = { waypoints[p2].x + 2*radius * -u_y,
+			 waypoints[p2].y + 2*radius * u_x,
+			 alt  };
+  struct point p2_center = { waypoints[p2].x + radius * -u_y,
+			     waypoints[p2].y + radius * u_x,
+			     alt  };
+ 
+  float qdr_out_2 = M_PI - atan2(u_y, u_x);
+  float qdr_out_1 = qdr_out_2 + M_PI;
+  
+  switch (oval_status) {
+  case OC1 :
+    nav_circle_XY(p1_center.x,p1_center.y, -radius);
+    if (NavQdrCloseTo(DegOfRad(qdr_out_1)-10)) {
+      oval_status = OR12;
+      InitStage();
+    }
+    return;
+
+  case OR12:
+    nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
+    if (approaching_xy(p2_in.x, p2_in.y,CARROT)) { 
+      oval_status = OC2;
+      InitStage();
+    }
+    return;
+
+  case OC2 :
+    nav_circle_XY(p2_center.x, p2_center.y, -radius);
+    if (NavQdrCloseTo(DegOfRad(qdr_out_2)-10)) {
+      oval_status = OR21;
+      InitStage();
+    }
+   return;
+
+  case OR21:
+    nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
+    if (approaching_xy(waypoints[p1].x, waypoints[p1].y,CARROT)) { 
+      oval_status = OC1;
+      InitStage();
+    }
+    return;
+  }
+}
+

sw/airborne/nav.h

@@ -101,6 +101,10 @@ extern void nav_eight_init( void );
 extern void nav_eight(uint8_t, uint8_t, float);
 #define Eight(a, b, c) nav_eight((a), (b), (c))
 
+extern void nav_oval_init( void );
+extern void nav_oval(uint8_t, uint8_t, float);
+#define Oval(a, b, c) nav_oval((b), (a), (c))
+
 extern float ground_alt;
 
 extern float survey_west, survey_east, survey_north, survey_south;

sw/tools/gen_flight_plan.ml

@@ -237,7 +237,7 @@ let rec index_stage = fun x ->
     | "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" ->
 	incr stage;
 	Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
-    | "survey_rectangle" | "eight" ->
+    | "survey_rectangle" | "eight" | "oval"->
 	incr stage; incr stage;
 	Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
     | "exception" ->
@@ -404,6 +404,19 @@ let rec print_stage = fun index_of_waypoints x ->
 	lprintf "Eight(%s, %s, %s);\n" center turn_about r;
 	output_cam_mode x index_of_waypoints;
 	lprintf "return;\n"
+    | "oval" ->
+	stage ();
+	lprintf "nav_oval_init();\n";
+	lprintf "NextStage();\n";
+	left ();
+	stage ();
+	let p1 = get_index_waypoint (ExtXml.attrib x "p1") index_of_waypoints
+	and p2 = get_index_waypoint (ExtXml.attrib x "p2") index_of_waypoints in
+	let r = parsed_attrib  x "radius" in
+	let _vmode = output_vmode x p1 "" in
+	lprintf "Oval(%s, %s, %s);\n" p1 p2 r;
+	output_cam_mode x index_of_waypoints;
+	lprintf "return;\n"
     | "set" ->
 	stage ();
 	let var = ExtXml.attrib  x "var"