Merge branch 'cam' into dev

resolved conflicts:
	conf/messages.xml
	sw/airborne/modules/digital_cam/dc.c

conf/airframes/funjet_cam_example.xml

@@ -30,7 +30,7 @@
     <subsystem name="attitude" 		type="infrared"/>
     <subsystem name="gps" 		    type="ublox_lea5h"/>
 
-    <subsystem name="navigation"/>
+    <subsystem name="navigation"    type="extra"/>
   </firmware>
 
 
@@ -44,6 +44,9 @@
       <define name="POINT_CAM_YAW_PITCH"  value="1"/>
       <define name="SHOW_CAM_COORDINATES" value="1"/>
     </load>
+    <load name="digital_cam.xml">
+      <define name="DC_SHUTTER_LED" value="3"/>
+    </load>
     <load name="sys_mon.xml"/>
   </modules>
 

conf/messages.xml

@@ -130,8 +130,8 @@
    </message>
 
    <message name="CAM" id="20">
-     <field name="phi" type="int8" unit="deg"/>
+     <field name="phi" type="int16" unit="deg"/>
-     <field name="theta" type="int8" unit="deg"/>
+     <field name="theta" type="int16" unit="deg"/>
      <field name="target_x" type="int16" unit="m"/>
      <field name="target_y" type="int16" unit="m"/>
    </message>
@@ -489,8 +489,29 @@
     <field name="gnd2"   type="float"/>
   </message>
 
- <!-- 63 is free -->
+  <message name="CAM_POINT" id="63">
- <!-- 64 is free -->
+      <field name="cam_point_distance_from_home" type="uint16" unit="m"/>
+      <field name="cam_point_lat" type="float" unit="deg"/>
+      <field name="cam_point_lon" type="float" unit="deg"/>
+  </message>
+
+  <message name="DC_INFO" id="64">
+    <field name="mode" type="int16" unit=""/>
+    <field name="utm_east" type="float" unit="m"/>
+    <field name="utm_north" type="float" unit="m"/>
+    <field name="course" type="float" unit="deg"/>
+    <field name="buffer" type="int8"/>
+    <field name="dist" type="float" unit="m"/>
+    <field name="next_dist" type="float" unit="m"/>
+    <field name="start_x" type="float" unit="m"/>
+    <field name="start_y" type="float" unit="m"/>
+    <field name="start_angle" type="float" unit="deg"/>
+    <field name="angle" type="float" unit="deg"/>
+    <field name="last_block" type="float"/>
+    <field name="count" type="int16" unit=""/>
+    <field name="shutter" type="uint8" unit="s"/>
+  </message>
+
  <!-- 65 is free -->
  <!-- 66 is free -->
  <!-- 67 is free -->
@@ -723,16 +744,16 @@
   </message>
 
   <message name="DC_SHOT" id="110">
-    <field name="photo_nr" type="int16"></field>
+    <field name="photo_nr" type="int16"/>
-    <field name="utm_east"  type="int32" unit="cm"></field>
+    <field name="utm_east"  type="int32" unit="cm"/>
-    <field name="utm_north" type="int32" unit="cm"></field>
+    <field name="utm_north" type="int32" unit="cm"/>
     <field name="z" type="float" unit="m"/>
-    <field name="utm_zone"  type="uint8"></field>
+    <field name="utm_zone"  type="uint8"/>
-    <field name="phi" type="int16" unit="decideg"></field>
+    <field name="phi" type="int16" unit="decideg"/>
-    <field name="theta" type="int16" unit="decideg"></field>
+    <field name="theta" type="int16" unit="decideg"/>
-    <field name="course" type="int16" unit="decideg"></field>
+    <field name="course" type="int16" unit="decideg"/>
-    <field name="speed"  type="uint16" unit="cm/s"></field>
+    <field name="speed"  type="uint16" unit="cm/s"/>
-    <field name="itow"  type="uint32" unit="ms"></field>
+    <field name="itow"  type="uint32" unit="ms"/>
   </message>
 
   <message name="TEST_BOARD_RESULTS" ID="111">

conf/modules/digital_cam.xml

@@ -39,4 +39,3 @@
 
 
 </module>
-

conf/settings/dc.xml

@@ -19,6 +19,9 @@
       <dl_setting max="255" min="1" step="1" var="dc_autoshoot_quartersec_period" handler="Periodic" shortname="Periodic" param="DC_AUTOSHOOT_QUARTERSEC_PERIOD" unit="quarter-sec"/>
       <dl_setting max="5" min="0" step="1" var="dc_autoshoot_meter_grid" shortname="UTM%" param="DC_AUTOSHOOT_METER_GRID" unit="meter"/>
 
+      <dl_setting max="250" min="0" step="5" module="digital_cam/dc" var="dc_gps_dist" handler="Survey" shortname="Linear-Interval"/>
+      <dl_setting max="90" min="5" step="5" module="digital_cam/dc" var="dc_circle_interval" handler="Circle" shortname="Circle-Interval"/>
+      <dl_setting max="1" min="0" step="1" var="dc_cam_tracing" shortname="Cam-Tracing"/>
     </dl_settings>
   </dl_settings>
 </settings>

sw/airborne/ap_downlink.h

@@ -181,10 +181,13 @@
 
 #define PERIODIC_SEND_NAVIGATION(_chan) SEND_NAVIGATION(_chan)
 
