Skip to content

Commit

Permalink
extended digital_cam module: shoot at angular interval on circles, sh…
Browse files Browse the repository at this point in the history 
…oot at linear interval on straight lines
  • Loading branch information
AxSt authored and AxSt committed Feb 15, 2011
1 parent 91a6bae commit cfde74c
Show file tree
Hide file tree
Showing 6 changed files with 304 additions and 36 deletions.
45 changes: 30 additions & 15 deletions conf/messages.xml
View file
Expand Up @@ -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">
Expand All @@ -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"/>
Expand Down
3 changes: 3 additions & 0 deletions conf/settings/dc.xml
View file
Expand Up @@ -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>
106 changes: 105 additions & 1 deletion sw/airborne/modules/digital_cam/dc.c
View file
Expand Up @@ -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
Expand All @@ -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;
}


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;
}


/*
Expand Down

0 comments on commit cfde74c

Please sign in to comment.