Fix And Update Photogrammetry Calculator

[fix] Sweep in radians with GUI in degrees.
paparazzi · Aug 4, 2012 · 9893a2a · 9893a2a
1 parent 7d9c54b
commit 9893a2a
Show file tree

Hide file tree

Showing 3 changed files with 52 additions and 14 deletions.
diff --git a/conf/settings/modules/photogrammetry_calculator.xml b/conf/settings/modules/photogrammetry_calculator.xml
@@ -15,9 +15,9 @@
       <dl_setting max="500" min="1" step="1" var="photogrammetry_radius_min" shortname="r_min" />
 
 
-      <dl_setting max="500" min="1" step="1" var="photogrammetry_height" shortname="computed height" />
-      <dl_setting max="1000" min="1" step="1" var="photogrammetry_sidestep" shortname="computed sidestep" />
-      <dl_setting max="1000" min="1" step="1" var="photogrammetry_triggerstep" shortname="computed trigger" />
+      <dl_setting max="500" min="1" step="1" var="photogrammetry_height" module="cartography/photogrammetry_calculator" shortname="computed height" handler="UpdateHeight" />
+      <dl_setting max="1000" min="1" step="1" var="photogrammetry_sidestep" module="cartography/photogrammetry_calculator" shortname="computed sidestep" handler="UpdateSideStep" />
+      <dl_setting max="1000" min="1" step="1" var="photogrammetry_triggerstep" module="cartography/photogrammetry_calculator" shortname="computed trigger" handler="UpdateTriggerStep" />
 
     </dl_settings>
   </dl_settings>

diff --git 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
@@ -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);		\
 }