+
 #if defined CAM || defined MOBILE_CAM
-#define SEND_CAM(_chan) Downlink({ int16_t x = cam_target_x; int16_t y = cam_target_y; int8_t phi = DegOfRad(cam_phi_c); int8_t theta = DegOfRad(cam_theta_c); DOWNLINK_SEND_CAM(_chan, &phi, &theta, &x, &y);})
+#define SEND_CAM(_chan) Downlink({ int16_t x = cam_target_x; int16_t y = cam_target_y; int16_t phi = DegOfRad(cam_phi_c); int16_t theta = DegOfRad(cam_theta_c); DOWNLINK_SEND_CAM(_chan, &phi, &theta, &x, &y);})
+#define PERIODIC_SEND_CAM_POINT(_chan) DOWNLINK_SEND_CAM_POINT(_chan, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon)
 #else
 #define SEND_CAM(_chan) {}
+#define PERIODIC_SEND_CAM_POINT(_chan) {}
 #endif
 
 #define PERIODIC_SEND_DL_VALUE(_chan) PeriodicSendDlValue(_chan) /** generated from the xml settings config in conf/settings */

sw/airborne/modules/cam_control/cam.c

@@ -87,6 +87,7 @@ uint8_t cam_target_wp;
 uint8_t cam_target_ac;
 
 uint8_t cam_mode;
+bool_t cam_lock;
 
 int16_t cam_pan_command;
 int16_t cam_tilt_command;
@@ -102,8 +103,23 @@ void cam_init( void ) {
 }
 
 void cam_periodic( void ) {
+#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
+  //Position the camera for straight view.
+  if (pprz_mode == PPRZ_MODE_AUTO2){
+#endif
     switch (cam_mode) {
     case CAM_MODE_OFF:
+#if defined(CAM_PAN0)
+      cam_pan_c = RadOfDeg(CAM_PAN0);
+#else
+      cam_pan_c = RadOfDeg(0);
+#endif
+#if defined(CAM_TILT0)
+      cam_tilt_c = RadOfDeg(CAM_TILT0);
+#else
+      cam_tilt_c = RadOfDeg(90);
+#endif
+      cam_angles();
       break;
     case CAM_MODE_ANGLES:
       cam_angles();
@@ -120,13 +136,65 @@ void cam_periodic( void ) {
     case CAM_MODE_AC_TARGET:
       cam_ac_target();
       break;
+      // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
+      // The "TARGET" waypoint coordinates are not used.
+      // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
+    case CAM_MODE_STABILIZED:
+      cam_waypoint_target();
+      break;
+      // In this mode the angles come from the pan and tilt radio channels.
+      // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
+      // in order to calculate the coordinates of where the camera is looking.
+    case CAM_MODE_RC:
+      cam_waypoint_target();
+      break;
     }
+#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
+  }else if (pprz_mode == PPRZ_MODE_AUTO1){
+    //Position the camera for straight view.
+
+#if defined(CAM_TILT_POSITION_FOR_FPV)
+    cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
+#else
+    cam_tilt_c = RadOfDeg(90);
+#endif
+#if defined(CAM_PAN_POSITION_FOR_FPV)
+    cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
+#else
+    cam_pan_c = RadOfDeg(0);
+#endif
+    cam_angles();
+    cam_point_lon = 0;
+    cam_point_lat = 0;
+    cam_point_distance_from_home = 0;
+  }
+#endif
+
+
+#if defined(COMMAND_CAM_PWR_SW)
+  if(video_tx_state){ ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; }else{ ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; }
+#elif defined(VIDEO_TX_SWITCH)
+  if(video_tx_state){ LED_OFF(VIDEO_TX_SWITCH); }else{ LED_ON(VIDEO_TX_SWITCH); }
+#endif
 }
 
 /** Computes the servo values from cam_pan_c and cam_tilt_c */
 void cam_angles( void ) {
   float cam_pan = 0;
   float cam_tilt = 0;
+  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)){
+     cam_pan_c = RadOfDeg(CAM_PAN_MAX);
+
+  }else{
+          if(cam_pan_c < RadOfDeg(CAM_PAN_MIN)){ cam_pan_c = RadOfDeg(CAM_PAN_MIN); }
+       }
+
+  if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)){
+     cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
+
+  }else{
+          if(cam_tilt_c < RadOfDeg(CAM_TILT_MIN)){ cam_tilt_c = RadOfDeg(CAM_TILT_MIN); }
+       }
 
 #ifdef CAM_PAN_NEUTRAL
   float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
@@ -134,6 +202,8 @@ void cam_angles( void ) {
     cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
   else
     cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
+#else
+  cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX-CAM_PAN_MIN) );
 #endif
 
 #ifdef CAM_TILT_NEUTRAL
@@ -142,6 +212,8 @@ void cam_angles( void ) {
     cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
   else
     cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
+#else
+  cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN))  * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_TILT_MAX-CAM_TILT_MIN) );
 #endif
 
   cam_pan = TRIM_PPRZ(cam_pan);

sw/airborne/modules/cam_control/cam.h

@@ -38,8 +38,24 @@
 #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 */
