Re: [Paparazzi-devel] Interest for a new survey pattern? (Zamboni-survey)

[Jorn Anke]

Hi,

Here is the subroutine I came up with as an implementation of a zamboni survey pattern. The routine has only been tested in simulator, but it appears to be working as intended, except the yellow photo-shot dots seemes to appear somehow irregulary. I can however not figure out why.

For those who might be interested in testing the routine, the code and instructions are attached under.

The routine takes 7 parameters:

- center_wp, (wp defining the center of the area to be surveyed)
- dir_wp, (wp only used to define the flight direction, -from center_wp against dir_wp)
- sweep_length, (length of the flightlines)
- sweep_spacing, (spacing between flight lines)
- sweep_lines, (number of parallel flightlines,  must be an odd number, 5 or greater)
- shot_dist,
- altitude

Note that the width of the field to be surveyed is calculated from number of (sweep_lines-1) * sweep_spacing.

The turn-direction is by default to the right, seen in the flight direction, but the turn-direction/flight-pattern can be reversed/mirrored by assigning a negative value for sweep_spacing.

I should probably also be noted that this the first lines of code I have ever written in C -or for paparazzi, so the code may not follow the required programming standards. (If it by accident do, it's probably because I used the poly_survey_adv routine as a template). 
 

Cheers,
 
Jorn



As Chistopher explained to me in an earlier post to this thread, the routine has to be referenced for the linker, e.g. like this;
__________________________________________________________________________________________________________________

"You can add it for instance in /conf/firmwares/subsystems/fixedwing/navigation_extra.makefile or make a new subsystem makefile for just your files. Or alternatively add a <makefile> section in your airframe file containing ap.srcs += subsystems/navigation/zamboni_survey.c"
(It may also be neccessary to comment out the reference to the poly_survey_adv routine, since some of the parameter names might be conflicting)


Sample "header" and "block" from the flightplan:
__________________________________________________________________________________________________________________
<header>
#include "subsystems/navigation/zamboni_survey.h"
#include "subsystems/datalink/datalink.h"
/*
#ifndef VARDECLARED
#define VARDECLARED
float varsweepsize=110;
#endif
*/
#ifdef DC_AUTOSHOOT_METER_GRID
#include "modules/digital_cam/dc.h"
//#define LINE_START_FUNCTION dc_autoshoot = DC_AUTOSHOOT_PERIODIC;
#define LINE_START_FUNCTION dc_Survey(dc_gps_dist);
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
#endif
</header>

...

      <block name="ZamboniSurvey" strip_button="Zamboni 1" strip_icon="survey.png">
      <!-- init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float shot_dist, float altitude) /-->
      <call fun="init_zamboni_survey(WP_Z_CENTER, WP_Z_DIR, 300, -40, 7, 40, 250)"/>
      <call fun="zamboni_survey()"/>
    </block>
__________________________________________________________________________________________________________________



File to be saved in folder: sw/airborne/subsystems/navigation/ e.g. as "zamboni_survey.h"
__________________________________________________________________________________________________________________
#ifndef ZAMBONI_H
#define ZAMBONI_H

#include "std.h"

typedef struct {float x; float y;} point2d;
typedef enum {ERR, ENTRY, SEG, TURN1, RET, TURN2} survey_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 shot_dist, float altitude);
extern bool_t zamboni_survey(void);

#endif
__________________________________________________________________________________________________________________



File to be saved in folder: sw/airborne/subsystems/navigation/ e.g. as "zamboni_survey.c"
__________________________________________________________________________________________________________________
#include "zamboni_survey.h"

#include "subsystems/nav.h"
#include "estimator.h"
#include "autopilot.h"
#include "generated/flight_plan.h"

#ifdef DIGITAL_CAM
#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;

/**
static variables, used for initial calculations
**/
// properties for the filightpattern
float flight_angle, 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 or initial calculations 
  float dx_flight_wpts, dy_flight_wpts;
  float len;

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
   center_wp   :  the waypoint difining the center of the survey-rectangle
   sweep_length:  the length of the survey-rectangle
   angle       :  angle in which to do the flyovers. (0 = north, 90 = east, 180 = south, 270 = west)
   sweep_width :  distance between the sweeps
   shot_dist   :  distance between the shots
   min_rad     :  minimal radius when navigating
   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 shot_dist, float altitude)
{
  counter = 0;
  // 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;

  // 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;
  return_angle = flight_angle + 180;
  if (return_angle > 359) return_angle -= 360;
 
  // calculate the flightline vector from start to end of one flightline, (delta x and delta y for one flightline)
  // (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;
   
  // 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;
 
  //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;
 
  //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);

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

  //fast climbing to desired altitude
  NavVerticalAutoThrottleMode(100.0);
  NavVerticalAltitudeMode(z_altitude, 0.0);
 
  z_stage = 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
**/
bool_t zamboni_survey(void)
{
  // retain altitude
  NavVerticalAutoThrottleMode(0.0);
  NavVerticalAltitudeMode(z_altitude, 0.0);
 
  //entry circle around entry-center until the desired altitude is reached
  if (z_stage == 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 = TURN1;
      NavVerticalAutoThrottleMode(0.0);
      nav_init_stage();
    }
  }
 
  //Turn from stage to return
  else if (z_stage == TURN1) {
    nav_circle_XY(x_turn_center1, y_turn_center1, turnradius1);
    if (NavCourseCloseTo(return_angle)){
        // && nav_approaching_xy(x_seg_end, y_seg_end, x_seg_start, y_seg_start, 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;
     
      z_stage = 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);
      #endif
    }
  }
 
  //fly the segment until seg_end is reached
  else if (z_stage == 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, CARROT)) {
      #ifdef DIGITAL_CAM
        dc_stop();
      #endif
      z_stage = TURN2;  
    }
  }

  //turn from stage to return
  else if (z_stage == TURN2) {
    nav_circle_XY(x_turn_center2, y_turn_center2, turnradius2);
    if (NavCourseCloseTo(flight_angle)) {
      counter = counter + 1;
      z_stage = 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);
      #endif
    }
 
  //return
  } else if (z_stage == 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)) {
     
      // 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;
 
      z_stage = TURN1;
      nav_init_stage();
      #ifdef DIGITAL_CAM
        dc_stop();
      #endif
    }
  }
  if (counter >= laps) {
    return FALSE;
  }
  else {
    return TRUE;
  }
}
__________________________________________________________________________________________________________________