Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
AxSt
2011-02-15 15:30:06 +01:00
parent 91a6bae5bd
commit cfde74cda8
6 changed files with 305 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+30 -15
View File
@@ -712,24 +712,34 @@
<field name="azimuth_sp" type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
</message>
<message name="BETH_CONTROLLER_TWIST" id="109">
<field name="S" type="float"/>
<field name="S_dot" type="float"/>
<field name="U_twt" type="float"/>
<field name="error" type="float"/>
<message name="DC_INFO" id="109">
<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>
<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">
@@ -742,7 +752,12 @@
<field name="one" type="float"/>
</message>
<!--113 is free -->
<message name="BETH_CONTROLLER_TWIST" id="113">
<field name="S" type="float"/>
<field name="S_dot" type="float"/>
<field name="U_twt" type="float"/>
<field name="error" type="float"/>
</message>
<message name="PAYLOAD" id="114">
<field name="values" type="uint8[]" unit="none"/>
conf/settings/dc.xml
+3
View File
@@ -19,6 +19,9 @@
<dl_setting max="255" min="1" step="1" var="dc_autoshoot_quartersec_period" handler="Periodic" shortname="Periodic" param="DC_AUTOSHOOT_QUARTERSEC_PERIOD" unit="quarter-sec"/>
<dl_setting max="5" min="0" step="1" var="dc_autoshoot_meter_grid" shortname="UTM%" param="DC_AUTOSHOOT_METER_GRID" unit="meter"/>
<dl_setting max="250" min="0" step="5" module="digital_cam/dc" var="dc_gps_dist" handler="Survey" shortname="Linear-Interval"/>
<dl_setting max="90" min="5" step="5" module="digital_cam/dc" var="dc_circle_interval" handler="Circle" shortname="Circle-Interval"/>
<dl_setting max="1" min="0" step="1" var="dc_cam_tracing" shortname="Cam-Tracing"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/modules/digital_cam/dc.c
+106 -2
View File
@@ -27,13 +27,27 @@
uint8_t dc_autoshoot_meter_grid = 100;
uint8_t dc_autoshoot_quartersec_period = 2;
dc_autoshoot_type dc_autoshoot = DC_AUTOSHOOT_STOP;
uint16_t dc_gps_count = 0;
uint8_t dc_cam_tracing = 1;
float dc_cam_angle = 0;
float dc_circle_interval = 0;
float dc_circle_start_angle = 0;
float dc_circle_last_block = 0;
float dc_circle_max_blocks = 0;
float dc_gps_dist = 0;
float dc_gps_next_dist = 0;
float dc_gps_x = 0;
float dc_gps_y = 0;
bool_t dc_probing = FALSE;
#ifdef SENSOR_SYNC_SEND
uint16_t dc_photo_nr = 0;
uint16_t dc_buffer = 0;
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -42,18 +56,108 @@ uint16_t dc_photo_nr = 0;
#include "messages.h"
#include "downlink.h"
#include "estimator.h"
#include "latlong.h"
void dc_send_shot_position(void)
{
int16_t phi = DegOfRad(estimator_phi*10.0f);
int16_t theta = DegOfRad(estimator_theta*10.0f);
float gps_z = ((float)gps_alt) / 100.0f;
DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow);
int16_t photo_nr = -1;
latlong_of_utm(gps_utm_east/100, gps_utm_north/100, gps_utm_zone);
// float gps_lat_send = DegOfRad(latlong_lat);
// float gps_lon_send = DegOfRad(latlong_lon);
if (dc_buffer < DC_IMAGE_BUFFER) {
dc_buffer++;
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
#endif
DOWNLINK_SEND_DC_SHOT(DefaultChannel,
&photo_nr,
&gps_utm_east,
&gps_utm_north,
&gps_z,
&gps_utm_zone,
&phi,
&theta,
&gps_course,
&gps_gspeed,
&gps_itow);
}
#endif
uint8_t dc_info(void) {
#ifdef DOWNLINK_SEND_DC_INFO
float course = DegOfRad(estimator_psi);
DOWNLINK_SEND_DC_INFO(DefaultChannel,
&dc_autoshoot,
&estimator_x,
&estimator_y,
&course,
&dc_buffer,
&dc_gps_dist,
&dc_gps_next_dist,
&dc_gps_x,
&dc_gps_y,
&dc_circle_start_angle,
&dc_circle_interval,
&dc_circle_last_block,
&dc_gps_count,
&dc_autoshoot_quartersec_period);
#endif
return 0;
}
/* shoot on circle */
uint8_t dc_circle(float interval, float start) {
dc_autoshoot = DC_AUTOSHOOT_CIRCLE;
dc_gps_count = 0;
dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
if(start == DC_IGNORE) {
start = DegOfRad(estimator_psi);
}
dc_circle_start_angle = fmodf(start, 360.);
if (start < 0.)
start += 360.;
//dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
dc_circle_last_block = 0;
dc_circle_max_blocks = floorf(360./dc_circle_interval);
dc_probing = TRUE;
dc_info();
return 0;
}
/* shoot on survey */
uint8_t dc_survey(float interval, float x, float y) {
dc_autoshoot = DC_AUTOSHOOT_SURVEY;
dc_gps_count = 0;
dc_gps_dist = interval;
if (x == DC_IGNORE && y == DC_IGNORE) {
dc_gps_x = estimator_x;
dc_gps_y = estimator_y;
} else if (y == DC_IGNORE) {
dc_gps_x = waypoints[(uint8_t)x].x;
dc_gps_y = waypoints[(uint8_t)x].y;
} else {
dc_gps_x = x;
dc_gps_y = y;
}
dc_gps_next_dist = interval;
dc_info();
return 0;
}
uint8_t dc_stop(void) {
dc_autoshoot = DC_AUTOSHOOT_STOP;
dc_info();
return 0;
}
/*
sw/airborne/modules/digital_cam/dc.h
+159 -17
View File
@@ -37,11 +37,42 @@
#ifndef DC_H
#define DC_H
#include "float.h"
#include "std.h"
#include "led.h"
#include "estimator.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include "gps.h"
/* number of images taken since the last change of dc_mode */
extern uint16_t dc_gps_count;
/* distance between dc shots in meters */
extern float dc_gps_dist;
extern float dc_gps_next_dist;
/* angle a where first image will be taken at a + delta */
extern float dc_circle_start_angle;
/* angle between dc shots in degree */
extern float dc_circle_interval;
extern float dc_circle_max_blocks;
/* point of reference for the distance based mode */
extern float dc_gps_x, dc_gps_y;
extern float dc_circle_last_block;
extern bool_t dc_probing;
extern uint8_t dc_buffer_timer;
/* camera angle */
extern float dc_cam_angle;
extern uint8_t dc_cam_tracing;
/* Generic Set of Digital Camera Commands */
typedef enum {
@@ -76,7 +107,9 @@ typedef enum {
DC_AUTOSHOOT_STOP = 0,
DC_AUTOSHOOT_PERIODIC = 1,
DC_AUTOSHOOT_DISTANCE = 2,
DC_AUTOSHOOT_EXT_TRIG = 3
DC_AUTOSHOOT_EXT_TRIG = 3,
DC_AUTOSHOOT_SURVEY = 4,
DC_AUTOSHOOT_CIRCLE = 5
} dc_autoshoot_type;
extern dc_autoshoot_type dc_autoshoot;
@@ -93,9 +126,66 @@ void dc_send_shot_position(void);
#define dc_send_shot_position() {}
#endif
/* Macro value used to indicate a discardable argument */
#ifndef DC_IGNORE
#define DC_IGNORE FLT_MAX
#endif
/* Default values for buffer control */
#ifndef DC_IMAGE_BUFFER
#define DC_IMAGE_BUFFER 255
#endif
#ifndef DC_IMAGE_BUFFER_TPI
#define DC_IMAGE_BUFFER_TPI 0
#endif
/******************************************************************
* FUNCTIONS
*****************************************************************/
/**
Sets the dc control in circle mode.
The 'start' value is the reference course and 'intervall'
the minimum angle between shots.
If 'start' is 0 the current course is used instead.
In this mode the dc control assumes a perfect circular
course.
The first picture is taken at angle start+interval.
*/
extern uint8_t dc_circle(float interval, float start);
#define dc_Circle(interval) dc_circle(interval, DC_IGNORE)
/**
Sets the dc control in distance mode.
The values of 'x' and 'y' are the coordinates
of the reference point used for the distance
calculations.
If 'y' is 0 the value of 'x' is interpreted
as index of a waypoint declared in the flight plan.
If both 'x' and 'y' are 0 the current position
will be used as reference point.
In this mode, the dc control assumes a perfect
line formed course since the distance is calculated
relative to the first given point of reference.
So not usable for circles or other comparable
shapes.
*/
extern uint8_t dc_survey(float interval, float x, float y);
#define dc_Survey(interval) dc_survey(interval, DC_IGNORE, DC_IGNORE)
/**
Sets the dc control in inactive mode,
stopping all current actions.
*/
extern uint8_t dc_stop(void);
#define dc_Stop(_) dc_stop()
/* get settings */
static inline void dc_init(void)
@@ -108,7 +198,7 @@ static inline void dc_init(void)
#endif
}
/* shoot on grid */
/* shoot on grid
static inline void dc_shot_on_utm_north_close_to_100m_grid( void )
{
uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
@@ -117,31 +207,83 @@ static inline void dc_shot_on_utm_north_close_to_100m_grid( void )
dc_send_command(DC_SHOOT);
}
}
*/
static float dim_mod(float a, float b, float m) {
if (a < b) {
float tmp = a;
a = b;
b = tmp;
}
return fminf(a-b, b+m-a);
}
/* periodic 4Hz function */
static inline void dc_periodic_4Hz( void )
{
static uint8_t dc_shutter_timer = 0;
static uint8_t dc_shutter_timer = 0;
switch (dc_autoshoot) {
#ifdef DC_AUTOSHOOT_QUARTERSEC_PERIOD
if (dc_autoshoot == DC_AUTOSHOOT_PERIODIC)
{
if (dc_shutter_timer)
{
case DC_AUTOSHOOT_PERIODIC:
if (dc_shutter_timer) {
dc_shutter_timer--;
} else {
dc_send_command(DC_SHOOT);
dc_shutter_timer = dc_autoshoot_quartersec_period;
}
}
#endif
#ifdef DC_AUTOSHOOT_METER_GRID
if (dc_autoshoot == DC_AUTOSHOOT_DISTANCE)
dc_send_command(DC_SHOOT);
}
break;
case DC_AUTOSHOOT_DISTANCE:
{
// Shoot
dc_shot_on_utm_north_close_to_100m_grid();
uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid)
{
dc_send_command(DC_SHOOT);
}
#endif
}
break;
case DC_AUTOSHOOT_CIRCLE: {
float course = DegOfRad(estimator_psi) - dc_circle_start_angle;
if (course < 0.)
course += 360.;
float current_block = floorf(course/dc_circle_interval);
if (dc_probing) {
if (current_block == dc_circle_last_block) {
dc_probing = FALSE;
}
}
if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
dc_gps_count++;
dc_circle_last_block = current_block;
dc_send_command(DC_SHOOT);
}
}
break;
case DC_AUTOSHOOT_SURVEY : {
float dist_x = dc_gps_x - estimator_x;
float dist_y = dc_gps_y - estimator_y;
if (dc_probing) {
if (dist_x*dist_x + dist_y*dist_y < dc_gps_dist*dc_gps_dist) {
dc_probing = FALSE;
}
}
if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) {
dc_gps_next_dist += dc_gps_dist;
dc_gps_count++;
dc_send_command(DC_SHOOT);
}
}
break;
default :
dc_autoshoot = DC_AUTOSHOOT_STOP;
}
}
sw/airborne/subsystems/navigation/poly_survey_adv.c
+6 -2
View File
@@ -4,6 +4,7 @@
#include "estimator.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
//uncomment following line for use with digital_cam module
//#include "modules/digital_cam/dc.h"
@@ -277,7 +278,8 @@ bool_t poly_survey_adv(void)
psa_stage = SEG;
NavVerticalAutoThrottleMode(0.0);
nav_init_stage();
//dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
//uncomment following line for use with digital_cam module
//dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
}
}
//fly the segment until seg_end is reached
@@ -285,6 +287,7 @@ bool_t poly_survey_adv(void)
nav_points(seg_start, seg_end);
//calculate all needed points for the next flyover
if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
//uncomment following line for use with digital_cam module
//dc_stop();
VEC_CALC(seg_center1, seg_end, rad_vec, -);
ret_start.x = seg_end.x - 2*rad_vec.x;
@@ -324,7 +327,8 @@ bool_t poly_survey_adv(void)
if (NavCourseCloseTo(segment_angle)) {
psa_stage = SEG;
nav_init_stage();
//dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
//uncomment following line for use with digital_cam module
//dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
}
}
sw/airborne/subsystems/navigation/spiral.c
+1 -1
View File
@@ -101,7 +101,7 @@ bool_t SpiralNav(void)
// center reached?
if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) {
// nadir image
//dc_shutter();
//dc_send_command(DC_SHOOT);
CSpiralStatus = StartCircle;
}
break;