+#define CAM_MODE_STABILIZED    6   // Stabilized mode, input: camera angles from the pan and tilt radio channels, output pointing coordinates.
+#define CAM_MODE_RC            7   // Manual mode, input: camera angles from the pan and tilt radio channels, output servo positions.
+
+#ifndef CAM_PAN_MAX
+#define CAM_PAN_MAX 90
+#endif
+#ifndef CAM_PAN_MIN
+#define CAM_PAN_MIN -90
+#endif
+#ifndef CAM_TILT_MAX
+#define CAM_TILT_MAX 90
+#endif
+#ifndef CAM_TILT_MIN
+#define CAM_TILT_MIN -90
+#endif
 
 extern uint8_t cam_mode;
+extern uint8_t cam_lock;
 
 extern float cam_phi_c, cam_theta_c;
 
@@ -72,4 +88,13 @@ extern float test_cam_estimator_phi;
 extern float test_cam_estimator_theta;
 extern float test_cam_estimator_hspeed_dir;
 #endif // TEST_CAM
+
+#if defined(COMMAND_CAM_PWR_SW) || defined(VIDEO_TX_SWITCH)
+
+extern bool_t video_tx_state;
+#define VIDEO_TX_ON()   { video_tx_state = 1; 0; }
+#define VIDEO_TX_OFF()  { video_tx_state = 0; 0; }
+
+#endif
+
 #endif // CAM_H

sw/airborne/modules/cam_control/point.c

@@ -2,6 +2,8 @@
  * $Id$
  *
  * Copyright (C) 2005-2008  Arnold Schroeter
+ * Modified and expanded to show the coordinates of where the camera is looking at
+ * by Chris Efstathiou 23-Jan-2011 AD.
  *
  * This file is part of paparazzi.
  *
@@ -72,7 +74,13 @@
  */
 
 #include <math.h>
+#include "cam.h"
 #include "point.h"
+#include "autopilot.h"
+#include "generated/flight_plan.h"
+#include "subsystems/navigation/common_nav.h"
+#include "latlong.h"
+#include "gps.h"
 
 typedef struct {
          float fx;
@@ -84,6 +92,19 @@ typedef struct {
          float fy1; float fy2; float fy3;
          float fz1; float fz2; float fz3;} MATRIX;
 
+//bool_t cam_lock = 0;
+float cam_theta;
+float cam_phi;
+bool_t heading_positive = 0;
+#if defined(SHOW_CAM_COORDINATES)
+float   cam_point_x;
+float   cam_point_y;
+unsigned int cam_point_distance_from_home;
+float cam_point_lon,  cam_point_lat;
+float  memory_x, memory_y, memory_z;
+float distance_correction = 1;
+#endif
+
 void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC);
 void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC);
 
@@ -150,8 +171,254 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
                 svObjectPositionForPlane,
                 svObjectPositionForPlane2;
 
+  static VECTOR sv_cam_projection,
+                sv_cam_projection_buf;
+
   static MATRIX smRotation;
 
