SURVEY messages removed when not active

sw/airborne/ap_downlink.h

@@ -154,7 +154,10 @@
 
 #define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() /** generated from the xml settings config in conf/settings */
 
-#define PERIODIC_SEND_SURVEY() DOWNLINK_SEND_SURVEY(&nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south)
+#define PERIODIC_SEND_SURVEY() { \
+  if (nav_survey_active) \
+    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/nav.c

@@ -85,6 +85,8 @@ 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;
+bool_t nav_survey_active;
+
 
 void nav_init_stage( void ) {
   last_x = estimator_x; last_y = estimator_y;
@@ -410,7 +412,7 @@ static inline void compute_dist2_to_home(void) {
 
 
 #ifndef FAILSAFE_HOME_RADIUS
-#define FAILSAFE_HOME_RADIUS 50
+#define FAILSAFE_HOME_RADIUS 100
 #endif
 
 /** \brief Home mode navigation (circle around HOME) */
@@ -429,6 +431,8 @@ void nav_home(void) {
  * plan
  */
 void nav_update(void) {
+  nav_survey_active = FALSE;
+
   compute_dist2_to_home();
 
   auto_nav(); /* From flight_plan.h */

sw/airborne/nav.h

@@ -118,6 +118,7 @@ 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;
+extern bool_t nav_survey_active;
 
 void nav_update(void);
 void nav_home(void);

sw/airborne/nav_survey_rectangle.c

@@ -46,6 +46,8 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie
 void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
   static float survey_radius;
 
+  nav_survey_active = TRUE;
+
   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);