From 9893a2a7e1b8c7737a0b12cbafb9ce3c7c2541da Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 3 Aug 2012 11:18:03 +0200 Subject: [PATCH] Fix And Update Photogrammetry Calculator [fix] Sweep in radians with GUI in degrees. --- .../modules/photogrammetry_calculator.xml | 6 ++-- .../cartography/photogrammetry_calculator.c | 25 +++++++++++-- .../cartography/photogrammetry_calculator.h | 35 ++++++++++++++----- 3 files changed, 52 insertions(+), 14 deletions(-) diff --git a/conf/settings/modules/photogrammetry_calculator.xml b/conf/settings/modules/photogrammetry_calculator.xml index f5622bbaa5d..082c975451b 100644 --- a/conf/settings/modules/photogrammetry_calculator.xml +++ b/conf/settings/modules/photogrammetry_calculator.xml @@ -15,9 +15,9 @@ - - - + + + diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.c b/sw/airborne/modules/cartography/photogrammetry_calculator.c index 8f722a39b3a..02dc12c7150 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.c +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.c @@ -40,9 +40,13 @@ #define PHOTOGRAMMETRY_SIDELAP 50 #endif +#ifndef PHOTOGRAMMETRY_RESOLUTION +#define PHOTOGRAMMETRY_RESOLUTION 50 +#endif + // Flightplan Paramters -int photogrammetry_sweep_angle = 0; // in rad +float photogrammetry_sweep_angle = 0; // in rad int photogrammetry_sidestep = 0; int photogrammetry_triggerstep = 0; @@ -71,10 +75,10 @@ void init_photogrammetry_calculator(void) photogrammetry_height_max = PHOTOGRAMMETRY_HEIGHT_MAX; photogrammetry_radius_min = PHOTOGRAMMETRY_RADIUS_MIN; - photogrammetry_calculator_update(); + photogrammetry_calculator_update_camera2flightplan(); } -void photogrammetry_calculator_update(void) +void photogrammetry_calculator_update_camera2flightplan(void) { // Photogrammetry Goals @@ -98,4 +102,19 @@ void photogrammetry_calculator_update(void) photogrammetry_triggerstep = viewing_ratio_height * photogrammetry_height * (1.0f - photogrammetry_overlap_f); } +void photogrammetry_calculator_update_flightplan2camera(void) +{ + // Linear Projection Camera Model + float viewing_ratio_height = ((float) PHOTOGRAMMETRY_SENSOR_HEIGHT) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH); + float viewing_ratio_width = ((float) PHOTOGRAMMETRY_SENSOR_WIDTH) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH); + float pixel_projection_width = viewing_ratio_width / ((float)PHOTOGRAMMETRY_PIXELS_WIDTH); + + // Resolution <-> Height + photogrammetry_resolution = photogrammetry_height * 1000.0f * pixel_projection_width; + + // Overlap <-> track width + photogrammetry_sidelap = 100.0f - photogrammetry_sidestep / viewing_ratio_width / photogrammetry_height * 100.0f; + photogrammetry_overlap = 100.0f - photogrammetry_triggerstep / viewing_ratio_height / photogrammetry_height * 100.0f; +} + diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.h b/sw/airborne/modules/cartography/photogrammetry_calculator.h index a906474646e..6b16748b7b8 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.h +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.h @@ -83,7 +83,7 @@ Add to flightplan // Flightplan Variables -extern int photogrammetry_sweep_angle; +extern float photogrammetry_sweep_angle; extern int photogrammetry_sidestep; extern int photogrammetry_triggerstep; extern int photogrammetry_height; @@ -99,34 +99,53 @@ extern int photogrammetry_overlap; extern int photogrammetry_resolution; void init_photogrammetry_calculator(void); -void photogrammetry_calculator_update(void); +void photogrammetry_calculator_update_camera2flightplan(void); +void photogrammetry_calculator_update_flightplan2camera(void); -// Update Parameters on Settings Change +// Update Flightplan on Camera Change #define photogrammetry_calculator_UpdateSideLap(X) { \ photogrammetry_sidelap = X; \ - photogrammetry_calculator_update(); \ + photogrammetry_calculator_update_camera2flightplan(); \ } #define photogrammetry_calculator_UpdateOverLap(X) { \ photogrammetry_overlap = X; \ - photogrammetry_calculator_update(); \ + photogrammetry_calculator_update_camera2flightplan(); \ } #define photogrammetry_calculator_UpdateResolution(X) { \ photogrammetry_resolution = X; \ - photogrammetry_calculator_update(); \ + photogrammetry_calculator_update_camera2flightplan(); \ } +// Update Camera on Flightplan Change +#define photogrammetry_calculator_UpdateHeight(X) { \ + photogrammetry_height = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ +} + +#define photogrammetry_calculator_UpdateSideStep(X) { \ + photogrammetry_sidestep = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ +} + +#define photogrammetry_calculator_UpdateTriggerStep(X) { \ + photogrammetry_triggerstep = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ +} + + // Flightplan Routine Wrappers #define PhotogrammetryCalculatorPolygonSurvey(_WP, _COUNT) { \ WaypointAlt(WP__BASELEG) = photogrammetry_height + GROUND_ALT; \ - int _ang = 90 - photogrammetry_sweep_angle; \ + WaypointAlt(_WP) = photogrammetry_height + GROUND_ALT; \ + int _ang = 90 - DegOfRad(photogrammetry_sweep_angle); \ if (_ang > 90) _ang -= 180; if (_ang < -90) _ang += 180; \ InitializePolygonSurvey((_WP), (_COUNT), 2*photogrammetry_sidestep, _ang); \ } #define PhotogrammetryCalculatorPolygonSurveyADV(_WP, _COUNT) { \ - init_poly_survey_adv((_WP), (_COUNT), photogrammetry_sweep_angle, \ + init_poly_survey_adv((_WP), (_COUNT), DegOfRad(photogrammetry_sweep_angle), \ photogrammetry_sidestep, photogrammetry_triggerstep, \ photogrammetry_radius_min, photogrammetry_height + GROUND_ALT); \ }