diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index b93cc056aa..413a1c06fc 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -127,6 +127,11 @@ struct FloatRates {
     n = sqrtf((v).x*(v).x + (v).y*(v).y);      \
   }
 
+#define FLOAT_VECT2_NORMALIZE(_v) {             \
+    const float n = sqrtf((_v).x*(_v).x + (_v).y*(_v).y);   \
+    FLOAT_VECT2_SMUL(_v, _v, 1./n);             \
+  }
+
 
 /*
  * Dimension 3 Vectors
diff --git a/sw/airborne/subsystems/navigation/zamboni_survey.c b/sw/airborne/subsystems/navigation/zamboni_survey.c
index 1bbd3e3f4d..46923c326d 100644
--- a/sw/airborne/subsystems/navigation/zamboni_survey.c
+++ b/sw/airborne/subsystems/navigation/zamboni_survey.c
@@ -1,3 +1,30 @@
+/*
+ * Copyright (C) 2013 Jorn Anke, Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/navigation/zamboni_survey.c
+ *
+ * Zamboni pattern survey for fixedwings.
+ */
+
 #include "zamboni_survey.h"
 
 #include "subsystems/nav.h"
@@ -8,230 +35,175 @@
 #include "modules/digital_cam/dc.h"
 #endif
 
-int counter;
-/**
-variables used to store values from the flight plan
-**/
-float x_wp_center, y_wp_center;
-float x_wp_dir, y_wp_dir;
-float z_sweep_length;
-float z_sweep_spacing;
-int z_sweep_lines;
-float z_shot_dist;
-float z_altitude;
+
+struct ZamboniSurvey zs;
 
 /**
-static variables, used for initial calculations
-**/
-// properties for the filightpattern
-float flight_angle, zamboni_return_angle;
-float dx_sweep_width, dy_sweep_width;
-float dx_flightline, dy_flightline;
-float dx_flight_vec, dy_flight_vec;
-float turnradius1, turnradius2;
-int laps;
-
-// points for navigation
-float x_seg_start, y_seg_start;
-float x_seg_end, y_seg_end;
-float x_turn_center1, y_turn_center1;
-float x_turn_center2, y_turn_center2;
-float x_ret_start, y_ret_start;
-float x_ret_end, y_ret_end;
-
-// variables used for initial calculations
-  float dx_flight_wpts, dy_flight_wpts;
-  float len;
-
-// constant for storing value for pre-leave-angle, (leave turncircles a small angle before the 180deg turns are compleated to get a smoother transition to flight-lines)
-  int pre_leave_angle=2;
-
-z_survey_stage z_stage;
-/*
-z_stage starts at ENTRY and than circles trought the other
-states until to rectangle is completely covered
-ENTRY : getting in the right position and height for the first flyover
-SEG   : fly from seg_start to seg_end and take pictures,
-        then calculate navigation points of next flyover
-TURN1 : do a 180° turn around seg_center1
-RET   : fly from ret_start to ret_end
-TURN2 : do a 180° turn around seg_center2
-*/
-
-/**
- initializes the variables needed for the survey to start
-   wp_center     :  the waypoint defining the center of the survey-rectangle
-   wp_dir        :  the waypoint defining the orientation of the survey-rectangle
-   sweep_length  :  the length of the survey-rectangle
-   sweep_spacing :  distance between the sweeps
-   sweep_lines   :  number of sweep_lines to fly
-   altitude      :  the altitude that must be reached before the flyover starts
-**/
+ * initializes the variables needed for the survey to start.
+ *
+ * @param center_wp     the waypoint defining the center of the survey-rectangle
+ * @param dir_wp        the waypoint defining the orientation of the survey-rectangle
+ * @param sweep_length  the length of the survey-rectangle
+ * @param sweep_spacing distance between the sweeps
+ * @param sweep_lines   number of sweep_lines to fly
+ * @param altitude      the altitude that must be reached before the flyover starts
+ */
 bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
 {
-  counter = 0;
+  zs.current_laps = 0;
+  zs.pre_leave_angle = 2;
+
   // copy variables from the flight plan
-  x_wp_center = waypoints[center_wp].x;
-  y_wp_center = waypoints[center_wp].y;
-  x_wp_dir = waypoints[dir_wp].x;
-  y_wp_dir = waypoints[dir_wp].y;
-  z_sweep_length = sweep_length;
-  z_sweep_spacing = sweep_spacing;
-  z_sweep_lines = sweep_lines;
-  //z_shot_dist = shot_dist;
-  z_altitude = altitude;
+  VECT2_COPY(zs.wp_center, waypoints[center_wp]);
+  VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
+  zs.altitude = altitude;
 
   // if turning right leave circle before angle is reached, if turning left - leave after
-  if (z_sweep_spacing>0) pre_leave_angle -= pre_leave_angle;
+  if (sweep_spacing > 0) {
+    zs.pre_leave_angle -= zs.pre_leave_angle;
+  }
+
+  struct FloatVect2 flight_vec;
+  VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
+  FLOAT_VECT2_NORMALIZE(flight_vec);
 
   // calculate the flight_angle
-  dx_flight_wpts = x_wp_dir - x_wp_center;
-  dy_flight_wpts = y_wp_dir - y_wp_center;
-  if (dy_flight_wpts == 0) dy_flight_wpts = 0.01; // to avoid dividing by zero
-  flight_angle = 180*atan2(dx_flight_wpts, dy_flight_wpts)/M_PI;
-  zamboni_return_angle = flight_angle + 180;
-  if (zamboni_return_angle > 359) zamboni_return_angle -= 360;
+  zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
+  zs.return_angle = zs.flight_angle + 180;
+  if (zs.return_angle > 359) {
+    zs.return_angle -= 360;
+  }
 
-  // calculate the flightline vector from start to end of one flightline, (delta x and delta y for one flightline)
+  // calculate the vector from one flightline perpendicular to the next flightline left,
+  // seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
   // (used later to move the flight pattern one flightline up for each round)
-  len = sqrtf(dx_flight_wpts * dx_flight_wpts + dy_flight_wpts * dy_flight_wpts);
-  dx_flight_vec = dx_flight_wpts/len;
-  dy_flight_vec = dy_flight_wpts/len;
-  dx_flightline = dx_flight_vec * z_sweep_length;
-  dy_flightline = dy_flight_vec * z_sweep_length;
-
-  // calculate the vector from one flightline perpendicular to the next flightline left, seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
-  // (used later to move the flight pattern one flightline up for each round)
-  dx_sweep_width = -(dy_flight_wpts/len) * z_sweep_spacing;
-  dy_sweep_width = +(dx_flight_wpts/len) * z_sweep_spacing;
+  zs.sweep_width.x = -flight_vec.y * sweep_spacing;
+  zs.sweep_width.y = +flight_vec.x * sweep_spacing;
 
   // calculate number of laps to fly and turning radius for each end
-  laps = (z_sweep_lines+1)/2;
-  turnradius1 = (laps-1) * z_sweep_spacing * 0.5;
-  turnradius2 = laps * z_sweep_spacing * 0.5;
+  zs.total_laps = (sweep_lines+1)/2;
+  zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
+  zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;
+
+  struct FloatVect2 flight_line;
+  VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
 
   //CALCULATE THE NAVIGATION POINTS
   //start and end of flight-line in flight-direction
-  x_seg_start = x_wp_center - dx_flightline * 0.5;
-  y_seg_start = y_wp_center - dy_flightline * 0.5;
-  x_seg_end = x_wp_center + dx_flightline * 0.5;
-  y_seg_end = y_wp_center + dy_flightline * 0.5;
+  VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
+  VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
 
+  struct FloatVect2 sweep_span;
+  VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
   //start and end of flight-line in return-direction
-  x_ret_start = x_seg_end - dx_sweep_width * (laps-1);
-  y_ret_start = y_seg_end - dy_sweep_width * (laps-1);
-  x_ret_end = x_seg_start - dx_sweep_width * (laps-1);
-  y_ret_end = y_seg_start - dy_sweep_width * (laps-1);
+  VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
+  VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
 
   //turn-centers at both ends
-  x_turn_center1 = (x_seg_end + x_ret_start)/2;
-  y_turn_center1 = (y_seg_end + y_ret_start)/2;
-  x_turn_center2 = (x_seg_start + x_ret_end + dx_sweep_width) / 2;
-  y_turn_center2 = (y_seg_start + y_ret_end + dy_sweep_width) / 2;
+  zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
+  zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
+  zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
+  zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;
 
   //fast climbing to desired altitude
   NavVerticalAutoThrottleMode(100.0);
-  NavVerticalAltitudeMode(z_altitude, 0.0);
+  NavVerticalAltitudeMode(zs.altitude, 0.0);
 
-  z_stage = Z_ENTRY;
+  zs.stage = Z_ENTRY;
 
   return FALSE;
 }
 
 /**
-   main navigation routine. This is called periodically evaluates the current
-   Position and stage and navigates accordingly.
-   Returns True until the survey is finished
-**/
+ * main navigation routine.
+ * This is called periodically evaluates the current
+ * Position and stage and navigates accordingly.
+ *
+ * @returns TRUE until the survey is finished
+ */
 bool_t zamboni_survey(void)
 {
   // retain altitude
   NavVerticalAutoThrottleMode(0.0);
-  NavVerticalAltitudeMode(z_altitude, 0.0);
+  NavVerticalAltitudeMode(zs.altitude, 0.0);
 
   //go from center of field to end of field - (before starting the syrvey)
-  if (z_stage == Z_ENTRY) {
-    nav_route_xy(x_wp_center, y_wp_center, x_seg_end, y_seg_end);
-    if (nav_approaching_xy(x_seg_end, y_seg_end, x_wp_center, y_wp_center, CARROT)) {
-      z_stage = Z_TURN1;
+  if (zs.stage == Z_ENTRY) {
+    nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y);
+    if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) {
+      zs.stage = Z_TURN1;
       NavVerticalAutoThrottleMode(0.0);
       nav_init_stage();
     }
   }
 
   //Turn from stage to return
-  else if (z_stage == Z_TURN1) {
-    nav_circle_XY(x_turn_center1, y_turn_center1, turnradius1);
-    if (NavCourseCloseTo(zamboni_return_angle+pre_leave_angle)){
-        // && nav_approaching_xy(x_seg_end, y_seg_end, x_seg_start, y_seg_start, CARROT
+  else if (zs.stage == Z_TURN1) {
+    nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1);
+    if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){
+      // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
       //calculate SEG-points for the next flyover
-      x_seg_start = x_seg_start + dx_sweep_width;
-      y_seg_start = y_seg_start + dy_sweep_width;
-      x_seg_end = x_seg_end + dx_sweep_width;
-      y_seg_end = y_seg_end + dy_sweep_width;
+      VECT2_ADD(zs.seg_start, zs.sweep_width);
+      VECT2_ADD(zs.seg_end, zs.sweep_width);
 
-      z_stage = Z_RET;
+      zs.stage = Z_RET;
       nav_init_stage();
-      #ifdef DIGITAL_CAM
-        //dc_survey(z_shot_dist, x_ret_start - dx_flight_vec * z_shot_dist, y_ret_start - dy_flight_vec * z_shot_dist);
-        LINE_START_FUNCTION;
-      #endif
+#ifdef DIGITAL_CAM
+      LINE_START_FUNCTION;
+#endif
     }
   }
 
   //fly the segment until seg_end is reached
-  else if (z_stage == Z_RET) {
-    nav_route_xy(x_ret_start, y_ret_start, x_ret_end, y_ret_end);
-    if (nav_approaching_xy(x_ret_end, y_ret_end, x_ret_start, y_ret_start, 0)) {
-      counter = counter + 1;
-      #ifdef DIGITAL_CAM
-        //dc_stop();
-        LINE_STOP_FUNCTION;
-      #endif
-      z_stage = Z_TURN2;
+  else if (zs.stage == Z_RET) {
+    nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y);
+    if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) {
+      zs.current_laps = zs.current_laps + 1;
+#ifdef DIGITAL_CAM
+      //dc_stop();
+      LINE_STOP_FUNCTION;
+#endif
+      zs.stage = Z_TURN2;
     }
   }
 
   //turn from stage to return
-  else if (z_stage == Z_TURN2) {
-    nav_circle_XY(x_turn_center2, y_turn_center2, turnradius2);
-    if (NavCourseCloseTo(flight_angle+pre_leave_angle)) {
-      //counter = counter + 1;
-      z_stage = Z_SEG;
+  else if (zs.stage == Z_TURN2) {
+    nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2);
+    if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) {
+      //zs.current_laps = zs.current_laps + 1;
+      zs.stage = Z_SEG;
       nav_init_stage();
-      #ifdef DIGITAL_CAM
-        //dc_survey(z_shot_dist, x_seg_start + dx_flight_vec * z_shot_dist, y_seg_start + dy_flight_vec * z_shot_dist);
-        LINE_START_FUNCTION;
-      #endif
+#ifdef DIGITAL_CAM
+      LINE_START_FUNCTION;
+#endif
     }
 
-  //return
-  } else if (z_stage == Z_SEG) {
-    nav_route_xy(x_seg_start, y_seg_start, x_seg_end, y_seg_end);
-    if (nav_approaching_xy(x_seg_end, y_seg_end, x_seg_start, y_seg_start, 0)) {
+    //return
+  } else if (zs.stage == Z_SEG) {
+    nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y);
+    if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) {
 
       // calculate the rest of the points for the next fly-over
-      x_ret_start = x_ret_start + dx_sweep_width;
-      y_ret_start = y_ret_start + dy_sweep_width;
-      x_ret_end = x_ret_end + dx_sweep_width;
-      y_ret_end = y_ret_end + dy_sweep_width;
-      x_turn_center1 = (x_seg_end + x_ret_start)/2;
-      y_turn_center1 = (y_seg_end + y_ret_start)/2;
-      x_turn_center2 = (x_seg_start + x_ret_end + dx_sweep_width) / 2;
-      y_turn_center2 = (y_seg_start + y_ret_end + dy_sweep_width) / 2;
+      VECT2_ADD(zs.ret_start, zs.sweep_width);
+      VECT2_ADD(zs.ret_end, zs.sweep_width);
+      zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2;
+      zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2;
+      zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2;
+      zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2;
 
-      z_stage = Z_TURN1;
+      zs.stage = Z_TURN1;
       nav_init_stage();
-      #ifdef DIGITAL_CAM
-        //dc_stop();
-        LINE_STOP_FUNCTION;
-      #endif
+#ifdef DIGITAL_CAM
+      //dc_stop();
+      LINE_STOP_FUNCTION;
+#endif
     }
   }