+/***        BELOW IS THE CODE THAT READS THE RC PAN AND TILT CHANNELS AND CONVERTS THEM TO ANGLES (RADIANS)   ***/
+/***        IT IS USED FOR CALCULATING THE COORDINATES OF THE POINT WHERE THE CAMERA IS LOOKING AT            ***/
+/***        THIS IS DONE ONLY FOR THE CAM_MODE_STABILIZED OR CAM_MODE_RC.                                     ***/
+/***        IN OTHER MODES ONLY THE CAM_POINT WAYPOINT AND THE DISTANCE FROM TARGET IS UPDATED                ***/
+if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC || cam_mode == CAM_MODE_WP_TARGET || cam_mode == CAM_MODE_XY_TARGET ){
+
+if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC ){
+
+/*########################################  TILT CONTROL INPUT  #############################################*/
+#ifdef CAM_TILT_NEUTRAL
+
+#if defined(RADIO_TILT)
+   if ((*fbw_state).channels[RADIO_TILT] >= 0){
+      cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ) *
+      (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
+
+   }else{
+          cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MIN_PPRZ) *
+          (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
+        }
+#elif defined(RADIO_PITCH)
+  if ((*fbw_state).channels[RADIO_PITCH] >= 0){
+     cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ) *
+     (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
+
+   }else{
+          cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MIN_PPRZ) *
+          (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
+        }
+#else
+#error RADIO_TILT or RADIO_PITCH not defined.
+#endif
+
+#else   //#ifdef CAM_TILT_NEUTRAL
+
+#if defined(RADIO_TILT)
+    cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ));
+#elif defined(RADIO_PITCH)
+    cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ));
+#else
+#error RADIO_TILT or RADIO_PITCH not defined.
+#endif
+
+#endif   //#ifdef CAM_TILT_NEUTRAL
+/*########################################  END OF TILT CONTROL INPUT  ########################################*/
+
+/*###########################################  PAN CONTROL INPUT  #############################################*/
+#ifdef CAM_PAN_NEUTRAL
+
+#if defined(RADIO_PAN)
+   if ((*fbw_state).channels[RADIO_PAN] >= 0){
+      cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ) *
+      RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
+
+   }else{
+           cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MIN_PPRZ) *
+           RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
+        }
+#elif defined(RADIO_ROLL)
+   if ((*fbw_state).channels[RADIO_ROLL] >= 0){
+      cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ) *
+      RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
+
+   }else{
+           cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MIN_PPRZ) *
+           RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
+        }
+#else
+#error RADIO_PAN or RADIO_ROLL not defined.
+#endif
+
+#else   //#ifdef CAM_PAN_NEUTRAL
+
+#if defined(RADIO_PAN)
+    cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ));
+#elif defined(RADIO_ROLL)
+   cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ));
+#else
+#error RADIO_PAN or RADIO_ROLL not defined.
+#endif
+
+#endif  //#ifdef CAM_PAN_NEUTRAL
+/*########################################  END OF PAN CONTROL INPUT  #############################################*/
+
+   // Bound Pan and Tilt angles.
+   if (cam_theta > RadOfDeg(CAM_TILT_MAX)){
+      cam_theta = RadOfDeg(CAM_TILT_MAX);
+
+   }else if(cam_theta < RadOfDeg(CAM_TILT_MIN)){ cam_theta = RadOfDeg(CAM_TILT_MIN); }
+
+   if (cam_phi > RadOfDeg(CAM_PAN_MAX)){
+      cam_phi = RadOfDeg(CAM_PAN_MAX);
+
+   }else if(cam_phi < RadOfDeg(CAM_PAN_MIN)){ cam_phi = RadOfDeg(CAM_PAN_MIN); }
+
+
+   svPlanePosition.fx = fPlaneEast;
+   svPlanePosition.fy = fPlaneNorth;
+   svPlanePosition.fz = fPlaneAltitude;
+
+// FOR TESTING ANGLES IN THE SIMULATOR ONLY CODE UNCOMMENT THE TWO BELOW LINES
+//cam_phi = RadOfDeg(90); // LOOK 45 DEGREES TO THE LEFT, -X IS TO THE LEFT AND +X IS TO THE RIGHT
+//cam_theta = RadOfDeg(70);     //  LOOK 45 DEGREES DOWN, 0 IS STRAIGHT DOWN 90 IS STRAIGHT IN FRONT
+
+   if ( cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC){ // Not much to see after 80 degrees of tilt so stop tracking.
+      *fPan = cam_phi;
+      *fTilt = cam_theta;
+      cam_point_distance_from_home = 0;
+      cam_point_lon = 0;
+      cam_point_lat = 0;
+      return;
+
+   }else{
+          sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta)*(fPlaneAltitude-ground_alt));
+          sv_cam_projection_buf.fy = svPlanePosition.fy;
+       }
+
+#if defined(WP_CAM_POINT)
+   sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a;
+#else
+   sv_cam_projection_buf.fz = ground_alt;
+#endif
+
+   /* distance between plane and camera projection */
+   vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition);
+
+   float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation.
+   if(heading_radians > RadOfDeg(180)){ heading_radians -= RadOfDeg(360); }
+   if(heading_radians < RadOfDeg(-180)){ heading_radians += RadOfDeg(360); }
+   //heading_radians += cam_theta;
+
+   /* camera pan angle correction, using a clockwise rotation */
+   smRotation.fx1 = (float)(cos(cam_phi));
+   smRotation.fx2 = (float)(sin(cam_phi));
+   smRotation.fx3 = 0.;
+   smRotation.fy1 = -smRotation.fx2;
+   smRotation.fy2 = smRotation.fx1;
+   smRotation.fy3 = 0.;
+   smRotation.fz1 = 0.;
+   smRotation.fz2 = 0.;
+   smRotation.fz3 = 1.;
+
+   vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection);
+
+   /* yaw correction using a counter clockwise rotation*/
+   smRotation.fx1 = (float)(cos(heading_radians));
+   smRotation.fx2 = -(float)(sin(heading_radians));
+   smRotation.fx3 = 0.;
+   smRotation.fy1 = -smRotation.fx2;
+   smRotation.fy2 = smRotation.fx1;
+   smRotation.fy3 = 0.;
+   smRotation.fz1 = 0.;
+   smRotation.fz2 = 0.;
+   smRotation.fz3 = 1.;
+
+   vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf);
+
+#if defined(RADIO_CAM_LOCK)
+   if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] > MAX_PPRZ/2)) && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = TRUE; }
+   if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] < MIN_PPRZ/2 && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = FALSE; }
+#endif
+   // When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
+   if (cam_lock == FALSE){
+      fObjectEast = (fPlaneEast + sv_cam_projection.fx) ;
+      fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ;
+      fAltitude = ground_alt;
+      memory_x = fObjectEast;
+      memory_y = fObjectNorth;
+      memory_z = fAltitude;
+#if defined(WP_CAM_POINT)
+      waypoints[WP_CAM_POINT].x = fObjectEast;
+      waypoints[WP_CAM_POINT].y = fObjectNorth;
+      waypoints[WP_CAM_POINT].a = ground_alt;
+#endif
+#if defined(SHOW_CAM_COORDINATES)
+      cam_point_x = fObjectEast;
+      cam_point_y = fObjectNorth;
+
+      cam_point_distance_from_home = distance_correction * (uint16_t) (sqrt((cam_point_x*cam_point_x) + (cam_point_y*cam_point_y) ));
+
+      latlong_of_utm(((gps_utm_east/100.) + sv_cam_projection.fx), ((gps_utm_north/100.) + sv_cam_projection.fy), gps_utm_zone);
+      cam_point_lon = latlong_lon*(180/M_PI);
+      cam_point_lat = latlong_lat*(180/M_PI);
+#endif
+
+   }else{
+           fObjectEast = memory_x;
+           fObjectNorth = memory_y;
+           fAltitude = memory_z;
+#if defined(WP_CAM_POINT)
+           waypoints[WP_CAM_POINT].x = fObjectEast;
+           waypoints[WP_CAM_POINT].y = fObjectNorth;
+           waypoints[WP_CAM_POINT].a = fAltitude;
+#endif
+        }
+
+if (cam_mode == CAM_MODE_RC && cam_lock == 0 ){
+   *fPan = cam_phi;
+   *fTilt = cam_theta;
+   return;
+}
+
+#if defined(WP_CAM_POINT)
+  else{
+           waypoints[WP_CAM_POINT].x = fObjectEast;
+           waypoints[WP_CAM_POINT].y = fObjectNorth;
+           waypoints[WP_CAM_POINT].a = fAltitude;
+      }
+#endif
+/***    END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT     ***/
+}else{
+/***    THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET           ***/
+        cam_point_distance_from_home = distance_correction * (uint16_t) fabs( ((uint16_t) (sqrt((fObjectNorth*fObjectNorth) + (fObjectEast*fObjectEast) ))) -
+                                          ((uint16_t) (sqrt((fPlaneNorth*fPlaneNorth) + (fPlaneEast*fPlaneEast) ))) );
+
+        latlong_of_utm((nav_utm_east0 + fObjectEast), (nav_utm_north0 + fObjectNorth), gps_utm_zone);
+        cam_point_lon = latlong_lon*(180/M_PI);
+        cam_point_lat = latlong_lat*(180/M_PI);
+
+#if defined(WP_CAM_POINT)
+        waypoints[WP_CAM_POINT].x = fObjectEast;
+        waypoints[WP_CAM_POINT].y = fObjectNorth;
+        waypoints[WP_CAM_POINT].a = fAltitude;
+#endif
+
+     }
+
+}
+
+//************************************************************************************************
+//************************************************************************************************
+//************************************************************************************************
+//************************************************************************************************
+
+/*
+By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle)
+to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly
+applied to the rotation matrices (in radians).
+In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left
+and +90 is to the top (counterclockwise rotation).
+When reading back the actual rotated coordinates sv_cam_projection.fx has the y coordinate and sv_cam_projection.fy has the x
+represented on a circle in standard mathematical notation.
+*/
   svPlanePosition.fx = fPlaneNorth;
   svPlanePosition.fy = fPlaneEast;
   svPlanePosition.fz = fPlaneAltitude;
