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

[Jorn Anke]

Hi,

Eventually I found some time to look at this survey pattern again. But, it turned out I was not any good in C-programming, so I started abusing the flight-plan instead. There is obviously not much math and stuff available, so I had to use some dirty tricks to obtain what I wanted, (using dummy waypoints for storing variables, pre-computing some variables, etc).

However, here is a first demo of a working flight-plan for a "zamboni" survey pattern.

Feel free to give it a try and to post comments.


Cheers,

Jorn     
---------------------------------------------------------------------

<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">

<flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
  <header>
#include "subsystems/navigation/nav_line.h"
#include "subsystems/datalink/datalink.h"
#include "modules/digital_cam/dc.h"
#define LINE_START_FUNCTION dc_Survey(dc_gps_dist);
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;

</header>

  <waypoints>

    <waypoint name="HOME" x="0" y="0"/>
    <waypoint name="STDBY" x="49.5" y="100.1"/>
    <waypoint name="1" x="10.1" y="189.9"/>
    <waypoint name="2" x="132.3" y="139.1"/>
    <waypoint name="MOB" x="137.0" y="-11.6"/>
    <waypoint name="S1" x="-119.2" y="69.6"/>
    <waypoint name="S2" x="274.4" y="209.5"/>
    <waypoint alt="30.0" name="AF" x="177.4" y="45.1"/>
    <waypoint alt="0.0" name="TD" x="28.8" y="57.0"/>
    <waypoint name="_BASELEG" x="168.8" y="-13.8"/>
    <waypoint name="CLIMB" x="-114.5" y="162.3"/>

    <!-- Waypoints and variables used by "Zamboni-survey". These are the only variables that has to be changed  /-->
   
    <!-- Startpoint for the survey-pattern (relative to home) /-->
    <waypoint name= "ENTER" x="250" y="20" />
    <waypoint name= "_L_S_FL" x="200" y="40" alt="9"/>
    <!-- x is used to store Lenght of the rectangle
         y is used to store Spacing between the flight lines
         alt is used to store number of parallell FlightLines. NB: must be a odd number, (e.g. 5, 7, 9...).
    /-->

    <!-- Precomputed values for rotating the flight-pattern against north. Un-comment one. /-->
    <!-- <waypoint name= "_ROT" x="0.0000" y="1.0000"/>        rotation 0 deg from north /-->
    <!-- <waypoint name= "_ROT" x="0.2588" y="0.9659"/>        /-->  <!-- 15 deg /-->
    <!-- <waypoint name= "_ROT" x="0.5000" y="0.8660"/>        /-->  <!-- 30 deg /-->
    <!-- <waypoint name= "_ROT" x="0.7071" y="0.7071"/>        /-->  <!-- 45 deg /-->
    <!-- <waypoint name= "_Rot" x="0.8660" y="0.5000"/>        /-->  <!-- 60 deg /-->
    <!-- <waypoint name= "_ROT" x="0.9659" y="0.2588"/>        /-->  <!-- 75 deg /-->
    <!-- <waypoint name= "_ROT" x="1.0000" y="0.0000"/>        /-->  <!-- 90 deg (go east) /-->
    <!-- <waypoint name= "_ROT" x="0.9659" y="-0.2588"/>       /-->  <!-- 105 deg /-->
    <!-- <waypoint name= "_ROT" x="0.8660" y="-0.5000"/>       /-->  <!-- 120 deg /-->
    <waypoint name= "_ROT" x="0.7071" y="-0.7071"/>       <!-- 135 deg /-->
    <!-- <waypoint name= "_ROT" x="0.5000" y="-0.8660"/>       /-->  <!-- 150 deg /-->
    <!-- <waypoint name= "_ROT" x="0.2588" y="-0.9659"/>       /-->  <!-- 165 deg /-->
    <!-- <waypoint name= "_ROT" x="0.0000" y="-1.0000"/>       /-->  <!-- 180 deg (go south) /-->
    <!-- <waypoint name= "_ROT" x="-0.2588" y="-0.9659"/>      /-->  <!-- 195 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.5000" y="-0.8660"/>      /-->  <!-- 210 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.7071" y="-0.7071"/>      /-->  <!-- 225 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.8660" y="-0.5000"/>      /-->  <!-- 240 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.9659" y="-0.2588"/>      /-->  <!-- 255 deg /-->
    <!-- <waypoint name= "_ROT" x="-1.0000" y="0.0000"/>       /-->  <!-- 270 deg (go west) /-->
    <!-- <waypoint name= "_ROT" x="-0.9659" y="0.2588"/>       /-->  <!-- 285 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.8660" y="0.5000"/>       /-->  <!-- 300 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.7071" y="0.7071"/>       /-->  <!-- 315 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.5000" y="0.8660"/>       /-->  <!-- 330 deg /-->
    <!-- <waypoint name= "_ROT" x="-0.2588" y="0.9659"/>       /-->  <!-- 345 deg /-->
 
    <!-- Waypoints, values are calculated later. WP's A-D are the corners of teh survey-rectangle, P1-P7 av navpoints /-->
    <waypoint name= "A" x="1" y="1"/>
    <waypoint name= "B" x="1" y="-1"/>
    <waypoint name= "C" x="-1" y="-1"/>
    <waypoint name= "D" x="-1" y="1"/>
    <waypoint name= "P1" x="0" y="0"/>
    <waypoint name= "P2" x="0" y="0"/>
    <waypoint name= "P3" x="0" y="0"/>
    <waypoint name= "P4" x="0" y="0"/>
    <waypoint name= "P5" x="0" y="0"/>
    <waypoint name= "P6" x="0" y="0"/>
    <waypoint name= "P7" x="0" y="0"/>

    <!-- Dummy waypoints, used to store variables, values are calculated later /-->
    <waypoint name= "_LS_DX_DY" x="0" y="0"/>
    <waypoint name= "_ROUNDS_RAB_RCD" x="0" y="0" alt="0"/>   
  </waypoints>

  <sectors>
    <sector name="ZamboniSector" color="blue">
      <corner name="A"/>
      <corner name="B"/>
      <corner name="C"/>
      <corner name="D"/>
    </sector>
  </sectors>

  <exceptions>
    <!--exception cond="datalink_time > 22" deroute="Standby"/-->
  </exceptions>
 
  <blocks>
    <block name="Zamboni-survey" strip_icon="survey.png" group="extra_pattern">
      <!-- Calculate some initial values /-->
      <!-- Delta distances betwen adjecent flightlines /-->
      <set var="WaypointX(WP__LS_DX_DY)" value="WaypointY(WP__ROT)*WaypointY(WP__L_S_FL)"/>
      <set var="WaypointY(WP__LS_DX_DY)" value="-WaypointX(WP__ROT)*WaypointY(WP__L_S_FL)"/>
      <set var="WaypointX(WP__ROUNDS_RAB_RCD)" value="-0.5+WaypointAlt(WP__L_S_FL)/2"/>
      <set var="WaypointY(WP__ROUNDS_RAB_RCD)"   value="WaypointY(WP__L_S_FL)*(WaypointX(WP__ROUNDS_RAB_RCD)-1)*0.5"/>
      <set var="WaypointAlt(WP__ROUNDS_RAB_RCD)" value="WaypointY(WP__L_S_FL)*WaypointX(WP__ROUNDS_RAB_RCD)*0.5"/>
      <!-- Sector corners /-->
      <set var="WaypointX(WP_A)" value="WaypointX(WP_ENTER)+WaypointX(WP__ROT)*WaypointX(WP__L_S_FL)*0.5+WaypointX(WP__LS_DX_DY)*WaypointX(WP__ROUNDS_RAB_RCD)"/>
      <set var="WaypointY(WP_A)" value="WaypointY(WP_ENTER)+WaypointY(WP__ROT)*WaypointX(WP__L_S_FL)*0.5+WaypointY(WP__LS_DX_DY)*WaypointX(WP__ROUNDS_RAB_RCD)"/>
      <set var="WaypointX(WP_B)" value="WaypointX(WP_ENTER)+WaypointX(WP__ROT)*WaypointX(WP__L_S_FL)*0.5-WaypointX(WP__LS_DX_DY)*(WaypointX(WP__ROUNDS_RAB_RCD)+2)"/>
      <set var="WaypointY(WP_B)" value="WaypointY(WP_ENTER)+WaypointY(WP__ROT)*WaypointX(WP__L_S_FL)*0.5-WaypointY(WP__LS_DX_DY)*(WaypointX(WP__ROUNDS_RAB_RCD)+2)"/>
      <set var="WaypointX(WP_C)" value="WaypointX(WP_ENTER)-WaypointX(WP__ROT)*WaypointX(WP__L_S_FL)*0.5-WaypointX(WP__LS_DX_DY)*(WaypointX(WP__ROUNDS_RAB_RCD)+2)"/>
      <set var="WaypointY(WP_C)" value="WaypointY(WP_ENTER)-WaypointY(WP__ROT)*WaypointX(WP__L_S_FL)*0.5-WaypointY(WP__LS_DX_DY)*(WaypointX(WP__ROUNDS_RAB_RCD)+2)"/>
      <set var="WaypointX(WP_D)" value="WaypointX(WP_ENTER)-WaypointX(WP__ROT)*WaypointX(WP__L_S_FL)*0.5+WaypointX(WP__LS_DX_DY)*WaypointX(WP__ROUNDS_RAB_RCD)"/>
      <set var="WaypointY(WP_D)" value="WaypointY(WP_ENTER)-WaypointY(WP__ROT)*WaypointX(WP__L_S_FL)*0.5+WaypointY(WP__LS_DX_DY)*WaypointX(WP__ROUNDS_RAB_RCD)"/>
      <!-- Waypoints for the survey-pattern /-->
      <set var="WaypointX(WP_P1)" value="WaypointX(WP_ENTER)+WaypointX(WP__ROT)*WaypointX(WP__L_S_FL)*0.5"/>
      <set var="WaypointY(WP_P1)" value="WaypointY(WP_ENTER)+WaypointY(WP__ROT)*WaypointX(WP__L_S_FL)*0.5"/>
      <set var="WaypointX(WP_P3)" value="WaypointX(WP_P1)+WaypointX(WP__LS_DX_DY)*(WaypointX(WP__ROUNDS_RAB_RCD)-1)"/>
      <set var="WaypointY(WP_P3)" value="WaypointY(WP_P1)+WaypointY(WP__LS_DX_DY)*(WaypointX(WP__ROUNDS_RAB_RCD)-1)"/>
      <set var="WaypointX(WP_P2)" value="(WaypointX(WP_P1)+WaypointX(WP_P3))/2"/>
      <set var="WaypointY(WP_P2)" value="(WaypointY(WP_P1)+WaypointY(WP_P3))/2"/>
      <set var="WaypointX(WP_P4)" value="WaypointX(WP_P3)-WaypointX(WP__ROT)*WaypointX(WP__L_S_FL)"/>
      <set var="WaypointY(WP_P4)" value="WaypointY(WP_P3)-WaypointY(WP__ROT)*WaypointX(WP__L_S_FL)"/>
      <set var="WaypointX(WP_P6)" value="WaypointX(WP_P4)-WaypointX(WP__LS_DX_DY)*WaypointX(WP__ROUNDS_RAB_RCD)"/>
      <set var="WaypointY(WP_P6)" value="WaypointY(WP_P4)-WaypointY(WP__LS_DX_DY)*WaypointX(WP__ROUNDS_RAB_RCD)"/>
      <set var="WaypointX(WP_P5)" value="(WaypointX(WP_P4)+WaypointX(WP_P6))/2"/>
      <set var="WaypointY(WP_P5)" value="(WaypointY(WP_P4)+WaypointY(WP_P6))/2"/>
      <set var="WaypointX(WP_P7)" value="WaypointX(WP_P6)+WaypointX(WP__ROT)*WaypointX(WP__L_S_FL)"/>
      <set var="WaypointY(WP_P7)" value="WaypointY(WP_P6)+WaypointY(WP__ROT)*WaypointX(WP__L_S_FL)"/>
 
      <go wp="ENTER"/>
      <go wp="P1"/>
      <circle radius="WaypointY(WP__ROUNDS_RAB_RCD)" until="NavCircleCount() > 0.48" wp="P2"/>
      <go from="P3" wp="P4" hmode="route"/>
      <circle radius="WaypointAlt(WP__ROUNDS_RAB_RCD)" until="NavCircleCount() > 0.48" wp="P5"/>
      <go from="P6" wp="P7" hmode="route"/>
     
      <for var="i" from="1" to="WaypointX(WP__ROUNDS_RAB_RCD)">
        <set var="WaypointX(WP_P2)" value="WaypointX(WP_P2)-WaypointX(WP__LS_DX_DY)"/>
        <set var="WaypointY(WP_P2)" value="WaypointY(WP_P2)-WaypointY(WP__LS_DX_DY)"/>
        <set var="WaypointX(WP_P3)" value="WaypointX(WP_P3)-WaypointX(WP__LS_DX_DY)"/>
        <set var="WaypointY(WP_P3)" value="WaypointY(WP_P3)-WaypointY(WP__LS_DX_DY)"/>
        <set var="WaypointX(WP_P4)" value="WaypointX(WP_P4)-WaypointX(WP__LS_DX_DY)"/>
        <set var="WaypointY(WP_P4)" value="WaypointY(WP_P4)-WaypointY(WP__LS_DX_DY)"/>
        <set var="WaypointX(WP_P5)" value="WaypointX(WP_P5)-WaypointX(WP__LS_DX_DY)"/>
        <set var="WaypointY(WP_P5)" value="WaypointY(WP_P5)-WaypointY(WP__LS_DX_DY)"/>
        <set var="WaypointX(WP_P6)" value="WaypointX(WP_P6)-WaypointX(WP__LS_DX_DY)"/>
        <set var="WaypointY(WP_P6)" value="WaypointY(WP_P6)-WaypointY(WP__LS_DX_DY)"/>
        <set var="WaypointX(WP_P7)" value="WaypointX(WP_P7)-WaypointX(WP__LS_DX_DY)"/>
        <set var="WaypointY(WP_P7)" value="WaypointY(WP_P7)-WaypointY(WP__LS_DX_DY)"/>

        <circle radius="WaypointY(WP__ROUNDS_RAB_RCD)" until="NavCircleCount() > 0.48" wp="P2"/>
        <go from="P3" wp="P4" hmode="route"/>
        <circle radius="WaypointAlt(WP__ROUNDS_RAB_RCD)" until="NavCircleCount() > 0.48" wp="P5"/>
        <go from="P6" wp="P7" hmode="route"/>
  
      </for>
      <deroute block="Standby"/>
    </block>


    <block name="Wait GPS">
      <set value="1" var="kill_throttle"/>
      <while cond="!GpsFixValid()"/>
    </block>
    <block name="Geo init">
      <while cond="LessThan(NavBlockTime(), 10)"/>
      <call fun="NavSetGroundReferenceHere()"/>
    </block>
    <block name="Holding point">
      <!--set var="nav_mode" value="NAV_MODE_ROLL"/-->
      <set value="1" var="kill_throttle"/>
      <attitude roll="0" throttle="0" vmode="throttle"/>
    </block>
    <block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
      <set value="0" var="kill_throttle"/>
      <set value="0" var="estimator_flight_time"/>
      <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
    </block>
    <block key="<Control>a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
      <circle radius="nav_radius" wp="STDBY"/>
    </block>
    <block key="F8" name="Figure 8 around wp 1" strip_button="Figure 8 (wp 1-2)" strip_icon="eight.png" group="base_pattern">
      <eight center="1" radius="nav_radius" turn_around="2"/>
    </block>
   
    <block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png" group="land">
      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
      <deroute block="land"/>
    </block>
    <block name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png" group="land">
      <set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
      <deroute block="land"/>
    </block>
    <block name="land">
      <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
      <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(estimator_z - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
    </block>
    <block name="final">
      <exception cond="ground_alt + 10 > estimator_z" deroute="flare"/>
      <go from="AF" hmode="route" vmode="glide" wp="TD"/>
    </block>
    <block name="flare">
      <go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
      <attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
    </block>
  </blocks>
</flight_plan>