-  if (counter >= laps) {
-    #ifdef DIGITAL_CAM
+  if (zs.current_laps >= zs.total_laps) {
+#ifdef DIGITAL_CAM
     LINE_STOP_FUNCTION;
-    #endif
+#endif
     return FALSE;
   }
   else {
diff --git a/sw/airborne/subsystems/navigation/zamboni_survey.h b/sw/airborne/subsystems/navigation/zamboni_survey.h
index 607c36a2fa..54251c6303 100644
--- a/sw/airborne/subsystems/navigation/zamboni_survey.h
+++ b/sw/airborne/subsystems/navigation/zamboni_survey.h
@@ -1,13 +1,76 @@
-#ifndef ZAMBONI_H
-#define ZAMBONI_H
+/*
+ * Copyright (C) 2013 Jorn Anke, Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/navigation/zamboni_survey.h
+ *
+ * Zamboni pattern survey for fixedwings.
+ */
+
+#ifndef ZAMBONI_SURVEY_H
+#define ZAMBONI_SURVEY_H
 
 #include "std.h"
+#include "math/pprz_algebra_float.h"
 
-//typedef struct {float x; float y;} point2d;
 typedef enum {Z_ERR, Z_ENTRY, Z_SEG, Z_TURN1, Z_RET, Z_TURN2} z_survey_stage;
 
+struct ZamboniSurvey {
+  /* variables used to store values from the flight plan */
+  struct FloatVect2 wp_center;
+  struct FloatVect2 wp_dir;
+  struct FloatVect2 sweep_width;
+  float altitude;
+
+  /** in degrees. Leave turncircles a small angle before the 180deg turns are completed
+   * to get a smoother transition to flight-lines
+   */
+  int pre_leave_angle;
+  float flight_angle; ///< in degrees
+  float return_angle; ///< in degrees
+  int current_laps;
+  int total_laps;
+  float turnradius1;
+  float turnradius2;
+  struct FloatVect2 turn_center1;
+  struct FloatVect2 turn_center2;
+  struct FloatVect2 seg_start;
+  struct FloatVect2 seg_end;
+  struct FloatVect2 ret_start;
+  struct FloatVect2 ret_end;
+  /**
+   * z_stage starts at ENTRY and than circles trought the other
+   * states until to rectangle is completely covered
+   * ENTRY : getting in the right position and height for the first flyover
+   * SEG   : fly from seg_start to seg_end and take pictures,
+   * then calculate navigation points of next flyover
+   * TURN1 : do a 180° turn around seg_center1
+   * RET   : fly from ret_start to ret_end
+   * TURN2 : do a 180° turn around seg_center2
+   */
+  z_survey_stage stage;
+};
+
 
 extern bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude);
 extern bool_t zamboni_survey(void);
 
-#endif
+#endif //ZAMBONI_SURVEY_H