@@ -270,6 +537,78 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
   /* fPan is deactivated
   */
   *fPan = 0;
+#else
+#ifdef POINT_CAM_YAW_PITCH_NOSE
+
+/*
+                 -45   0   45
+                    \  |  /
+                     \ | /
+                      \|/
+                      ##
+                      ##
+         _____________##______________
+left tip|_____________________________|right wing tip
+                      ##
+                      ##
+                      ##
+                      ##
+                ______##______
+               |_____|_|_____|
+                      |
+*/
+
+#if defined(CAM_PAN_MODE) && CAM_PAN_MODE == 360
+  /* fixed to the plane*/
+  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, (svObjectPositionForPlane2.fx)));
+
+  *fTilt = (float)(atan2( sqrt(   svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
+                                + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ),
+                          -svObjectPositionForPlane2.fz
+                        ));
+
+   // I need to avoid oscillations around the 180 degree mark.
+/*
+   if (*fPan > 0 && *fPan <= RadOfDeg(175)){ heading_positive = 1; }
+   if (*fPan < 0 && *fPan >= RadOfDeg(-175)){ heading_positive = 0; }
+
+   if (*fPan > RadOfDeg(175) && heading_positive == 0){
+      *fPan = RadOfDeg(-180);
+
+   }else if (*fPan < RadOfDeg(-175) && heading_positive){
+            *fPan = RadOfDeg(180);
+            heading_positive = 0;
+         }
+*/
+#else
+  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, fabs(svObjectPositionForPlane2.fx)));
+
+  *fTilt = (float)(atan2( sqrt(   svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
+                                + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ),
+                          -svObjectPositionForPlane2.fz
+                        ));
+
+  if (svObjectPositionForPlane2.fx < 0)
+  {
+        *fPan = -*fPan;
+        *fTilt = -*fTilt;
+  }
+
+   // I need to avoid oscillations around the 180 degree mark.
+/*
+   if (*fPan > 0 && *fPan <= RadOfDeg(85)){ heading_positive = 1; }
+   if (*fPan < 0 && *fPan >= RadOfDeg(-85)){ heading_positive = 0; }
+
+   if (*fPan > RadOfDeg(85) && heading_positive == 0){
+      *fPan = RadOfDeg(-90);
+
+   }else if (*fPan < RadOfDeg(-85) && heading_positive){
+            *fPan = RadOfDeg(90);
+            heading_positive = 0;
+         }
+*/
+#endif
+
 #else
 #ifdef POINT_CAM_YAW_PITCH
 
@@ -320,7 +659,10 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
             -90  -> camera looks backwards
   */
   /* fixed to the plane*/
