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

We had really nice weather conditions today, so I decided to go test-fly the zamboni flight-pattern. As far as I could tell from looking at the plane from the ground, the flight went even better the I would dear to hope. In particular because this was also the maiden flight for me with paparazzi.

The only thing which did not work as expected (I think) was the phototriggering - from the yellow dots appearing in the GCS . I may have misunderstood how to interact with the distance-based trigger, and will have a closer look into that.

The flight-log and a kml-file from the flight is attached. Note that there are actually two zamboni missions in the log - where the last mission was flown in reversed direction - on top of the first mission. The whole flight was in Auto2, except the final part of the landing.

I am still no good in analyzing log-files, so If anyone would have a look, and comment of anything strange or worth improving, I would be really grateful.

Cheers,

Jørn

From: address@hidden
To: address@hidden
Subject: RE: Interest for a new survey pattern? (Zamboni-survey)
Date: Wed, 9 Jan 2013 22:39:32 +0100

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">
      
      <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;
}
}
__________________________________________________________________________________________________________________

From:	Jorn Anke
Subject:	Re: [Paparazzi-devel] Interest for a new survey pattern? (Zamboni-survey)
Date:	Sat, 12 Jan 2013 19:11:32 +0100