diff --git a/conf/airframes/funjet_cam_example.xml b/conf/airframes/funjet_cam_example.xml
index 839a8f761d..a62561af4a 100644
--- a/conf/airframes/funjet_cam_example.xml
+++ b/conf/airframes/funjet_cam_example.xml
@@ -30,7 +30,7 @@
-
+
@@ -44,6 +44,9 @@
+
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index 63582aceeb..0df612d14c 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -130,8 +130,8 @@
-
-
+
+
@@ -489,8 +489,29 @@
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -723,16 +744,16 @@
-
-
-
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
diff --git a/conf/modules/digital_cam.xml b/conf/modules/digital_cam.xml
index e190c57f90..627061410c 100644
--- a/conf/modules/digital_cam.xml
+++ b/conf/modules/digital_cam.xml
@@ -1,8 +1,8 @@
-
@@ -33,10 +33,9 @@
-
+
-
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/ap_downlink.h b/sw/airborne/ap_downlink.h
index f0957d6cbe..017f716362 100644
--- a/sw/airborne/ap_downlink.h
+++ b/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 */
diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c
index 5e40d64ad6..fb46f92cb0 100644
--- a/sw/airborne/modules/cam_control/cam.c
+++ b/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,31 +103,98 @@ void cam_init( void ) {
}
void cam_periodic( void ) {
- switch (cam_mode) {
- case CAM_MODE_OFF:
- break;
- case CAM_MODE_ANGLES:
+#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();
+ break;
+ case CAM_MODE_NADIR:
+ cam_nadir();
+ break;
+ case CAM_MODE_XY_TARGET:
+ cam_target();
+ break;
+ case CAM_MODE_WP_TARGET:
+ cam_waypoint_target();
+ break;
+ 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();
- break;
- case CAM_MODE_NADIR:
- cam_nadir();
- break;
- case CAM_MODE_XY_TARGET:
- cam_target();
- break;
- case CAM_MODE_WP_TARGET:
- cam_waypoint_target();
- break;
- case CAM_MODE_AC_TARGET:
- cam_ac_target();
- break;
+ 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);
diff --git a/sw/airborne/modules/cam_control/cam.h b/sw/airborne/modules/cam_control/cam.h
index 062f021e64..2cdfcd9c19 100644
--- a/sw/airborne/modules/cam_control/cam.h
+++ b/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
diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c
index 5e13c9abc5..35169b8a38 100644
--- a/sw/airborne/modules/cam_control/point.c
+++ b/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
+#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
}
diff --git a/sw/airborne/modules/cam_control/point.h b/sw/airborne/modules/cam_control/point.h
index 3d7e6447f5..96d2f31109 100644
--- a/sw/airborne/modules/cam_control/point.h
+++ b/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,
diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c
index 9589bda2f5..e5775de29b 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
@@ -48,13 +62,98 @@ 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);
+ float gps_z = ((float)gps.hmsl) / 1000.0f;
+ 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 ) {
}
}
*/
-
diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h
index 85170fc0a4..faf4b4c854 100644
--- a/sw/airborne/modules/digital_cam/dc.h
+++ b/sw/airborne/modules/digital_cam/dc.h
@@ -24,8 +24,8 @@
/** \file dc.h
* \brief Standard Digital Camera Control Interface
*
- * -Standard IO
- * -I2C Control
+ * -Standard IO
+ * -I2C Control
*
* Usage: (from the flight plan, the settings or any airborne code):
* - dc_send_command( )
@@ -37,34 +37,65 @@
#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 {
- DC_GET_STATUS = 0,
+ DC_GET_STATUS = 0,
- DC_HOLD = 13,
- DC_SHOOT = 32,
+ DC_HOLD = 13,
+ DC_SHOOT = 32,
- DC_WIDER = 'w',
- DC_TALLER = 't',
+ DC_WIDER = 'w',
+ DC_TALLER = 't',
- DC_UP = 'u',
- DC_DOWN = 'd',
- DC_CENTER = 'c',
- DC_LEFT = 'l',
- DC_RIGHT = 'r',
+ DC_UP = 'u',
+ DC_DOWN = 'd',
+ DC_CENTER = 'c',
+ DC_LEFT = 'l',
+ DC_RIGHT = 'r',
- DC_MENU = 'm',
- DC_HOME = 'h',
- DC_PLAY = 'p',
+ DC_MENU = 'm',
+ DC_HOME = 'h',
+ DC_PLAY = 'p',
- DC_ON = 'O',
- DC_OFF = 'o',
+ DC_ON = 'O',
+ DC_OFF = 'o',
} dc_command_type;
@@ -73,10 +104,12 @@ static inline void dc_send_command(uint8_t cmd);
/* Auotmatic Digital Camera Photo Triggering */
typedef enum {
- DC_AUTOSHOOT_STOP = 0,
- DC_AUTOSHOOT_PERIODIC = 1,
- DC_AUTOSHOOT_DISTANCE = 2,
- DC_AUTOSHOOT_EXT_TRIG = 3
+ DC_AUTOSHOOT_STOP = 0,
+ DC_AUTOSHOOT_PERIODIC = 1,
+ DC_AUTOSHOOT_DISTANCE = 2,
+ 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;
+static uint8_t dc_shutter_timer = 0;
-#ifdef DC_AUTOSHOOT_QUARTERSEC_PERIOD
- if (dc_autoshoot == DC_AUTOSHOOT_PERIODIC)
- {
- if (dc_shutter_timer)
- {
+ switch (dc_autoshoot) {
+
+ 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..0edfc465b7 100644
--- a/sw/airborne/subsystems/navigation/poly_survey_adv.c
+++ b/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
/**
@@ -108,41 +111,41 @@ static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
point2d tmp;
for (i=0;i fabs(dir_vec.y)) {
- if ((y->x - x->x) / dir_vec.x < 0.0){
- tmp = *x;
- *x = *y;
- *y = tmp;
- }
+ if ((y->x - x->x) / dir_vec.x < 0.0){
+ tmp = *x;
+ *x = *y;
+ *y = tmp;
+ }
}
else
- if ((y->y - x->y) / dir_vec.y < 0.0) {
- tmp = *x;
- *x = *y;
- *y = tmp;
- }
+ if ((y->y - x->y) / dir_vec.y < 0.0) {
+ tmp = *x;
+ *x = *y;
+ *y = tmp;
+ }
return TRUE;
}
@@ -179,32 +182,32 @@ bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float s
if (return_angle > 359) return_angle -= 360;
if (angle <= 45.0 || angle >= 315.0) {
- //north
- dir_vec.y = 1.0;
- dir_vec.x = 1.0*tanf(angle_rad);
- sweep.x = 1.0;
- sweep.y = - dir_vec.x / dir_vec.y;
+ //north
+ dir_vec.y = 1.0;
+ dir_vec.x = 1.0*tanf(angle_rad);
+ sweep.x = 1.0;
+ sweep.y = - dir_vec.x / dir_vec.y;
}
else if (angle <= 135.0) {
- //east
- dir_vec.x = 1.0;
- dir_vec.y = 1.0/tanf(angle_rad);
- sweep.y = - 1.0;
- sweep.x = dir_vec.y / dir_vec.x;
+ //east
+ dir_vec.x = 1.0;
+ dir_vec.y = 1.0/tanf(angle_rad);
+ sweep.y = - 1.0;
+ sweep.x = dir_vec.y / dir_vec.x;
}
else if (angle <= 225.0) {
- //south
- dir_vec.y = -1.0;
- dir_vec.x = -1.0*tanf(angle_rad);
- sweep.x = -1.0;
- sweep.y = dir_vec.x / dir_vec.y;
+ //south
+ dir_vec.y = -1.0;
+ dir_vec.x = -1.0*tanf(angle_rad);
+ sweep.x = -1.0;
+ sweep.y = dir_vec.x / dir_vec.y;
}
else {
- //west
- dir_vec.x = -1.0;
- dir_vec.y = -1.0/tanf(angle_rad);
- sweep.y = 1.0;
- sweep.x = - dir_vec.y / dir_vec.x;
+ //west
+ dir_vec.x = -1.0;
+ dir_vec.y = -1.0/tanf(angle_rad);
+ sweep.y = 1.0;
+ sweep.x = - dir_vec.y / dir_vec.x;
}
//normalize
@@ -225,18 +228,18 @@ bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float s
//cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (div < 0.0) {
- for(i=1;i 0.0) {
- small.x = waypoints[poly_first+i].x;
- small.y = waypoints[poly_first+i].y;
- }
+ for(i=1;i 0.0) {
+ small.x = waypoints[poly_first+i].x;
+ small.y = waypoints[poly_first+i].y;
+ }
}
else
- for(i=1;i 0.0) {
- small.x = waypoints[poly_first+i].x;
- small.y = waypoints[poly_first+i].y;
- }
+ for(i=1;i 0.0) {
+ small.x = waypoints[poly_first+i].x;
+ small.y = waypoints[poly_first+i].y;
+ }
//calculate the line the defines the first flyover
seg_start.x = small.x + 0.5*sweep_vec.x;
@@ -244,8 +247,8 @@ bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float s
VEC_CALC(seg_end, seg_start, dir_vec, +);
if (!get_two_intersects(&seg_start, &seg_end, seg_start, seg_end)) {
- psa_stage = ERR;
- return FALSE;
+ psa_stage = ERR;
+ return FALSE;
}
//center of the entry circle
@@ -270,62 +273,68 @@ bool_t poly_survey_adv(void)
{
//entry circle around entry-center until the desired altitude is reached
if (psa_stage == ENTRY) {
- nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
- if (NavCourseCloseTo(segment_angle)
- && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
- && fabs(estimator_z - psa_altitude) <= 20) {
- 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);
- }
+ nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
+ if (NavCourseCloseTo(segment_angle)
+ && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
+ && fabs(estimator_z - psa_altitude) <= 20) {
+ psa_stage = SEG;
+ NavVerticalAutoThrottleMode(0.0);
+ nav_init_stage();
+#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
if (psa_stage == SEG) {
- 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();
- 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;
+ 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)) {
+#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;
- //if we get no intersection the survey is finished
- if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
- return FALSE;
+ //if we get no intersection the survey is finished
+ if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
+ return FALSE;
- ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
- ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;
+ ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
+ ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;
- seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
- seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);
+ seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
+ seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);
- psa_stage = TURN1;
- nav_init_stage();
- }
+ psa_stage = TURN1;
+ nav_init_stage();
+ }
}
//turn from stage to return
else if (psa_stage == TURN1) {
- nav_circle_XY(seg_center1.x, seg_center1.y, -psa_min_rad);
- if (NavCourseCloseTo(return_angle)) {
- psa_stage = RET;
- nav_init_stage();
- }
- //return
+ nav_circle_XY(seg_center1.x, seg_center1.y, -psa_min_rad);
+ if (NavCourseCloseTo(return_angle)) {
+ psa_stage = RET;
+ nav_init_stage();
+ }
+ //return
} else if (psa_stage == RET) {
- nav_points(ret_start, ret_end);
- if (nav_approaching_xy(ret_end.x, ret_end.y, ret_start.x, ret_start.y, 0)) {
- psa_stage = TURN2;
- nav_init_stage();
- }
- //turn from return to stage
+ nav_points(ret_start, ret_end);
+ if (nav_approaching_xy(ret_end.x, ret_end.y, ret_start.x, ret_start.y, 0)) {
+ psa_stage = TURN2;
+ nav_init_stage();
+ }
+ //turn from return to stage
} else if (psa_stage == TURN2) {
- nav_circle_XY(seg_center2.x, seg_center2.y, -(2*psa_min_rad+psa_sweep_width)*0.5);
- 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);
- }
+ nav_circle_XY(seg_center2.x, seg_center2.y, -(2*psa_min_rad+psa_sweep_width)*0.5);
+ if (NavCourseCloseTo(segment_angle)) {
+ psa_stage = SEG;
+ nav_init_stage();
+#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
+ }
}
return TRUE;
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
index 219e778603..78f7f490af 100644
--- a/sw/airborne/subsystems/navigation/spiral.c
+++ b/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;