extended digital_cam module: shoot at angular interval on circles, shoot at linear interval on straight lines

conf/messages.xml

@@ -712,24 +712,34 @@
     <field name="azimuth_sp" type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
   </message>
 
-  <message name="BETH_CONTROLLER_TWIST" id="109">
+  <message name="DC_INFO" id="109">
-    <field name="S" type="float"/>
+    <field name="mode" type="int16" unit=""/>
-    <field name="S_dot" type="float"/>
+    <field name="utm_east" type="float" unit="m"/>
-    <field name="U_twt" type="float"/>
+    <field name="utm_north" type="float" unit="m"/>
-    <field name="error" type="float"/>
+    <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>
 
   <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">
@@ -742,7 +752,12 @@
     <field name="one" type="float"/>
   </message>
 
- <!--113 is free -->
+  <message name="BETH_CONTROLLER_TWIST" id="113">
+    <field name="S" type="float"/>
+    <field name="S_dot" type="float"/>
+    <field name="U_twt" type="float"/>
+    <field name="error" type="float"/>
+  </message>
 
   <message name="PAYLOAD" id="114">
     <field name="values" type="uint8[]" unit="none"/>

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/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
@@ -42,18 +56,108 @@ uint16_t dc_photo_nr = 0;
 #include "messages.h"
 #include "downlink.h"
 #include "estimator.h"
+#include "latlong.h"
 
   void dc_send_shot_position(void)
   {
     int16_t phi = DegOfRad(estimator_phi*10.0f);
     int16_t theta = DegOfRad(estimator_theta*10.0f);
     float gps_z = ((float)gps_alt) / 100.0f;
-    DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta,  &gps_course, &gps_gspeed, &gps_itow);
+    int16_t photo_nr = -1;
+
+    latlong_of_utm(gps_utm_east/100, gps_utm_north/100, gps_utm_zone);
+
+//    float gps_lat_send = DegOfRad(latlong_lat);
+//    float gps_lon_send = DegOfRad(latlong_lon);
+
+  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
+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;
+}
 
 
 /*

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 "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,66 @@ 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()
+
 
 /* get settings */
 static inline void dc_init(void)
@@ -108,7 +198,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_north / 100) % 100;
@@ -117,31 +207,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;
+static uint8_t dc_shutter_timer = 0;
+ 
+ switch (dc_autoshoot) {
 
-#ifdef DC_AUTOSHOOT_QUARTERSEC_PERIOD
+  case DC_AUTOSHOOT_PERIODIC:  
-  if (dc_autoshoot == 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);
-  }
+      }
-#endif
+      break;
-#ifdef DC_AUTOSHOOT_METER_GRID
+
-  if (dc_autoshoot == DC_AUTOSHOOT_DISTANCE)
+  case 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);
   }
-#endif
+  }
+  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;
+ }
 }
 
 

sw/airborne/subsystems/navigation/poly_survey_adv.c

@@ -4,6 +4,7 @@
 #include "estimator.h"
 #include "autopilot.h"
 #include "generated/flight_plan.h"
+//uncomment following line for use with digital_cam module
 //#include "modules/digital_cam/dc.h"
 
 
@@ -277,7 +278,8 @@ 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);
+//uncomment following line for use with digital_cam module
+	  //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);
 	}
   }
   //fly the segment until seg_end is reached
@@ -285,6 +287,7 @@ 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)) {
+//uncomment following line for use with digital_cam module
 	  //dc_stop();
 	  VEC_CALC(seg_center1, seg_end, rad_vec, -);
 	  ret_start.x = seg_end.x - 2*rad_vec.x;
@@ -324,7 +327,8 @@ 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);
+//uncomment following line for use with digital_cam module
+	  //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);
 	}
   }
 

sw/airborne/subsystems/navigation/spiral.c

@@ -101,7 +101,7 @@ bool_t SpiralNav(void)
 	  // center reached?
 	  if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) {
 		// nadir image
-		//dc_shutter();
+		//dc_send_command(DC_SHOOT);
 		CSpiralStatus = StartCircle;
 	  }
 	  break;