-  *fPan = (float)(atan2(svObjectPositionForPlane2.fx, fabs(svObjectPositionForPlane2.fy)));
+  *fPan = (float)(atan2(svObjectPositionForPlane2.fx, fabs(svObjectPositionForPlane2.fy)));// Or is it the opposite??? (CEF)
+  // (CEF) It turned out that Object_North is loaded with x and Object_East with y (reverse). See line #155
+  // this means that:
+  // *fPan = (float)(atan2(svObjectPositionForPlane2.fy, svObjectPositionForPlane2.fy)); // makes the camera 0 to look to the nose?
 
   /* fTilt =   0  -> camera looks down
               90  -> camera looks into right hemisphere
@@ -376,4 +718,5 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
 #endif
 #endif
 #endif
+#endif
 }

sw/airborne/modules/cam_control/point.h

@@ -26,6 +26,13 @@
 #ifndef POINT_H
 #define POINT_H
 
+#if defined(SHOW_CAM_COORDINATES)
+extern unsigned int cam_point_distance_from_home;
+extern float cam_point_lon,  cam_point_lat;
+extern float distance_correction;
+//extern bool_t cam_lock; // Locks to the coordinates where the cam was looking when the variable was set.
+#endif
+
 void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
             float fRollAngle, float fPitchAngle, float fYawAngle,
             float fObjectEast, float fObjectNorth, float fAltitude,

sw/airborne/modules/digital_cam/dc.c

@@ -27,13 +27,27 @@
 uint8_t dc_autoshoot_meter_grid = 100;
 uint8_t dc_autoshoot_quartersec_period = 2;
 dc_autoshoot_type dc_autoshoot = DC_AUTOSHOOT_STOP;
+uint16_t dc_gps_count = 0;
+uint8_t dc_cam_tracing = 1;
+float dc_cam_angle = 0;
 
+float dc_circle_interval = 0;
+float dc_circle_start_angle = 0;
+float dc_circle_last_block = 0;
+float dc_circle_max_blocks = 0;
 
+float dc_gps_dist = 0;
+float dc_gps_next_dist = 0;
+float dc_gps_x = 0;
+float dc_gps_y = 0;
+
+bool_t dc_probing = FALSE;
 
 
 #ifdef SENSOR_SYNC_SEND
 
 uint16_t dc_photo_nr = 0;
+uint16_t dc_buffer = 0;
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -49,12 +63,97 @@ uint16_t dc_photo_nr = 0;
     int16_t phi = DegOfRad(estimator_phi*10.0f);
     int16_t theta = DegOfRad(estimator_theta*10.0f);
   float gps_z = ((float)gps.hmsl) / 1000.0f;
-    DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps.utm_pos.east, &gps.utm_pos.north, &gps_z, &gps.utm_pos.zone, &phi, &theta,  &gps.course, &gps.gspeed, &gps.tow);
+  int16_t photo_nr = -1;
+
+  if (dc_buffer < DC_IMAGE_BUFFER) {
+    dc_buffer++;
     dc_photo_nr++;
+    photo_nr = dc_photo_nr;
   }
 
-#endif
+  DOWNLINK_SEND_DC_SHOT(DefaultChannel,
+                        &photo_nr,
+                        &gps_utm_east,
+                        &gps_utm_north,
+                        &gps_z,
+                        &gps_utm_zone,
+                        &phi,
+                        &theta,
+                        &gps_course,
+                        &gps_gspeed,
+                        &gps_itow);
+}
+#endif /* SENSOR_SYNC_SEND */
 
+uint8_t dc_info(void) {
+#ifdef DOWNLINK_SEND_DC_INFO
+  float course = DegOfRad(estimator_psi);
+  DOWNLINK_SEND_DC_INFO(DefaultChannel,
+                        &dc_autoshoot,
+                        &estimator_x,
+                        &estimator_y,
+                        &course,
+                        &dc_buffer,
+                        &dc_gps_dist,
+                        &dc_gps_next_dist,
+                        &dc_gps_x,
+                        &dc_gps_y,
+                        &dc_circle_start_angle,
+                        &dc_circle_interval,
+                        &dc_circle_last_block,
+                        &dc_gps_count,
+                        &dc_autoshoot_quartersec_period);
+#endif
+  return 0;
+}
+
+/* shoot on circle */
+uint8_t dc_circle(float interval, float start) {
+  dc_autoshoot = DC_AUTOSHOOT_CIRCLE;
+  dc_gps_count = 0;
+  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
+
+  if(start == DC_IGNORE) {
+    start = DegOfRad(estimator_psi);
+  }
+
+  dc_circle_start_angle = fmodf(start, 360.);
+  if (start < 0.)
+    start += 360.;
+  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
+  dc_circle_last_block = 0;
+  dc_circle_max_blocks = floorf(360./dc_circle_interval);
+  dc_probing = TRUE;
+  dc_info();
+  return 0;
+}
+
+/* shoot on survey */
+uint8_t dc_survey(float interval, float x, float y) {
+  dc_autoshoot = DC_AUTOSHOOT_SURVEY;
+  dc_gps_count = 0;
+  dc_gps_dist = interval;
+
+  if (x == DC_IGNORE && y == DC_IGNORE) {
+    dc_gps_x = estimator_x;
+    dc_gps_y = estimator_y;
+  } else if (y == DC_IGNORE) {
+    dc_gps_x = waypoints[(uint8_t)x].x;
+    dc_gps_y = waypoints[(uint8_t)x].y;
+  } else {
+    dc_gps_x = x;
+    dc_gps_y = y;
+  }
+  dc_gps_next_dist = interval;
+  dc_info();
+  return 0;
+}
+
+uint8_t dc_stop(void) {
+  dc_autoshoot = DC_AUTOSHOOT_STOP;
+  dc_info();
+  return 0;
+}
 
 
 /*
@@ -89,4 +188,3 @@ static inline void dc_shoot_on_gps( void ) {
   }
 }
 */
