diff --git a/conf/messages.xml b/conf/messages.xml
index 2029c96743..0f66cc19cb 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -712,24 +712,34 @@
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
@@ -742,7 +752,12 @@
-
+
+
+
+
+
+
diff --git a/conf/settings/dc.xml b/conf/settings/dc.xml
index b2e44f1e56..c859849583 100644
--- a/conf/settings/dc.xml
+++ b/conf/settings/dc.xml
@@ -19,6 +19,9 @@
+
+
+
diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c
index 99543a6f56..d010c133bb 100644
--- a/sw/airborne/modules/digital_cam/dc.c
+++ b/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;
+}
/*
diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h
index bed149f856..74b90c3c13 100644
--- a/sw/airborne/modules/digital_cam/dc.h
+++ b/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
- if (dc_autoshoot == DC_AUTOSHOOT_PERIODIC)
- {
- if (dc_shutter_timer)
- {
+ case DC_AUTOSHOOT_PERIODIC:
+ if (dc_shutter_timer) {
dc_shutter_timer--;
} else {
- dc_send_command(DC_SHOOT);
dc_shutter_timer = dc_autoshoot_quartersec_period;
- }
- }
-#endif
-#ifdef DC_AUTOSHOOT_METER_GRID
- if (dc_autoshoot == DC_AUTOSHOOT_DISTANCE)
+ dc_send_command(DC_SHOOT);
+ }
+ break;
+
+ case DC_AUTOSHOOT_DISTANCE:
{
- // Shoot
- dc_shot_on_utm_north_close_to_100m_grid();
+ uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
+ 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;
+ }
}
diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.c b/sw/airborne/subsystems/navigation/poly_survey_adv.c
index b56ff6f579..b798c2cc80 100644
--- a/sw/airborne/subsystems/navigation/poly_survey_adv.c
+++ b/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);
}
}
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
index b25ef937e2..bed9fcfa15 100644
--- a/sw/airborne/subsystems/navigation/spiral.c
+++ b/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;