From cfde74cda8bfb8129f7db46b0d9ea6b1719ec318 Mon Sep 17 00:00:00 2001 From: AxSt Date: Tue, 15 Feb 2011 15:30:06 +0100 Subject: [PATCH] extended digital_cam module: shoot at angular interval on circles, shoot at linear interval on straight lines --- conf/messages.xml | 45 +++-- conf/settings/dc.xml | 3 + sw/airborne/modules/digital_cam/dc.c | 106 ++++++++++- sw/airborne/modules/digital_cam/dc.h | 176 ++++++++++++++++-- .../subsystems/navigation/poly_survey_adv.c | 8 +- sw/airborne/subsystems/navigation/spiral.c | 2 +- 6 files changed, 304 insertions(+), 36 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 2029c96743c..0f66cc19cb6 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -712,24 +712,34 @@ - - - - - + + + + + + + + + + + + + + + - - - + + + - - - - - - + + + + + + @@ -742,7 +752,12 @@ - + + + + + + diff --git a/conf/settings/dc.xml b/conf/settings/dc.xml index b2e44f1e56d..c8598495830 100644 --- a/conf/settings/dc.xml +++ b/conf/settings/dc.xml @@ -19,6 +19,9 @@ + + + diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c index 99543a6f560..d010c133bb8 100644 --- a/sw/airborne/modules/digital_cam/dc.c +++ b/sw/airborne/modules/digital_cam/dc.c @@ -27,13 +27,27 @@ uint8_t dc_autoshoot_meter_grid = 100; uint8_t dc_autoshoot_quartersec_period = 2; dc_autoshoot_type dc_autoshoot = DC_AUTOSHOOT_STOP; +uint16_t dc_gps_count = 0; +uint8_t dc_cam_tracing = 1; +float dc_cam_angle = 0; +float dc_circle_interval = 0; +float dc_circle_start_angle = 0; +float dc_circle_last_block = 0; +float dc_circle_max_blocks = 0; +float dc_gps_dist = 0; +float dc_gps_next_dist = 0; +float dc_gps_x = 0; +float dc_gps_y = 0; + +bool_t dc_probing = FALSE; #ifdef SENSOR_SYNC_SEND uint16_t dc_photo_nr = 0; +uint16_t dc_buffer = 0; #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -42,18 +56,108 @@ uint16_t dc_photo_nr = 0; #include "messages.h" #include "downlink.h" #include "estimator.h" +#include "latlong.h" void dc_send_shot_position(void) { int16_t phi = DegOfRad(estimator_phi*10.0f); int16_t theta = DegOfRad(estimator_theta*10.0f); float gps_z = ((float)gps_alt) / 100.0f; - DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow); + int16_t photo_nr = -1; + + latlong_of_utm(gps_utm_east/100, gps_utm_north/100, gps_utm_zone); + +// float gps_lat_send = DegOfRad(latlong_lat); +// float gps_lon_send = DegOfRad(latlong_lon); + + if (dc_buffer < DC_IMAGE_BUFFER) { + dc_buffer++; dc_photo_nr++; + photo_nr = dc_photo_nr; } + + DOWNLINK_SEND_DC_SHOT(DefaultChannel, + &photo_nr, + &gps_utm_east, + &gps_utm_north, + &gps_z, + &gps_utm_zone, + &phi, + &theta, + &gps_course, + &gps_gspeed, + &gps_itow); + } #endif +uint8_t dc_info(void) { + +#ifdef DOWNLINK_SEND_DC_INFO + float course = DegOfRad(estimator_psi); + DOWNLINK_SEND_DC_INFO(DefaultChannel, + &dc_autoshoot, + &estimator_x, + &estimator_y, + &course, + &dc_buffer, + &dc_gps_dist, + &dc_gps_next_dist, + &dc_gps_x, + &dc_gps_y, + &dc_circle_start_angle, + &dc_circle_interval, + &dc_circle_last_block, + &dc_gps_count, + &dc_autoshoot_quartersec_period); +#endif + return 0; +} +/* shoot on circle */ +uint8_t dc_circle(float interval, float start) { + dc_autoshoot = DC_AUTOSHOOT_CIRCLE; + dc_gps_count = 0; + dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.); + + if(start == DC_IGNORE) { + start = DegOfRad(estimator_psi); + } + + dc_circle_start_angle = fmodf(start, 360.); + if (start < 0.) + start += 360.; + //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval); + dc_circle_last_block = 0; + dc_circle_max_blocks = floorf(360./dc_circle_interval); + dc_probing = TRUE; + dc_info(); + return 0; +} +/* shoot on survey */ +uint8_t dc_survey(float interval, float x, float y) { + dc_autoshoot = DC_AUTOSHOOT_SURVEY; + dc_gps_count = 0; + dc_gps_dist = interval; + + if (x == DC_IGNORE && y == DC_IGNORE) { + dc_gps_x = estimator_x; + dc_gps_y = estimator_y; + } else if (y == DC_IGNORE) { + dc_gps_x = waypoints[(uint8_t)x].x; + dc_gps_y = waypoints[(uint8_t)x].y; + } else { + dc_gps_x = x; + dc_gps_y = y; + } + dc_gps_next_dist = interval; + dc_info(); + return 0; +} +uint8_t dc_stop(void) { + dc_autoshoot = DC_AUTOSHOOT_STOP; + dc_info(); + return 0; +} /* diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h index bed149f856b..74b90c3c138 100644 --- a/sw/airborne/modules/digital_cam/dc.h +++ b/sw/airborne/modules/digital_cam/dc.h @@ -37,11 +37,42 @@ #ifndef DC_H #define DC_H +#include "float.h" #include "std.h" #include "led.h" +#include "estimator.h" +#include "subsystems/nav.h" #include "generated/airframe.h" #include "gps.h" +/* number of images taken since the last change of dc_mode */ +extern uint16_t dc_gps_count; + +/* distance between dc shots in meters */ +extern float dc_gps_dist; + +extern float dc_gps_next_dist; + +/* angle a where first image will be taken at a + delta */ +extern float dc_circle_start_angle; + +/* angle between dc shots in degree */ +extern float dc_circle_interval; + +extern float dc_circle_max_blocks; + +/* point of reference for the distance based mode */ +extern float dc_gps_x, dc_gps_y; + +extern float dc_circle_last_block; + +extern bool_t dc_probing; + +extern uint8_t dc_buffer_timer; + +/* camera angle */ +extern float dc_cam_angle; +extern uint8_t dc_cam_tracing; /* Generic Set of Digital Camera Commands */ typedef enum { @@ -76,7 +107,9 @@ typedef enum { DC_AUTOSHOOT_STOP = 0, DC_AUTOSHOOT_PERIODIC = 1, DC_AUTOSHOOT_DISTANCE = 2, - DC_AUTOSHOOT_EXT_TRIG = 3 + DC_AUTOSHOOT_EXT_TRIG = 3, + DC_AUTOSHOOT_SURVEY = 4, + DC_AUTOSHOOT_CIRCLE = 5 } dc_autoshoot_type; extern dc_autoshoot_type dc_autoshoot; @@ -93,9 +126,66 @@ void dc_send_shot_position(void); #define dc_send_shot_position() {} #endif +/* Macro value used to indicate a discardable argument */ +#ifndef DC_IGNORE +#define DC_IGNORE FLT_MAX +#endif + +/* Default values for buffer control */ +#ifndef DC_IMAGE_BUFFER +#define DC_IMAGE_BUFFER 255 +#endif + +#ifndef DC_IMAGE_BUFFER_TPI +#define DC_IMAGE_BUFFER_TPI 0 +#endif + /****************************************************************** * FUNCTIONS *****************************************************************/ +/** + Sets the dc control in circle mode. + The 'start' value is the reference course and 'intervall' + the minimum angle between shots. + If 'start' is 0 the current course is used instead. + + In this mode the dc control assumes a perfect circular + course. + The first picture is taken at angle start+interval. +*/ +extern uint8_t dc_circle(float interval, float start); + +#define dc_Circle(interval) dc_circle(interval, DC_IGNORE) + +/** + Sets the dc control in distance mode. + The values of 'x' and 'y' are the coordinates + of the reference point used for the distance + calculations. + If 'y' is 0 the value of 'x' is interpreted + as index of a waypoint declared in the flight plan. + If both 'x' and 'y' are 0 the current position + will be used as reference point. + + In this mode, the dc control assumes a perfect + line formed course since the distance is calculated + relative to the first given point of reference. + So not usable for circles or other comparable + shapes. +*/ +extern uint8_t dc_survey(float interval, float x, float y); + +#define dc_Survey(interval) dc_survey(interval, DC_IGNORE, DC_IGNORE) + + +/** + Sets the dc control in inactive mode, + stopping all current actions. +*/ +extern uint8_t dc_stop(void); + +#define dc_Stop(_) dc_stop() + /* get settings */ static inline void dc_init(void) @@ -108,7 +198,7 @@ static inline void dc_init(void) #endif } -/* shoot on grid */ +/* shoot on grid static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) { uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100; @@ -117,31 +207,83 @@ static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) dc_send_command(DC_SHOOT); } } +*/ +static float dim_mod(float a, float b, float m) { + if (a < b) { + float tmp = a; + a = b; + b = tmp; + } + return fminf(a-b, b+m-a); +} /* periodic 4Hz function */ static inline void dc_periodic_4Hz( void ) { - static uint8_t dc_shutter_timer = 0; +static uint8_t dc_shutter_timer = 0; + + switch (dc_autoshoot) { -#ifdef DC_AUTOSHOOT_QUARTERSEC_PERIOD - if (dc_autoshoot == DC_AUTOSHOOT_PERIODIC) - { - if (dc_shutter_timer) - { + case DC_AUTOSHOOT_PERIODIC: + if (dc_shutter_timer) { dc_shutter_timer--; } else { - dc_send_command(DC_SHOOT); dc_shutter_timer = dc_autoshoot_quartersec_period; - } - } -#endif -#ifdef DC_AUTOSHOOT_METER_GRID - if (dc_autoshoot == DC_AUTOSHOOT_DISTANCE) + dc_send_command(DC_SHOOT); + } + break; + + case DC_AUTOSHOOT_DISTANCE: { - // Shoot - dc_shot_on_utm_north_close_to_100m_grid(); + uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100; + if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid) + { + dc_send_command(DC_SHOOT); } -#endif + } + break; + + case DC_AUTOSHOOT_CIRCLE: { + float course = DegOfRad(estimator_psi) - dc_circle_start_angle; + if (course < 0.) + course += 360.; + float current_block = floorf(course/dc_circle_interval); + + if (dc_probing) { + if (current_block == dc_circle_last_block) { + dc_probing = FALSE; + } + } + + if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) { + dc_gps_count++; + dc_circle_last_block = current_block; + dc_send_command(DC_SHOOT); + } + } + break; + + case DC_AUTOSHOOT_SURVEY : { + float dist_x = dc_gps_x - estimator_x; + float dist_y = dc_gps_y - estimator_y; + + if (dc_probing) { + if (dist_x*dist_x + dist_y*dist_y < dc_gps_dist*dc_gps_dist) { + dc_probing = FALSE; + } + } + + if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) { + dc_gps_next_dist += dc_gps_dist; + dc_gps_count++; + dc_send_command(DC_SHOOT); + } + } + break; + + default : + dc_autoshoot = DC_AUTOSHOOT_STOP; + } } diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.c b/sw/airborne/subsystems/navigation/poly_survey_adv.c index b56ff6f5797..b798c2cc808 100644 --- a/sw/airborne/subsystems/navigation/poly_survey_adv.c +++ b/sw/airborne/subsystems/navigation/poly_survey_adv.c @@ -4,6 +4,7 @@ #include "estimator.h" #include "autopilot.h" #include "generated/flight_plan.h" +//uncomment following line for use with digital_cam module //#include "modules/digital_cam/dc.h" @@ -277,7 +278,8 @@ bool_t poly_survey_adv(void) psa_stage = SEG; NavVerticalAutoThrottleMode(0.0); nav_init_stage(); - //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5); +//uncomment following line for use with digital_cam module + //dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5); } } //fly the segment until seg_end is reached @@ -285,6 +287,7 @@ bool_t poly_survey_adv(void) nav_points(seg_start, seg_end); //calculate all needed points for the next flyover if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) { +//uncomment following line for use with digital_cam module //dc_stop(); VEC_CALC(seg_center1, seg_end, rad_vec, -); ret_start.x = seg_end.x - 2*rad_vec.x; @@ -324,7 +327,8 @@ bool_t poly_survey_adv(void) if (NavCourseCloseTo(segment_angle)) { psa_stage = SEG; nav_init_stage(); - //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5); +//uncomment following line for use with digital_cam module + //dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5); } } diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index b25ef937e23..bed9fcfa15f 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -101,7 +101,7 @@ bool_t SpiralNav(void) // center reached? if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) { // nadir image - //dc_shutter(); + //dc_send_command(DC_SHOOT); CSpiralStatus = StartCircle; } break;