[fixewing] remove DownlinkSendWp

conf/conf_example.xml

@@ -29,7 +29,7 @@
    telemetry="telemetry/default_fixedwing_imu.xml"
    flight_plan="flight_plans/versatile.xml"
    settings="settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/nps.xml"
-   settings_modules="modules/imu_common.xml modules/ahrs_float_dcm.xml modules/gps.xml"
+   settings_modules="modules/gps.xml modules/ahrs_float_dcm.xml modules/imu_common.xml"
    gui_color="blue"
   />
   <aircraft

sw/airborne/firmwares/fixedwing/nav.c

@@ -472,6 +472,13 @@ static void send_nav(struct transport_tx *trans, struct link_device *dev)
   SEND_NAVIGATION(trans, dev);
 }
 
+static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
+{
+  float x = nav_utm_east0 +  waypoints[_wp].x;
+  float y = nav_utm_north0 + waypoints[_wp].y;
+  pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &_wp, &x, &y, &(waypoints[_wp].a), &nav_utm_zone0);
+}
+
 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
 {
   static uint8_t i;
@@ -482,6 +489,7 @@ static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
 
 void DownlinkSendWpNr(uint8_t _wp)
 {
+  if (_wp >= nb_waypoint) return;
   DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp);
 }
 

sw/airborne/firmwares/fixedwing/nav.h

@@ -244,11 +244,6 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
 
 extern void DownlinkSendWpNr(uint8_t _wp);
 
-#define DownlinkSendWp(_trans, _dev, i) {    \
-    float x = nav_utm_east0 +  waypoints[i].x; \
-    float y = nav_utm_north0 + waypoints[i].y; \
-    pprz_msg_send_WP_MOVED(_trans, _dev, AC_ID, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
-  }
 #endif /* DOWNLINK */
 
 #endif /* NAV_H */

sw/airborne/modules/enose/anemotaxis.c

@@ -45,7 +45,7 @@ bool nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
     last_plume_was_here();
     waypoints[plume].x = stateGetPositionEnu_f()->x;
     waypoints[plume].y = stateGetPositionEnu_f()->y;
-    //    DownlinkSendWp(plume);
+    //    DownlinkSendWpNr(plume);
   }
 
   struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
@@ -70,8 +70,8 @@ bool nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
         waypoints[c2].x = waypoints[c1].x - width * crosswind_x * sign;
         waypoints[c2].y = waypoints[c1].y - width * crosswind_y * sign;
 
-        //      DownlinkSendWp(c1);
-        //      DownlinkSendWp(c2);
+        //      DownlinkSendWpNr(c1);
+        //      DownlinkSendWpNr(c2);
 
         status = CROSSWIND;
         nav_init_stage();
@@ -84,7 +84,7 @@ bool nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
         waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS * upwind_x;
         waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS * upwind_y;
 
-        // DownlinkSendWp(c);
+        // DownlinkSendWpNr(c);
 
         sign = -sign;
         status = UTURN;
@@ -95,7 +95,7 @@ bool nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
         waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS * upwind_x;
         waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS * upwind_y;
 
-        // DownlinkSendWp(c);
+        // DownlinkSendWpNr(c);
 
         sign = -sign;
         status = UTURN;

sw/airborne/modules/enose/chemotaxis.c

@@ -34,7 +34,7 @@ bool nav_chemotaxis(uint8_t c, uint8_t plume)
     float y = stateGetPositionEnu_f()->y - waypoints[plume].y;
     waypoints[c].x = waypoints[plume].x + ALPHA * x;
     waypoints[c].y = waypoints[plume].y + ALPHA * y;
-    //    DownlinkSendWp(c);
+    //    DownlinkSendWpNr(c);
     /* Turn in the right direction */
     float dir_x = cos(M_PI_2 - stateGetHorizontalSpeedDir_f());
     float dir_y = sin(M_PI_2 - stateGetHorizontalSpeedDir_f());
@@ -48,7 +48,7 @@ bool nav_chemotaxis(uint8_t c, uint8_t plume)
     /* Store this plume */
     waypoints[plume].x = stateGetPositionEnu_f()->x;
     waypoints[plume].y = stateGetPositionEnu_f()->y;
-    // DownlinkSendWp(plume);
+    // DownlinkSendWpNr(plume);
     last_plume_value = chemo_sensor;
   }
 

sw/airborne/modules/nav/nav_catapult.c

@@ -193,7 +193,7 @@ bool nav_catapult_run(uint8_t _climb)
         float dir_L = sqrtf(dir_x * dir_x + dir_y * dir_y);
         WaypointX(_climb) = nav_catapult.pos.x + (dir_x / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
         WaypointY(_climb) = nav_catapult.pos.y + (dir_y / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
-        DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _climb);
+        DownlinkSendWpNr(_climb);
         // next step
         nav_catapult.status = NAV_CATAPULT_MOTOR_CLIMB;
       }