follow ac cam reenabled

conf/settings/cam.xml

@@ -13,15 +13,6 @@
       <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam" VAR="cam_pan_c"/>
     </dl_settings>
 
-    <dl_settings name="pos">
-<dl_setting min="-1000" max="1000" step="1" module="test_cam" var="test_cam_estimator_x" shortname="x"/>
-<dl_setting min="-1000" max="1000" step="1" module="test_cam" var="test_cam_estimator_y" shortname="y"/>
-<dl_setting min="-1000" max="1000" step="1" module="test_cam" var="test_cam_estimator_z" shortname="z"/>
-<dl_setting min="-0.5" max="0.5" step="0.1" module="test_cam" var="test_cam_estimator_phi" shortname="phi"/>
-<dl_setting min="-0.5" max="0.5" step="0.1" module="test_cam" var="test_cam_estimator_theta" shortname="theta"/>
-<dl_setting min="0" max="6.28" step="0.1" module="test_cam" var="test_cam_estimator_hspeed_dir" shortname="hspeed_dir"/>
-    </dl_settings>
-
     <dl_settings name="target">
 <dl_setting min="-1000" max="1000" step="1" module="cam" var="cam_target_x" shortname="x"/>
 <dl_setting min="-1000" max="1000" step="1" module="cam" var="cam_target_y" shortname="y"/>

sw/airborne/cam.c

@@ -38,7 +38,12 @@
 #endif // POINT_CAM
 
 #ifdef TEST_CAM
-#include "test_cam.h"
+float test_cam_estimator_x;
+float test_cam_estimator_y;
+float test_cam_estimator_z;
+float test_cam_estimator_phi;
+float test_cam_estimator_theta;
+float test_cam_estimator_hspeed_dir;
 #endif // TEST_CAM
 
 #ifdef CAM_PAN_NEUTRAL
@@ -79,6 +84,7 @@ float cam_theta_c;
 
 float cam_target_x, cam_target_y, cam_target_alt;
 uint8_t cam_target_wp;
+uint8_t cam_target_ac;
 
 uint8_t cam_mode;
 
@@ -89,7 +95,7 @@ void cam_nadir(void);
 void cam_angles(void);
 void cam_target(void);
 void cam_waypoint_target(void);
-void cam_ac_target( uint8_t ac_id );
+void cam_ac_target(void);
 
 void cam_init( void ) {
   cam_mode = CAM_MODE_OFF;
@@ -111,6 +117,9 @@ void cam_periodic( void ) {
   case CAM_MODE_WP_TARGET:
     cam_waypoint_target();
     break;
+  case CAM_MODE_AC_TARGET:
+    cam_ac_target();
+    break;
   }
 }
 
@@ -185,10 +194,12 @@ void cam_waypoint_target( void ) {
   cam_target();
 }
 
-void cam_ac_target( uint8_t ac_id ) {
-  struct ac_info_ * ac = get_ac_info(ac_id);
+void cam_ac_target( void ) {
+#ifdef TRAFFIC_INFO
+  struct ac_info_ * ac = get_ac_info(cam_target_ac);
   cam_target_x = ac->east;
   cam_target_y = ac->north;
   cam_target_alt = ac->alt;
   cam_target();
+#endif // TRAFFIC_INFO
 }

sw/airborne/cam.h

@@ -32,11 +32,12 @@
 #include <inttypes.h>
 #include "inter_mcu.h"
 
-#define CAM_MODE_OFF 0
-#define CAM_MODE_ANGLES 1
-#define CAM_MODE_NADIR 2
-#define CAM_MODE_XY_TARGET 3
-#define CAM_MODE_WP_TARGET 4
+#define CAM_MODE_OFF 0         /* Do nothing */
+#define CAM_MODE_ANGLES 1      /* Input: servo angles */
+#define CAM_MODE_NADIR 2       /* Input: () */
+#define CAM_MODE_XY_TARGET 3   /* Input: target_x, target_y */
+#define CAM_MODE_WP_TARGET 4   /* Input: waypoint no */
+#define CAM_MODE_AC_TARGET 5   /* Input: ac id */
 
 extern uint8_t cam_mode;
 
@@ -52,6 +53,9 @@ extern float cam_target_x, cam_target_y;
 extern uint8_t cam_target_wp;
 /** For CAM_MODE_WP_TARGET mode */
 
+extern uint8_t cam_target_ac;
+/** For CAM_MODE_AC_TARGET mode */
+
 void cam_periodic( void );
 void cam_init( void );
 
@@ -59,4 +63,13 @@ extern int16_t cam_pan_command;
 #define cam_SetPanCommand(x) { ap_state->commands[COMMAND_CAM_PAN] = cam_pan_command = x;}
 extern int16_t cam_tilt_command;
 #define cam_SetTiltCommand(x) { ap_state->commands[COMMAND_CAM_TILT] = cam_tilt_command = x;}
-#endif
+
+#ifdef TEST_CAM
+extern float test_cam_estimator_x;
+extern float test_cam_estimator_y;
+extern float test_cam_estimator_z;
+extern float test_cam_estimator_phi;
+extern float test_cam_estimator_theta;
+extern float test_cam_estimator_hspeed_dir;
+#endif // TEST_CAM
+#endif // CAM_H

sw/airborne/datalink.c

@@ -31,7 +31,10 @@
 #include <string.h>
 #include "datalink.h"
 
+#ifdef TRAFFIC_INFO
 #include "traffic_info.h"
+#endif // TRAFFIC_INFO
+
 #include "common_nav.h"
 #include "settings.h"
 #include "latlong.h"
@@ -54,7 +57,7 @@ void dl_parse_msg(void) {
   if (msg_id == DL_PING) {
     DOWNLINK_SEND_PONG();
   } else
-#ifdef NAV
+#ifdef TRAFFIC_INFO
   if (msg_id == DL_ACINFO) {
     uint8_t id = DL_ACINFO_ac_id(dl_buffer);
     float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
@@ -63,7 +66,10 @@ void dl_parse_msg(void) {
     float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
     float s = MOfCm(DL_ACINFO_speed(dl_buffer));
     SetAcInfo(id, ux, uy, c, a, s);
-  } else if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
+  } else
+#endif
+#ifdef NAV
+  if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
     uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
     float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
 

sw/airborne/traffic_info.c

@@ -29,6 +29,7 @@
 #include <inttypes.h>
 #include "traffic_info.h"
 
+struct ac_info_ the_acs[NB_ACS];
 
 struct ac_info_ * get_ac_info(uint8_t id) {
   id = id < NB_ACS ? id : 0;

sw/airborne/traffic_info.h

@@ -39,8 +39,7 @@ struct ac_info_ {
   float gspeed; /* m/s */
 };
 
-
-struct ac_info_ the_acs[NB_ACS];
+extern 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) { \