-

sw/airborne/modules/digital_cam/dc.h

@@ -37,11 +37,42 @@
 #ifndef DC_H
 #define DC_H
 
+#include "float.h"
 #include "std.h"
 #include "led.h"
+#include "estimator.h"
+#include "subsystems/nav.h"
 #include "generated/airframe.h"
 #include "subsystems/gps.h"
 
+/* number of images taken since the last change of dc_mode */
+extern uint16_t dc_gps_count;
+
+/* distance between dc shots in meters */
+extern float dc_gps_dist;
+
+extern float dc_gps_next_dist;
+
+/* angle a where first image will be taken at a + delta */
+extern float dc_circle_start_angle;
+
+/* angle between dc shots in degree */
+extern float dc_circle_interval;
+
+extern float dc_circle_max_blocks;
+
+/* point of reference for the distance based mode */
+extern float dc_gps_x, dc_gps_y;
+
+extern float dc_circle_last_block;
+
+extern bool_t dc_probing;
+
+extern uint8_t dc_buffer_timer;
+
+/* camera angle */
+extern float dc_cam_angle;
+extern uint8_t dc_cam_tracing;
 
 /* Generic Set of Digital Camera Commands */
 typedef enum {
@@ -76,7 +107,9 @@ typedef enum {
     DC_AUTOSHOOT_STOP = 0,
     DC_AUTOSHOOT_PERIODIC = 1,
     DC_AUTOSHOOT_DISTANCE = 2,
-	DC_AUTOSHOOT_EXT_TRIG = 3
+    DC_AUTOSHOOT_EXT_TRIG = 3,
+    DC_AUTOSHOOT_SURVEY = 4,
+    DC_AUTOSHOOT_CIRCLE = 5
 } dc_autoshoot_type;
 extern dc_autoshoot_type dc_autoshoot;
 
@@ -93,9 +126,73 @@ void dc_send_shot_position(void);
 #define dc_send_shot_position() {}
 #endif
 
+/* Macro value used to indicate a discardable argument */
+#ifndef DC_IGNORE
+#define DC_IGNORE FLT_MAX
+#endif
+
+/* Default values for buffer control */
+#ifndef DC_IMAGE_BUFFER
+#define DC_IMAGE_BUFFER 255
+#endif
+
+#ifndef DC_IMAGE_BUFFER_TPI
+#define DC_IMAGE_BUFFER_TPI 0
+#endif
+
 /******************************************************************
  * FUNCTIONS
  *****************************************************************/
+/**
+  Sets the dc control in circle mode.
+  The 'start' value is the reference course and 'intervall'
+  the minimum angle between shots.
+  If 'start' is 0 the current course is used instead.
+
+  In this mode the dc control assumes a perfect circular
+  course.
+  The first picture is taken at angle start+interval.
+*/
+extern uint8_t dc_circle(float interval, float start);
+
+#define dc_Circle(interval) dc_circle(interval, DC_IGNORE)
+
+/**
+  Sets the dc control in distance mode.
+  The values of 'x' and 'y' are the coordinates
+  of the reference point used for the distance
+  calculations.
+  If 'y' is 0 the value of 'x' is interpreted
+  as index of a waypoint declared in the flight plan.
+  If both 'x' and 'y' are 0 the current position
+  will be used as reference point.
+
+  In this mode, the dc control assumes a perfect
+  line formed course since the distance is calculated
+  relative to the first given point of reference.
+  So not usable for circles or other comparable
+  shapes.
+*/
+extern uint8_t dc_survey(float interval, float x, float y);
+
+#define dc_Survey(interval) dc_survey(interval, DC_IGNORE, DC_IGNORE)
+
+
+/**
+  Sets the dc control in inactive mode,
+  stopping all current actions.
+*/
+extern uint8_t dc_stop(void);
+
+#define dc_Stop(_) dc_stop()
+
+/**
+   Send an info message containing information
+   about position, course, buffer and all other
+   internal variables used by the dc control.
+*/
+extern uint8_t dc_info(void);
+
 
 /* get settings */
 static inline void dc_init(void)
@@ -108,7 +205,7 @@ static inline void dc_init(void)
 #endif
 }
 
