mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-26 16:30:07 +08:00
Merge branch 'cam' into dev
resolved conflicts: conf/messages.xml sw/airborne/modules/digital_cam/dc.c
This commit is contained in:
Felix Ruess
@@ -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>
|
||||
|
||||
|
||||
+34
-13
@@ -130,8 +130,8 @@
|
||||
</message>
|
||||
|
||||
<message name="CAM" id="20">
|
||||
<field name="phi" type="int8" unit="deg"/>
|
||||
<field name="theta" type="int8" unit="deg"/>
|
||||
<field name="phi" type="int16" 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 -->
|
||||
<!-- 64 is free -->
|
||||
<message name="CAM_POINT" id="63">
|
||||
<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="utm_east" type="int32" unit="cm"></field>
|
||||
<field name="utm_north" type="int32" unit="cm"></field>
|
||||
<field name="photo_nr" type="int16"/>
|
||||
<field name="utm_east" type="int32" unit="cm"/>
|
||||
<field name="utm_north" type="int32" unit="cm"/>
|
||||
<field name="z" type="float" unit="m"/>
|
||||
<field name="utm_zone" type="uint8"></field>
|
||||
<field name="phi" type="int16" unit="decideg"></field>
|
||||
<field name="theta" type="int16" unit="decideg"></field>
|
||||
<field name="course" type="int16" unit="decideg"></field>
|
||||
<field name="speed" type="uint16" unit="cm/s"></field>
|
||||
<field name="itow" type="uint32" unit="ms"></field>
|
||||
<field name="utm_zone" type="uint8"/>
|
||||
<field name="phi" type="int16" unit="decideg"/>
|
||||
<field name="theta" type="int16" unit="decideg"/>
|
||||
<field name="course" type="int16" unit="decideg"/>
|
||||
<field name="speed" type="uint16" unit="cm/s"/>
|
||||
<field name="itow" type="uint32" unit="ms"/>
|
||||
</message>
|
||||
|
||||
<message name="TEST_BOARD_RESULTS" ID="111">
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
<!DOCTYPE module SYSTEM "./module.dtd">
|
||||
|
||||
<!--
|
||||
<!--
|
||||
|
||||
// Use (parts of) the following section in airframe file to change
|
||||
// Use (parts of) the following section in airframe file to change
|
||||
|
||||
<section name="DIGITAL_CAMERA" prefix="DC_">
|
||||
|
||||
@@ -13,9 +13,9 @@
|
||||
<configure name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
|
||||
</section>
|
||||
|
||||
# ap.CFLAGS += -DGPS_TRIGGERED_FUNCTION="dc_shoot_on_gps"
|
||||
# ap.CFLAGS += -DDC_GPS_TRIGGER_START=1
|
||||
# ap.CFLAGS += -DDC_GPS_TRIGGER_STOP=3
|
||||
# ap.CFLAGS += -DGPS_TRIGGERED_FUNCTION="dc_shoot_on_gps"
|
||||
# ap.CFLAGS += -DDC_GPS_TRIGGER_START=1
|
||||
# ap.CFLAGS += -DDC_GPS_TRIGGER_STOP=3
|
||||
|
||||
-->
|
||||
|
||||
@@ -33,10 +33,9 @@
|
||||
<define name="DIGITAL_CAM" />
|
||||
<file name="led_cam_ctrl.c"/>
|
||||
<file name="dc.c"/>
|
||||
<define name="SENSOR_SYNC_SEND" value="1" />
|
||||
<define name="SENSOR_SYNC_SEND" value="1" />
|
||||
|
||||
</makefile>
|
||||
|
||||
|
||||
</module>
|
||||
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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 ) {
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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<poly_count-1;i++)
|
||||
if(intercept_two_lines(&tmp,a,b,waypoints[poly_first+i].x,waypoints[poly_first+i].y,waypoints[poly_first+i+1].x,waypoints[poly_first+i+1].y)) {
|
||||
if (count == 0) {
|
||||
if(intercept_two_lines(&tmp,a,b,waypoints[poly_first+i].x,waypoints[poly_first+i].y,waypoints[poly_first+i+1].x,waypoints[poly_first+i+1].y)) {
|
||||
if (count == 0) {
|
||||
*x = tmp;
|
||||
count++;
|
||||
}
|
||||
else {
|
||||
}
|
||||
else {
|
||||
*y = tmp;
|
||||
count++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//wrapover first,last polygon waypoint
|
||||
if (count == 1 && intercept_two_lines(&tmp,a,b,waypoints[poly_first+poly_count-1].x,waypoints[poly_first+poly_count-1].y,waypoints[poly_first].x,waypoints[poly_first].y)) {
|
||||
*y = tmp;
|
||||
count++;
|
||||
*y = tmp;
|
||||
count++;
|
||||
}
|
||||
|
||||
if (count != 2)
|
||||
return FALSE;
|
||||
return FALSE;
|
||||
|
||||
//change points
|
||||
if (fabs(dir_vec.x) > 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<poly_count;i++)
|
||||
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
|
||||
small.x = waypoints[poly_first+i].x;
|
||||
small.y = waypoints[poly_first+i].y;
|
||||
}
|
||||
for(i=1;i<poly_count;i++)
|
||||
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
|
||||
small.x = waypoints[poly_first+i].x;
|
||||
small.y = waypoints[poly_first+i].y;
|
||||
}
|
||||
}
|
||||
else
|
||||
for(i=1;i<poly_count;i++)
|
||||
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
|
||||
small.x = waypoints[poly_first+i].x;
|
||||
small.y = waypoints[poly_first+i].y;
|
||||
}
|
||||
for(i=1;i<poly_count;i++)
|
||||
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user