Follow() and MoveWaypoint() first tests

sw/airborne/autopilot/Makefile

@@ -62,7 +62,8 @@ $(TARGET).srcs	= \
 	if_calib.c \
 	mainloop.c \
 	cam.c \
-	traffic_info.c
+	traffic_info.c \
+	datalink.c
 
 ifeq ($(AIRCRAFT), Gorazoptere_brushless_ANALOG)
 	$(TARGET).srcs  += ahrs.S 

sw/airborne/autopilot/gps_ubx.c

@@ -133,9 +133,6 @@ void parse_gps_msg( void ) {
     }
   }
 #endif
-
-
-
 }
 
 

sw/airborne/autopilot/mainloop.c

@@ -40,6 +40,7 @@
 #include "estimator.h"
 #include "downlink.h"
 #include "uart.h"
+#include "datalink.h"
 
 #ifdef SECTION_IMU_ANALOG
 #include "ahrs.h"
@@ -112,6 +113,10 @@ int main( void ) {
 	gps_pos_available = FALSE;
       }
     }
+    if (dl_msg_available) {
+      dl_parse_msg();
+      dl_msg_available = FALSE;
+    }
     if (link_fbw_receive_complete) {
       /* receive radio control task from fbw */
       link_fbw_receive_complete = FALSE;

sw/airborne/autopilot/nav.c

@@ -167,10 +167,10 @@ static float qdr;
 #define Qdr(x) (Min(x, 350) < qdr && qdr < x+10)
 
 #define Follow(_ac_id, _distance) { \
-  struct ac_info_ * ac = get_the_other(_ac_id); \
+  struct ac_info_ * ac = get_ac_info(_ac_id); \
   vertical_mode = VERTICAL_MODE_AUTO_ALT; \
   desired_altitude = ac->alt; \
-  float alpha = M_PI/2 - RadOfDeg(ac->heading); \
+  float alpha = M_PI/2 - ac->course; \
   fly_to_xy(ac->east - _distance*cos(alpha), ac->north - _distance*sin(alpha)); \
 }
 
@@ -197,6 +197,7 @@ const uint8_t nb_waypoint = NB_WAYPOINT;
 
 struct point waypoints[NB_WAYPOINT+1] = WAYPOINTS;
 
+
 /** distance of carrot (in meter) */
 static float carrot;
 

sw/airborne/autopilot/nav.h

@@ -78,4 +78,14 @@ void nav_home(void);
 void nav_init(void);
 void nav_without_gps(void);
 
+#define MoveWaypoint(_id, _ux, _uy, _a) { \
+  if (_id < nb_waypoint) { \
+    waypoints[_id].x = _ux - nav_utm_east0; \
+    waypoints[_id].y = _uy - nav_utm_north0; \
+    waypoints[_id].a = _a; \
+    /*** printf("%d:x=%.0f y=%.0f a=%.0f\n",_id, waypoints[_id].x, waypoints[_id].y, _a); ***/  \
+  } \
+}
+
+
 #endif /* NAV_H */

sw/airborne/autopilot/traffic_info.c

@@ -21,31 +21,16 @@
  * Boston, MA 02111-1307, USA. 
  *
  */
-/** \file nav.c
+/** \file traffic_info.c
  *  \brief Informations relative to the other aircrafts
  *
  */
 
 #include <inttypes.h>
-#include "flight_plan.h"
+#include "traffic_info.h"
 
-#define NB_OTHERS 4
 
-struct ac_info_ {float east, north, heading, alt;};
+struct ac_info_ * get_ac_info(uint8_t id) {
-
+  id = (id < NB_ACS ? id : 0);
-struct ac_info_ the_others[NB_OTHERS];
+  return &the_acs[id];
-
-void
-set_the_other(uint8_t id, float utm_x, float utm_y, float heading, float alt) {
-  if (id < NB_OTHERS) {
-    the_others[id].east = utm_x -  NAV_UTM_EAST0;
-    the_others[id].north = utm_y - NAV_UTM_NORTH0;
-    the_others[id].heading = heading;
-    the_others[id].alt = alt;
-  }
-}
-
-struct ac_info_ * get_the_other(uint8_t id) {
-  id = (id < NB_OTHERS ? id : NB_OTHERS - 1);
-  return &the_others[id];
 }

sw/airborne/autopilot/traffic_info.h

@@ -29,12 +29,32 @@
 #ifndef TI_H
 #define TI_H
 
-struct ac_info_ {float east, north, heading, alt;};
+#define NB_ACS 4
+
+struct ac_info_ {
+  float east; /* m */
+  float north; /* m */
+  float course; /* rad (CW) */
+  float alt; /* m */
+  float gspeed; /* m/s */
+};
+
+
+struct ac_info_ the_acs[NB_ACS];
+
+#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/) { \
+  if (_id < NB_ACS) { \
+    the_acs[_id].east = _utm_x -  NAV_UTM_EAST0; \
+    the_acs[_id].north = _utm_y - NAV_UTM_NORTH0; \
+    the_acs[_id].course = _course; \
+    the_acs[_id].alt = _alt; \
+    the_acs[_id].gspeed = _gspeed; \
+    /*** printf("%d:x=%.0f y=%.0f c=%f a=%.0f\n",_id,the_acs[_id].east,the_acs[_id].north,the_acs[_id].course, the_acs[_id].alt); ***/  \
+  } \
+}
 
-void 
-set_the_other(uint8_t id, float utm_x, float utm_y, float heading, float alt);
 
 struct ac_info_ *
-get_the_other(uint8_t id);
+get_ac_info(uint8_t id);
 
 #endif