-/* shoot on grid */
+/* shoot on grid
 static inline void dc_shot_on_utm_north_close_to_100m_grid( void )
 {
   uint32_t dist_to_100m_grid = (gps.utm_pos.north / 100) % 100;
@@ -117,31 +214,83 @@ static inline void dc_shot_on_utm_north_close_to_100m_grid( void )
       dc_send_command(DC_SHOOT);
   }
 }
+*/
+static float dim_mod(float a, float b, float m) {
+  if (a < b) {
+    float tmp = a;
+    a = b;
+    b = tmp;
+  }
+  return fminf(a-b, b+m-a);
+}
 
 /* periodic 4Hz function */
 static inline void dc_periodic_4Hz( void )
 {
 static uint8_t dc_shutter_timer = 0;
 
-#ifdef DC_AUTOSHOOT_QUARTERSEC_PERIOD
+ switch (dc_autoshoot) {
-  if (dc_autoshoot == DC_AUTOSHOOT_PERIODIC)
+
-  {
+  case DC_AUTOSHOOT_PERIODIC:
-    if (dc_shutter_timer)
+    if (dc_shutter_timer) {
-    {
       dc_shutter_timer--;
     } else {
-      dc_send_command(DC_SHOOT);
       dc_shutter_timer = dc_autoshoot_quartersec_period;
+      dc_send_command(DC_SHOOT);
       }
-  }
+      break;
-#endif
+
-#ifdef DC_AUTOSHOOT_METER_GRID
+  case DC_AUTOSHOOT_DISTANCE:
-  if (dc_autoshoot == DC_AUTOSHOOT_DISTANCE)
   {
-    // Shoot
+  uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
-    dc_shot_on_utm_north_close_to_100m_grid();
+  if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid)
+  {
+      dc_send_command(DC_SHOOT);
+  }
+  }
+  break;
+
+  case DC_AUTOSHOOT_CIRCLE: {
+    float course = DegOfRad(estimator_psi) - dc_circle_start_angle;
+    if (course < 0.)
+     course += 360.;
+    float current_block = floorf(course/dc_circle_interval);
+
+    if (dc_probing) {
+      if (current_block == dc_circle_last_block) {
+        dc_probing = FALSE;
+      }
+    }
+
+    if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
+      dc_gps_count++;
+      dc_circle_last_block = current_block;
+      dc_send_command(DC_SHOOT);
+    }
+      }
+      break;
+
+  case DC_AUTOSHOOT_SURVEY : {
+    float dist_x = dc_gps_x - estimator_x;
+    float dist_y = dc_gps_y - estimator_y;
+
+    if (dc_probing) {
+      if (dist_x*dist_x + dist_y*dist_y < dc_gps_dist*dc_gps_dist) {
+        dc_probing = FALSE;
+      }
+    }
+
+    if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) {
+      dc_gps_next_dist += dc_gps_dist;
+      dc_gps_count++;
+      dc_send_command(DC_SHOOT);
+    }
+      }
+      break;
+
+  default :
+      dc_autoshoot = DC_AUTOSHOOT_STOP;
  }
-#endif
 }
 
 

sw/airborne/subsystems/navigation/poly_survey_adv.c

@@ -4,7 +4,10 @@
 #include "estimator.h"
 #include "autopilot.h"
 #include "generated/flight_plan.h"
-//#include "modules/digital_cam/dc.h"
+
+#ifdef DIGITAL_CAM
+#include "modules/digital_cam/dc.h"
+#endif
 
 
 /**
@@ -277,7 +280,9 @@ bool_t poly_survey_adv(void)
       psa_stage = SEG;
       NavVerticalAutoThrottleMode(0.0);
       nav_init_stage();
-	  //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
+#ifdef DIGITAL_CAM
+      dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
+#endif
     }
   }
   //fly the segment until seg_end is reached
@@ -285,7 +290,9 @@ bool_t poly_survey_adv(void)
     nav_points(seg_start, seg_end);
     //calculate all needed points for the next flyover
     if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
-	  //dc_stop();
+#ifdef DIGITAL_CAM
+      dc_stop();
+#endif
       VEC_CALC(seg_center1, seg_end, rad_vec, -);
       ret_start.x = seg_end.x - 2*rad_vec.x;
       ret_start.y = seg_end.y - 2*rad_vec.y;
@@ -324,7 +331,9 @@ bool_t poly_survey_adv(void)
     if (NavCourseCloseTo(segment_angle)) {
       psa_stage = SEG;
       nav_init_stage();
-	  //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
+#ifdef DIGITAL_CAM
+      dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
+#endif
     }
   }
 

sw/airborne/subsystems/navigation/spiral.c

@@ -12,7 +12,10 @@
 #include "estimator.h"
 #include "autopilot.h"
 #include "generated/flight_plan.h"
-//#include "modules/digital_cam/dc.h"
+
+#ifdef DIGITAL_CAM
+#include "modules/digital_cam/dc.h"
+#endif
 
 enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
 static enum SpiralStatus CSpiralStatus;
@@ -103,7 +106,9 @@ bool_t SpiralNav(void)
 	  // center reached?
 	  if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) {
 		// nadir image
-		//dc_shutter();
+#ifdef DIGITAL_CAM
+		dc_send_command(DC_SHOOT);
+#endif
 		CSpiralStatus = StartCircle;
 	  }
 	  break;
@@ -117,7 +122,9 @@ bool_t SpiralNav(void)
 		LastCircleY = estimator_y;
 		CSpiralStatus = Circle;
 		// Start helix
-		//dc_Circle(360/Segmente);
+#ifdef DIGITAL_CAM
+		dc_Circle(360/Segmente);
+#endif
 	  }
 	  break;
 	case Circle: {
@@ -143,17 +150,21 @@ bool_t SpiralNav(void)
 	  if(SRad + IRad < Spiralradius)
 		{
 		  SRad = SRad + IRad;
-		  /*if (dc_cam_tracing) {
+#ifdef DIGITAL_CAM
+		  if (dc_cam_tracing) {
 			// calculating Camwinkel for camera alignment
 			TransCurrentZ = estimator_z - ZPoint;
 			CamAngle = atan(SRad/TransCurrentZ) * 180  / 3.14;
 			//dc_cam_angle = CamAngle;
-			}*/
+          }
+#endif
 		}
 	  else {
 		SRad = Spiralradius;
+#ifdef DIGITAL_CAM
 		// Stopps DC
-		//dc_stop();
+		dc_stop();
+#endif
 	  }
 	  CSpiralStatus = Circle;
 	  break;