*** empty log message ***

sw/airborne/ap_downlink.h

@@ -154,7 +154,7 @@
 
 #define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() /** generated from the xml settings config in conf/settings */
 
-#define PERIODIC_SEND_SURVEY() DOWNLINK_SEND_SURVEY(&survey_east, &survey_north, &survey_west, &survey_south)
+#define PERIODIC_SEND_SURVEY() DOWNLINK_SEND_SURVEY(&nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south)
 
 #define PERIODIC_SEND_RANGEFINDER() DOWNLINK_SEND_RANGEFINDER(&rangemeter, &ctl_grz_z_dot, &ctl_grz_z_dot_sum_err, &ctl_grz_z_dot_setpoint, &ctl_grz_z_sum_err, &ctl_grz_z_setpoint, &flying)
 

sw/airborne/discsurvey.c

@@ -13,7 +13,7 @@ static struct point c1;
 static struct point c2;
 
 bool_t disc_survey_init( float grid ) {
-  survey_shift = grid;
+  nav_survey_shift = grid;
   status = DOWNWIND;
   sign = 1;
   c1.x = estimator_x;
@@ -28,7 +28,7 @@ bool_t disc_survey( uint8_t center, float radius) {
   float upwind_x = cos(wind_dir);
   float upwind_y = sin(wind_dir);
 
-  float grid = survey_shift / 2;
+  float grid = nav_survey_shift / 2;
 
   switch (status) {
   case UTURN:

sw/airborne/nav.c

@@ -81,6 +81,10 @@ float nav_glide_pitch_trim;
 
 float nav_ground_speed_setpoint, nav_ground_speed_pgain;
 
+/* Used in nav_survey_rectangle. Defined here for downlink and uplink */
+float nav_survey_shift;
+float nav_survey_west, nav_survey_east, nav_survey_north, nav_survey_south;
+
 void nav_init_stage( void ) {
   last_x = estimator_x; last_y = estimator_y;
   stage_time = 0;

sw/airborne/nav.h

@@ -114,6 +114,9 @@ extern float nav_radius;
 extern float nav_ground_speed_pgain, nav_ground_speed_setpoint;
 
 
+extern float nav_survey_shift;
+extern float nav_survey_west, nav_survey_east, nav_survey_north, nav_survey_south;
+
 void nav_update(void);
 void nav_home(void);
 void nav_init(void);

sw/airborne/nav_survey_rectangle.c

@@ -0,0 +1,155 @@
+#include "nav_survey_rectangle.h"
+#include "estimator.h"
+
+static struct point survey_from;
+static struct point survey_to;
+static bool_t survey_uturn __attribute__ ((unused)) = FALSE;
+static survey_orientation_t survey_orientation = NS;
+
+#define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y))
+#define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y))
+#define SurveyGoingEast() ((survey_orientation == WE) && (survey_to.x > survey_from.x))
+#define SurveyGoingWest() ((survey_orientation == WE) && (survey_to.x < survey_from.x))
+
+
+void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) {
+  nav_survey_west = Min(waypoints[wp1].x, waypoints[wp2].x);
+  nav_survey_east = Max(waypoints[wp1].x, waypoints[wp2].x);
+  nav_survey_south = Min(waypoints[wp1].y, waypoints[wp2].y);
+  nav_survey_north = Max(waypoints[wp1].y, waypoints[wp2].y);
+  survey_orientation = so;
+  
+  if (survey_orientation == NS) {
+    survey_from.x = survey_to.x = Min(Max(estimator_x, nav_survey_west+grid/2.), nav_survey_east-grid/2.);
+    if (estimator_y > nav_survey_north || (estimator_y > nav_survey_south && estimator_hspeed_dir > M_PI/2. && estimator_hspeed_dir < 3*M_PI/2)) {
+      survey_to.y = nav_survey_south;
+      survey_from.y = nav_survey_north;
+    } else {
+      survey_from.y = nav_survey_south;
+      survey_to.y = nav_survey_north;
+    }
+  } else { /* survey_orientation == WE */
+    survey_from.y = survey_to.y = Min(Max(estimator_y, nav_survey_south+grid/2.), nav_survey_north-grid/2.);
+    if (estimator_x > nav_survey_east || (estimator_x > nav_survey_west && estimator_hspeed_dir > M_PI)) {
+      survey_to.x = nav_survey_west;
+      survey_from.x = nav_survey_east;
+    } else {
+      survey_from.x = nav_survey_west;
+      survey_to.x = nav_survey_east;
+    }
+  }
+  nav_survey_shift = grid;
+  survey_uturn = FALSE;
+}
+
+
+void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
+  static float survey_radius;
+
+  nav_survey_west = Min(waypoints[wp1].x, waypoints[wp2].x);
+  nav_survey_east = Max(waypoints[wp1].x, waypoints[wp2].x);
+  nav_survey_south = Min(waypoints[wp1].y, waypoints[wp2].y);
+  nav_survey_north = Max(waypoints[wp1].y, waypoints[wp2].y);
+
+  /* Update the current segment from corners' coordinates*/
+  if (SurveyGoingNorth()) {
+    survey_to.y = nav_survey_north;
+    survey_from.y = nav_survey_south;
+  } else if (SurveyGoingSouth()) {
+    survey_to.y = nav_survey_south;
+    survey_from.y = nav_survey_north;
+  } else if (SurveyGoingEast()) {
+    survey_to.x = nav_survey_east;
+    survey_from.x = nav_survey_west;
+  } else if (SurveyGoingWest()) {
+    survey_to.x = nav_survey_west;
+    survey_from.x = nav_survey_east;
+  }
+
+  if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
+    if ((estimator_y < nav_survey_north && SurveyGoingNorth()) ||
+        (estimator_y > nav_survey_south && SurveyGoingSouth()) ||
+	(estimator_x < nav_survey_east && SurveyGoingEast()) ||
+        (estimator_x > nav_survey_west && SurveyGoingWest())) {
+      /* Continue ... */
+      nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y);
+    } else {
+      if (survey_orientation == NS) {
+	/* North or South limit reached, prepare U-turn and next leg */
+	float x0 = survey_from.x; /* Current longitude */
+	if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) {
+	  x0 += nav_survey_shift / 2;
+	  nav_survey_shift = -nav_survey_shift;
+	  nav_circle_radians = M_PI_2;
+	} else {
+	  nav_circle_radians = 0.;
+	}
+	
+	x0 = x0 + nav_survey_shift; /* Longitude of next leg */
+	survey_from.x = survey_to.x = x0;
+	
+	/* Swap South and North extremities */
+	float tmp = survey_from.y;
+	survey_from.y = survey_to.y;
+	survey_to.y = tmp;
+	
+	/** Do half a circle around WP 0 */
+	waypoints[0].x = x0 - nav_survey_shift/2.;
+	waypoints[0].y = survey_from.y;
+	
+      /* Computes the right direction for the circle */
+	survey_radius = nav_survey_shift / 2.;
+	if (SurveyGoingNorth()) {
+	  survey_radius = -survey_radius;
+	  if (survey_radius > 0.) 
+	    nav_circle_radians = - nav_circle_radians;
+	}
+      } else { /* (survey_orientation == WE) */
+	/* East or West limit reached, prepare U-turn and next leg */
+	/* There is a y0 declared in math.h (for ARM) !!! */
+	float my_y0 = survey_from.y; /* Current latitude */
+	if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) {
+	  my_y0 += nav_survey_shift / 2;
+	  nav_survey_shift = -nav_survey_shift;
+	  nav_circle_radians = M_PI_2;
+	} else {
+	  nav_circle_radians = 0.;
+	}
+	
+	my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
+	survey_from.y = survey_to.y = my_y0;
+	
+	/* Swap West and East extremities */
+	float tmp = survey_from.x;
+	survey_from.x = survey_to.x;
+	survey_to.x = tmp;
+	
+	/** Do half a circle around WP 0 */
+	waypoints[0].x = survey_from.x;
+	waypoints[0].y = my_y0 - nav_survey_shift/2.;
+	
+      /* Computes the right direction for the circle */
+	survey_radius = nav_survey_shift / 2.;
+	if (SurveyGoingWest()) {
+	  survey_radius = -survey_radius;
+	  if (survey_radius > 0.) 
+	    nav_circle_radians = - nav_circle_radians;
+	}
+      }
+
+      nav_in_segment = FALSE;
+      survey_uturn = TRUE;
+    }
+  } else { /* U-turn */
+    if (NavCircleCount() < 0.45) {
+      NavCircleWaypoint(0, survey_radius);
+    } else {
+      /* U-turn finished, back on a segment */
+      survey_uturn = FALSE;
+      nav_in_circle = FALSE;
+    }
+  }
+  NavVerticalAutoThrottleMode(0.); /* No pitch */
+  NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
+}
+

sw/airborne/nav_survey_rectangle.h

@@ -0,0 +1,17 @@
+/** Automatic survey of a sector (south-north or west-east sweep) */
+
+#ifndef NAV_SURVEY_RECTANGLE_H
+#define NAV_SURVEY_RECTANGLE_H
+
+#include "nav.h"
+
+typedef enum {NS, WE} survey_orientation_t;
+
+extern void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so);
+extern void nav_survey_rectangle(uint8_t wp1, uint8_t wp2);
+
+#define NavSurveyRectangleInit(_wp1, _wp2, _grid, _orientation) nav_survey_rectangle_init(_wp1, _wp2, _grid, _orientation)
+#define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle(_wp1, _wp2)
+
+
+#endif