mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[conf] clean old flight plans
- remove some really old examples - move previous competitions' FP to a sub-directory
This commit is contained in:
Gautier Hattenberger
@@ -1,25 +0,0 @@
|
||||
<!DOCTYPE procedure SYSTEM "flight_plan.dtd">
|
||||
|
||||
<procedure>
|
||||
<param name="rad" default_value="30"/>
|
||||
<param name="alt" default_value="GROUND_ALT+50"/>
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="center" x="0" y="0"/>
|
||||
</waypoints>
|
||||
|
||||
<blocks>
|
||||
<block NAME="go">
|
||||
<exception COND="(RcEvent1())" DEROUTE="event1"/>
|
||||
<circle WP="center" RADIUS="2*rad" alt="alt" until="And(Qdr(0), circle_count > 1)"/>
|
||||
<circle wp="center" wp_qdr="0" wp_dist="rad" RADIUS="rad" alt="alt" until="circle_count > 0.5"/>
|
||||
<circle WP="center" wp_qdr="180" wp_dist="rad" RADIUS="-rad" alt="alt" until="circle_count > 1"/>
|
||||
<circle WP="center" wp_qdr="0" wp_dist="rad" RADIUS="rad" alt="alt" until="circle_count > 0.5"/>
|
||||
<circle WP="center" RADIUS="2*rad" alt="alt" until="Qdr(90)"/>
|
||||
<circle WP="center" wp_qdr="90" wp_dist="rad" RADIUS="rad" alt="alt" until="circle_count > 0.5"/>
|
||||
<circle WP="center" wp_qdr="270" wp_dist="rad" RADIUS="-rad" alt="alt" until="circle_count > 1"/>
|
||||
<circle WP="center" wp_qdr="90" wp_dist="rad" RADIUS="rad" alt="alt" until="circle_count > 0.5"/>
|
||||
<deroute block="go"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</procedure>
|
||||
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="140" ground_alt="55" lat0="52.523886" lon0="10.463764" max_dist_from_home="2500" name="Gifhorn-Wilsche EMAV08" qfu="-90" security_height="81">
|
||||
<header>
|
||||
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="100" ground_alt="54" lat0="52.3825204" lon0="5.897420" max_dist_from_home="1500" name="EMAV09" security_height="25">
|
||||
<header>
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE procedure SYSTEM "flight_plan.dtd">
|
||||
<!DOCTYPE procedure SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<procedure>
|
||||
<waypoints>
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE procedure SYSTEM "flight_plan.dtd">
|
||||
<!DOCTYPE procedure SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<procedure>
|
||||
<exceptions>
|
||||
@@ -0,0 +1,138 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="150" ground_alt="46" home_mode_height="50" lat0="52.142596" lon0="5.840263" max_dist_from_home="250" name="IMAV2014 Carto" qfu="180." security_height="25">
|
||||
<!--flight_plan alt="285" ground_alt="185" lat0="43.463040" lon0="1.273303" max_dist_from_home="1000" name="IMAV2014 Carto Muret" security_height="25" home_mode_height="50"-->
|
||||
<header>
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y) {
|
||||
float dx = wp1_x - wp2_x;
|
||||
float dy = wp1_y - wp2_y;
|
||||
return sqrtf(dx*dx + dy*dy);
|
||||
}
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" lat="52.142983" lon="5.842166"/>
|
||||
<waypoint name="STDBY" lat="52.143239" lon="5.840320"/>
|
||||
<waypoint height="30.0" name="AF" lat="52.143978" lon="5.840164"/>
|
||||
<waypoint height="0.0" name="TD" lat="52.143185" lon="5.840102"/>
|
||||
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
|
||||
<waypoint name="CLIMB" x="100" y="-160.0"/>
|
||||
<waypoint name="N1" lat="52.143245" lon="5.840738"/>
|
||||
<waypoint name="N2" lat="52.143398" lon="5.841467"/>
|
||||
<waypoint name="N3" lat="52.143562" lon="5.842216"/>
|
||||
<waypoint name="N4" lat="52.143709" lon="5.842983"/>
|
||||
<waypoint name="S1" lat="52.141731" lon="5.841610"/>
|
||||
<waypoint name="S2" lat="52.141883" lon="5.842377"/>
|
||||
<waypoint name="S3" lat="52.142052" lon="5.843110"/>
|
||||
<waypoint name="S4" lat="52.142215" lon="5.843841"/>
|
||||
<waypoint name="_C1" lat="52.143043" lon="5.839784"/>
|
||||
<waypoint name="_C2" lat="52.143899" lon="5.843859"/>
|
||||
<waypoint name="_C3" lat="52.142386" lon="5.844636"/>
|
||||
<waypoint name="_C4" lat="52.141529" lon="5.840656"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="lime" name="Carto">
|
||||
<corner name="_C1"/>
|
||||
<corner name="_C2"/>
|
||||
<corner name="_C3"/>
|
||||
<corner name="_C4"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<includes>
|
||||
<include name="Data" procedure="IMAV2014_data.xml"/>
|
||||
</includes>
|
||||
<exceptions>
|
||||
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 150)" deroute="Center"/>
|
||||
</exceptions>
|
||||
<blocks>
|
||||
<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 value="1" var="kill_throttle"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Set Low Alt">
|
||||
<set value="100." var="WaypointAlt(WP_STDBY)"/>
|
||||
</block>
|
||||
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
|
||||
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Carto1"/>
|
||||
<call fun="{ hackhd_command(HACKHD_POWER_ON); FALSE; }"/>
|
||||
<while cond="LessThan(NavBlockTime(), 4)"/>
|
||||
<call fun="{ hackhd_command(HACKHD_SHOOT); FALSE; }"/>
|
||||
<set value="0" var="kill_throttle"/>
|
||||
<set value="0" var="autopilot_flight_time"/>
|
||||
<go from="HOME" pitch="30" throttle="1.0" vmode="throttle" wp="CLIMB"/>
|
||||
</block>
|
||||
<block group="home" key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<circle radius="nav_radius" wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Carto1" strip_button="Carto" strip_icon="survey.png">
|
||||
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<call fun="nav_compute_baseleg(WP_N3, WP_S3, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 15 > fabs(GetPosAlt() - WaypointAlt(WP_STDBY)))" wp="_BASELEG" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<attitude alt="WaypointAlt(WP_STDBY)" roll="0.0" until="stage_time>=2"/>
|
||||
<set value="MODULES_START" var="hackhd_hackhd_autoshoot_status"/>
|
||||
<go approaching_time="0" from="N3" hmode="route" wp="S3" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<set value="MODULES_STOP" var="hackhd_hackhd_autoshoot_status"/>
|
||||
</block>
|
||||
<block name="Carto2">
|
||||
<set value="wp_dist(WaypointX(WP_S1), WaypointY(WP_S1), WaypointX(WP_S3), WaypointY(WP_S3))/2" var="nav_radius"/>
|
||||
<call fun="nav_compute_baseleg(WP_S1, WP_N1, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10)" wp="_BASELEG" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<attitude alt="WaypointAlt(WP_STDBY)" roll="0.0" until="stage_time>=2"/>
|
||||
<set value="MODULES_START" var="hackhd_hackhd_autoshoot_status"/>
|
||||
<go approaching_time="0" from="S1" hmode="route" wp="N1" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<set value="MODULES_STOP" var="hackhd_hackhd_autoshoot_status"/>
|
||||
</block>
|
||||
<block name="Carto3">
|
||||
<set value="wp_dist(WaypointX(WP_N1), WaypointY(WP_N1), WaypointX(WP_N4), WaypointY(WP_N4))/2" var="nav_radius"/>
|
||||
<call fun="nav_compute_baseleg(WP_N4, WP_S4, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10)" wp="_BASELEG" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<attitude alt="WaypointAlt(WP_STDBY)" roll="0.0" until="stage_time>=2"/>
|
||||
<set value="MODULES_START" var="hackhd_hackhd_autoshoot_status"/>
|
||||
<go approaching_time="0" from="N4" hmode="route" wp="S4" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<set value="MODULES_STOP" var="hackhd_hackhd_autoshoot_status"/>
|
||||
</block>
|
||||
<block name="Carto4">
|
||||
<set value="wp_dist(WaypointX(WP_S2), WaypointY(WP_S2), WaypointX(WP_S4), WaypointY(WP_S4))/2" var="nav_radius"/>
|
||||
<call fun="nav_compute_baseleg(WP_S2, WP_N2, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10)" wp="_BASELEG" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<attitude alt="WaypointAlt(WP_STDBY)" roll="0.0" until="stage_time>=2"/>
|
||||
<set value="MODULES_START" var="hackhd_hackhd_autoshoot_status"/>
|
||||
<go approaching_time="0" from="S2" hmode="route" wp="N2" alt="WaypointAlt(WP_STDBY)"/>
|
||||
<set value="MODULES_STOP" var="hackhd_hackhd_autoshoot_status"/>
|
||||
<deroute block="Land Left AF-TD"/>
|
||||
</block>
|
||||
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block group="land" name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png">
|
||||
<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(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
|
||||
</block>
|
||||
<block name="final">
|
||||
<exception cond="GetAltRef() + 3 > GetPosAlt()" deroute="flare"/>
|
||||
<call fun="{ hackhd_command(HACKHD_POWER_OFF); FALSE; }"/>
|
||||
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<go exceeding_time="5" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
|
||||
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Center" pre_call="if (InsideRed(GetPosX(), GetPosY())) NavKillThrottle();">
|
||||
<circle radius="DEFAULT_CIRCLE_RADIUS" wp="HOME"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,33 @@
|
||||
<!DOCTYPE procedure SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<procedure>
|
||||
<waypoints>
|
||||
<!--waypoint name="C" lat="52.142983" lon="5.842166"/-->
|
||||
<!--waypoint name="_G1" lat="52.143090" lon="5.839932"/>
|
||||
<waypoint name="_G2" lat="52.143015" lon="5.838518"/>
|
||||
<waypoint name="_G3" lat="52.141036" lon="5.840327"/-->
|
||||
<waypoint name="_R1" lat="52.143034" lon="5.839884"/>
|
||||
<waypoint name="_R2" lat="52.142826" lon="5.836271"/>
|
||||
<waypoint name="_R3" lat="52.141897" lon="5.837266"/>
|
||||
<waypoint name="_R4" lat="52.142711" lon="5.839948"/>
|
||||
<!--waypoint name="_C1" x="-17.7" y="66.1"/>
|
||||
<waypoint name="_C2" x="268.4" y="163.1"/>
|
||||
<waypoint name="_C3" x="327.8" y="-7.0"/>
|
||||
<waypoint name="_C4" x="63.9" y="-104.9"/-->
|
||||
</waypoints>
|
||||
|
||||
<sectors>
|
||||
<sector name="Red" color="red">
|
||||
<corner name="_R1"/>
|
||||
<corner name="_R2"/>
|
||||
<corner name="_R3"/>
|
||||
<corner name="_R4"/>
|
||||
</sector>
|
||||
<!--sector name="Green" color="lime">
|
||||
<corner name="_G1"/>
|
||||
<corner name="_G2"/>
|
||||
<corner name="_G3"/>
|
||||
<corner name="_G4"/>
|
||||
</sector-->
|
||||
</sectors>
|
||||
</procedure>
|
||||
@@ -0,0 +1,120 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="61" ground_alt="46" home_mode_height="10" lat0="52.142596" lon0="5.840263" max_dist_from_home="250" name="IMAV2014 Digit" security_height="5">
|
||||
<header>
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y) {
|
||||
float dx = wp1_x - wp2_x;
|
||||
float dy = wp1_y - wp2_y;
|
||||
return sqrtf(dx*dx + dy*dy);
|
||||
}
|
||||
#define WPAlt(_wp) waypoints[_wp].z
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint lat="52.142983" lon="5.842166" name="HOME"/>
|
||||
<waypoint name="STDBY" x="10.6" y="-6.7"/>
|
||||
<waypoint name="TD" lat="52.143185" lon="5.840102"/>
|
||||
<waypoint name="CLIMB" x="0.5" y="6.6"/>
|
||||
<waypoint name="OBS1" x="172.0" y="24.5"/>
|
||||
<waypoint name="OBS2" x="172.5" y="29.2"/>
|
||||
<waypoint name="CAM" x="183.1" y="25.3"/>
|
||||
<waypoint name="ROOF" x="157.7" y="31.1"/>
|
||||
<waypoint name="HERE" x="8.1" y="-13.9"/>
|
||||
</waypoints>
|
||||
<includes>
|
||||
<include name="Data" procedure="IMAV2014_data.xml"/>
|
||||
</includes>
|
||||
<exceptions>
|
||||
<exception cond="(autopilot_flight_time > (60*8)) && (IndexOfBlock('Back Home') >= nav_block) && !(nav_block == IndexOfBlock('Holding point'))" deroute="Back Home"/>
|
||||
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 150)" deroute="Center"/>
|
||||
</exceptions>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
|
||||
</block>
|
||||
<block name="Start Engine">
|
||||
<call fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
|
||||
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
|
||||
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
|
||||
<call fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="stage_time>3 && autopilot_motors_on" vmode="throttle"/>
|
||||
<stay vmode="climb" climb="0.5" wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Observe 1" strip_button="Obs1" pre_call="nav_set_heading_towards_waypoint(WP_CAM)" group="obs1">
|
||||
<go from="STDBY" hmode="route" wp="OBS1"/>
|
||||
<set value="MODULES_START" var="ImageCapture_image_capture_run_status"/>
|
||||
<stay wp="OBS1" height="5.0" until="stage_time>(5*60)"/>
|
||||
<set value="MODULES_STOP" var="ImageCapture_image_capture_run_status"/>
|
||||
<deroute block="Back from Obs1"/>
|
||||
</block>
|
||||
<block name="Back from Obs1" strip_button="Back1" group="obs1">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_TD)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<path wpts="HERE,STDBY,TD"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="Observe 2" strip_button="Obs2" pre_call="nav_set_heading_towards_waypoint(WP_CAM)" group="obs2">
|
||||
<go from="STDBY" hmode="route" wp="OBS2"/>
|
||||
<set value="MODULES_START" var="ImageCapture_image_capture_run_status"/>
|
||||
<stay wp="OBS2" height="5.0" until="stage_time>(5*60)"/>
|
||||
<set value="MODULES_STOP" var="ImageCapture_image_capture_run_status"/>
|
||||
<deroute block="Back from Obs2"/>
|
||||
</block>
|
||||
<block name="Back from Obs2" strip_button="Back2" group="obs2">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_TD)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<path wpts="HERE,STDBY,TD"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="Roof" strip_button="Roof" group="roof">
|
||||
<go from="STDBY" hmode="route" wp="ROOF"/>
|
||||
<stay climb="-0.8" vmode="climb" wp="ROOF" until="0.2 > agl_dist_value_filtered"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time > 15"/>
|
||||
<stay climb="0.8" vmode="climb" wp="ROOF" until="agl_dist_value_filtered > 1.0"/>
|
||||
</block>
|
||||
<block name="Back from Roof" strip_button="BackRoof" group="roof">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_TD)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<path wpts="HERE,STDBY,TD"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="Back Home" strip_button="Back" group="house2">
|
||||
<set value="MODULES_STOP" var="ImageCapture_image_capture_run_status"/>
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_TD)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_TD)"/>
|
||||
<path wpts="HERE,STDBY,TD"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<go wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<exception cond="NavDetectGround()" deroute="Holding point"/>
|
||||
<exception cond="0.1 > agl_dist_value_filtered" deroute="Holding point"/>
|
||||
<exception cond="!nav_is_in_flight()" deroute="landed"/>
|
||||
<call fun="NavStartDetectGround()"/>
|
||||
<stay climb="-0.8" vmode="climb" wp="TD"/>
|
||||
</block>
|
||||
<block name="landed">
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
|
||||
</block>
|
||||
<block name="Center">
|
||||
<stay wp="HOME" climb="-1.0"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,163 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="58" ground_alt="46" home_mode_height="10" lat0="52.142596" lon0="5.840263" max_dist_from_home="250" name="IMAV2014 Digit" security_height="5">
|
||||
<header>
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y) {
|
||||
float dx = wp1_x - wp2_x;
|
||||
float dy = wp1_y - wp2_y;
|
||||
return sqrtf(dx*dx + dy*dy);
|
||||
}
|
||||
#define WPAlt(_wp) waypoints[_wp].z
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint lat="52.142983" lon="5.842166" name="HOME"/>
|
||||
<waypoint name="STDBY" x="-1.6" y="-5.9"/>
|
||||
<waypoint name="TD" lat="52.143185" lon="5.840102"/>
|
||||
<waypoint name="CLIMB" x="0.5" y="6.6"/>
|
||||
<waypoint name="CAM" x="76.7" y="3.8"/>
|
||||
<waypoint name="ROOF" x="157.7" y="31.1"/>
|
||||
<waypoint name="H1" x="29.6" y="-41.8"/>
|
||||
<waypoint name="H2" x="14.1" y="-9.0"/>
|
||||
<waypoint name="H3" x="45.1" y="30.2"/>
|
||||
<waypoint name="H4" x="71.6" y="28.5"/>
|
||||
<waypoint name="H5" x="94.7" y="67.5"/>
|
||||
<waypoint name="H6" x="182.1" y="89.5"/>
|
||||
<waypoint name="HERE" x="8.1" y="-13.9"/>
|
||||
</waypoints>
|
||||
<includes>
|
||||
<include name="Data" procedure="IMAV2014_data.xml"/>
|
||||
</includes>
|
||||
<exceptions>
|
||||
<exception cond="(autopilot_flight_time > (60*8)) && (IndexOfBlock('Back Home') >= nav_block) && !(nav_block == IndexOfBlock('Holding point'))" deroute="Back Home"/>
|
||||
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 150)" deroute="Center"/>
|
||||
</exceptions>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Start Engine">
|
||||
<call fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
|
||||
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
|
||||
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
|
||||
<call fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="stage_time>3 && autopilot_motors_on" vmode="throttle"/>
|
||||
<stay climb="0.5" vmode="climb" wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
<block name="House 1" strip_button="H1" group="house1">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_H1)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_H1)"/>
|
||||
<go from="HERE" hmode="route" wp="H1"/>
|
||||
<call fun="nav_set_heading_deg(310)"/>
|
||||
<stay wp="H1" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
|
||||
<call fun="viewvideo_save_shot()"/>
|
||||
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
|
||||
<stay wp="H1" until="stage_time > 2"/-->
|
||||
<deroute block="House 2"/>
|
||||
</block>
|
||||
<block name="House 2" strip_button="H2" group="house1">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_H2)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_H2)"/>
|
||||
<go from="HERE" hmode="route" wp="H2"/>
|
||||
<call fun="nav_set_heading_deg(90)"/>
|
||||
<stay wp="H2" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
|
||||
<call fun="viewvideo_save_shot()"/>
|
||||
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
|
||||
<stay wp="H2" until="stage_time > 2"/-->
|
||||
<deroute block="House 3"/>
|
||||
</block>
|
||||
<block name="House 3" strip_button="H3" group="house1">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_H3)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_H3)"/>
|
||||
<go from="HERE" hmode="route" wp="H3"/>
|
||||
<call fun="nav_set_heading_deg(130)"/>
|
||||
<stay wp="H3" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
|
||||
<call fun="viewvideo_save_shot()"/>
|
||||
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
|
||||
<stay wp="H3" until="stage_time > 2"/-->
|
||||
<deroute block="House 4"/>
|
||||
</block>
|
||||
<block name="House 4" strip_button="H4" group="house2">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_H4)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_H4)"/>
|
||||
<go from="HERE" hmode="route" wp="H4"/>
|
||||
<call fun="nav_set_heading_deg(80)"/>
|
||||
<stay wp="H4" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
|
||||
<call fun="viewvideo_save_shot()"/>
|
||||
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
|
||||
<stay wp="H4" until="stage_time > 20"/-->
|
||||
<deroute block="Back Home"/>
|
||||
</block>
|
||||
<block name="House 5" strip_button="H5" group="house2">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_H5)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_H5)"/>
|
||||
<go from="HERE" hmode="route" wp="H5"/>
|
||||
<call fun="nav_set_heading_deg(140)"/>
|
||||
<stay wp="H5" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
|
||||
<call fun="viewvideo_save_shot()"/>
|
||||
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
|
||||
<stay wp="H5" until="stage_time > 20"/-->
|
||||
<deroute block="House 6"/>
|
||||
</block>
|
||||
<block name="House 6" strip_button="H6" group="house2">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_H6)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_H6)"/>
|
||||
<go from="HERE" hmode="route" wp="H6"/>
|
||||
<call fun="nav_set_heading_deg(180)"/>
|
||||
<stay wp="H6" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
|
||||
<call fun="viewvideo_save_shot()"/>
|
||||
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
|
||||
<stay wp="H5" until="stage_time > 20"/-->
|
||||
<deroute block="Back Home"/>
|
||||
</block>
|
||||
<block group="roof" name="Roof" strip_button="Roof">
|
||||
<go from="STDBY" hmode="route" wp="ROOF"/>
|
||||
<stay climb="-0.8" until="0.2 > agl_dist_value_filtered" vmode="climb" wp="ROOF"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="stage_time > 15" vmode="throttle"/>
|
||||
<stay climb="0.8" until="agl_dist_value_filtered > 1.0" vmode="climb" wp="ROOF"/>
|
||||
</block>
|
||||
<block name="Back Home" strip_button="Back Home" group="roof">
|
||||
<set var="nav_flight_altitude" value="WPAlt(WP_TD)"/>
|
||||
<call fun="NavSetWaypointHere(WP_HERE)"/>
|
||||
<call fun="nav_set_heading_towards_waypoint(WP_TD)"/>
|
||||
<path wpts="HERE,H3,TD"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<go wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<exception cond="NavDetectGround()" deroute="Holding point"/>
|
||||
<exception cond="0.1 > agl_dist_value_filtered" deroute="Holding point"/>
|
||||
<exception cond="!nav_is_in_flight()" deroute="landed"/>
|
||||
<call fun="NavStartDetectGround()"/>
|
||||
<stay climb="-0.8" vmode="climb" wp="TD"/>
|
||||
</block>
|
||||
<block name="landed">
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Center">
|
||||
<stay climb="-1.0" wp="HOME"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan SECURITY_HEIGHT="25" lat0="47.4820" lon0="11.0945" ground_alt="730" qfu="280" alt="780" max_dist_from_home="300" name="Garmisch">
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="225" ground_alt="175" lat0="43.53900" lon0="1.24500" max_dist_from_home="1500" name="MAV07" qfu="0" security_height="25">
|
||||
<header>
|
||||
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="240" ground_alt="170" lat0="27.0936" lon0="77.9145" max_dist_from_home="650" name="MAV08" security_height="25">
|
||||
<header>
|
||||
@@ -1,33 +0,0 @@
|
||||
<!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/datalink/datalink.h"
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint name="STDBY" x="25.1" y="102.1"/>
|
||||
<waypoint name="1" x="115.4" y="191.1"/>
|
||||
<waypoint name="3" x="332.7" y="100.6"/>
|
||||
<waypoint name="2" x="107.5" y="16.3"/>
|
||||
<waypoint name="CLIMB" x="-67.3" y="98.8"/>
|
||||
</waypoints>
|
||||
<exceptions/>
|
||||
<blocks>
|
||||
<block name="Geo init">
|
||||
<call fun="NavSetGroundReferenceHere()"/>
|
||||
</block>
|
||||
<block name="Takeoff">
|
||||
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="HSIF"/>
|
||||
<set value="0" var="kill_throttle"/>
|
||||
<set value="0" var="autopilot_flight_time"/>
|
||||
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="HSIF">
|
||||
<circle radius="200" until="GetPosX() > 200" wp="2"/>
|
||||
<circle radius="-75" until="NavQdrCloseTo(330)" wp="3"/>
|
||||
<circle radius="200" until="0 > GetPosX()" wp="1"/>
|
||||
<circle radius="35" wp="STDBY"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -1,32 +0,0 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
|
||||
<flight_plan ground_alt="200" alt="250" lat0="43.4624" lon0="1.2727" max_dist_from_home="5000" name="FOOBAR" qfu="270" security_height="25">
|
||||
<waypoints>
|
||||
<waypoint name="1" x="100" y="0"/>
|
||||
<waypoint name="2" x="200" y="0"/>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
</waypoints>
|
||||
|
||||
<blocks>
|
||||
<!-- Hard coded order: DO NOT CHANGE (cf ihm.ml) -->
|
||||
<block name="nop">
|
||||
<circle radius="75" wp="HOME"/>
|
||||
</block>
|
||||
<block name="circle">
|
||||
<circle radius="nav_radius" wp="1"/>
|
||||
</block>
|
||||
|
||||
<block name="8">
|
||||
<eight radius="nav_radius" center="1" turn_around="2"/>
|
||||
</block>
|
||||
|
||||
<block name="glide12">
|
||||
<go from="1" hmode="route" vmode="glide" wp="2"/>
|
||||
</block>
|
||||
|
||||
<block name="end_glide">
|
||||
<circle wp="2" radius="75"/>
|
||||
</block>
|
||||
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -1,15 +0,0 @@
|
||||
<!-- DOCTYPE flight_plan SYSTEM "flight_plan.dtd" -->
|
||||
|
||||
<flight_plan NAME="Muret mini" LON0="1.27289" MAX_DIST_FROM_HOME="300" GROUND_ALT="185" SECURITY_HEIGHT="25" QFU="270" ALT="250" LAT0="43.46223">
|
||||
|
||||
<waypoints utm_x0="360284.8" utm_y0="4813595.5">
|
||||
<waypoint name="HOME" x="0.0" y="120.0" alt="250."/>
|
||||
</waypoints>
|
||||
|
||||
<blocks>
|
||||
<block NAME="home">
|
||||
<circle wp="HOME" alt="GROUND_ALT+50" radius="-75"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
|
||||
@@ -1,18 +0,0 @@
|
||||
<flight_plan SECURITY_HEIGHT="25" lat0="42.799347" lon0="1.665094" ground_alt="507" qfu="270" alt="550" max_dist_from_home="500" name="Sinsat">
|
||||
|
||||
<waypoints>
|
||||
<waypoint Y="0" NAME="HOME" X="0"/>
|
||||
</waypoints>
|
||||
<blocks>
|
||||
<block NAME="init">
|
||||
<while COND="(!launch)"/>
|
||||
<heading THROTTLE="0.8" PITCH="0.15" COURSE="QFU" UNTIL="(autopilot_flight_time > 8)" VMODE="throttle"/>
|
||||
<heading PITCH="0.15" CLIMB="3.0" COURSE="QFU" UNTIL="(GetPosAlt() > SECURITY_ALT)" VMODE="climb"/>
|
||||
<deroute BLOCK="circlehome"/>
|
||||
</block>
|
||||
<block NAME="circlehome">
|
||||
<circle WP="HOME" ALT="GROUND_ALT+50" RADIUS="75"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
|
||||
@@ -1,48 +0,0 @@
|
||||
<flight_plan name="booz test 1" lon0="-118.860691" lat0="34.233845" ground_alt="328" alt="330" max_dist_from_home="100" security_height="1">
|
||||
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="TARGET" x="10" y="0"/>
|
||||
</waypoints>
|
||||
|
||||
<blocks>
|
||||
<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 value="1" var="kill_throttle"/>
|
||||
<stay wp="HOME"/>
|
||||
</block>
|
||||
<block name="vertical steps" strip_button="Vert. Steps">
|
||||
<while cond="TRUE">
|
||||
<set var="tl_control_agl_sp" value="-3"/>
|
||||
<while cond="15 > stage_time"/>
|
||||
<set var="tl_control_agl_sp" value="-5"/>
|
||||
<while cond="15 > stage_time"/>
|
||||
</while>
|
||||
</block>
|
||||
<block name="vertical RC" strip_button="Vert. RC">
|
||||
<while cond="TRUE">
|
||||
<set var="tl_control_agl_sp" value=" -5. * rc_values[RADIO_THROTTLE]/MAX_PPRZ"/>
|
||||
</while>
|
||||
</block>
|
||||
<block name="on ground">
|
||||
<exception cond="estimator_in_flight" deroute="flying"/>
|
||||
<!-- <set value="1" var="kill_throttle"/> -->
|
||||
<stay wp="TARGET"/>
|
||||
</block>
|
||||
<block name="flying">
|
||||
<!-- <exception cond="!estimator_in_flight" deroute="on ground"/> -->
|
||||
<call fun="NavSetWaypointHere(WP_TARGET)"/>
|
||||
<set value="0" var="kill_throttle"/>
|
||||
<stay wp="TARGET"/>
|
||||
</block>
|
||||
</blocks>
|
||||
|
||||
</flight_plan>
|
||||
@@ -1,14 +0,0 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="240" ground_alt="170" lat0="27.0929" lon0="77.9169" max_dist_from_home="5000" name="UGV" security_height="25">
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0"/>
|
||||
<waypoint name="UGVWP" x="0.0" y="100.0"/>
|
||||
</waypoints>
|
||||
<blocks>
|
||||
<block name="Goto UGVWP">
|
||||
<go approaching_time="0" wp="UGVWP"/>
|
||||
<deroute block="Goto UGVWP"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -1,294 +0,0 @@
|
||||
open Printf
|
||||
open Latlong
|
||||
|
||||
module GroundPprz = Pprz.Messages(struct let name = "ground" end)
|
||||
module IhmUpPprz = Pprz.Messages(struct let name = "ihm_up" end)
|
||||
module IhmDownPprz = Pprz.Messages(struct let name = "ihm_down" end)
|
||||
|
||||
|
||||
type point = { x: int; y: int; z: int }
|
||||
type pattern =
|
||||
Circle of point * int
|
||||
| Eight of point * point * int
|
||||
| Line of point * point
|
||||
| Nop
|
||||
|
||||
let print_p = fun c p ->
|
||||
fprintf c "(%d,%d,%d)" p.x p.y p.z
|
||||
|
||||
let print_pattern = fun a ->
|
||||
match a with
|
||||
Circle (p, r) -> printf "Circle (%a %d) " print_p p r
|
||||
| Eight (p1, p2, r) -> printf "Eight (%a %a %d) " print_p p1 print_p p2 r
|
||||
| Line (p1, p2) -> printf "Line (%a %a) " print_p p1 print_p p2
|
||||
| Nop -> printf "Nop "
|
||||
|
||||
let print_patterns = fun t ->
|
||||
let i = ref 0 in
|
||||
List.iter (fun p -> printf "%d:" !i; incr i; print_pattern p) t;
|
||||
print_newline ()
|
||||
|
||||
|
||||
(** Hashtbl of timelines indexed by the aircraft id *)
|
||||
let timelines = Hashtbl.create 3
|
||||
let timeline_max_length = 8
|
||||
|
||||
let add_timeline ac_id =
|
||||
let t = [Nop] in
|
||||
Hashtbl.add timelines ac_id t;
|
||||
t
|
||||
|
||||
|
||||
(* FIXME (from flight_plans/ihm.xml) *)
|
||||
let nop_block = 0
|
||||
let circle_block = 1
|
||||
let eight_block = 2
|
||||
let glide_block = 3
|
||||
let end_glide_block = 4
|
||||
|
||||
(* FIXME (from settings/ihm.xml) *)
|
||||
let nav_radius_id = 0
|
||||
|
||||
(* FIXME *)
|
||||
let utm_ref = utm_of WGS84 { posn_lat=(Deg>>Rad)43.4624; posn_long=(Deg>>Rad)1.2727 }
|
||||
let geo_of = fun p -> of_utm WGS84 (utm_add utm_ref (float p.x, float p.y))
|
||||
|
||||
let send_circle = fun ac_id p r ->
|
||||
let wgs84 = geo_of p in
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"wp_id", Pprz.Int 1; (* FIXME *)
|
||||
"alt", Pprz.Float (float p.z);
|
||||
"lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
|
||||
"long", Pprz.Float ((Rad>>Deg)wgs84.posn_long)] in
|
||||
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
|
||||
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"index", Pprz.Int nav_radius_id;
|
||||
"value", Pprz.Float (float r) ] in
|
||||
GroundPprz.message_send "ihm" "DL_SETTING" vs;
|
||||
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"block_id", Pprz.Int circle_block ] in
|
||||
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
|
||||
|
||||
|
||||
let send_line = fun ac_id p1 p2 ->
|
||||
let wgs84_1 = geo_of p1
|
||||
and wgs84_2 = geo_of p2 in
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"wp_id", Pprz.Int 1; (* FIXME *)
|
||||
"alt", Pprz.Float (float p1.z);
|
||||
"lat", Pprz.Float ((Rad>>Deg)wgs84_1.posn_lat);
|
||||
"long", Pprz.Float ((Rad>>Deg)wgs84_1.posn_long)] in
|
||||
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
|
||||
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"wp_id", Pprz.Int 2; (* FIXME *)
|
||||
"alt", Pprz.Float (float p2.z);
|
||||
"lat", Pprz.Float ((Rad>>Deg)wgs84_2.posn_lat);
|
||||
"long", Pprz.Float ((Rad>>Deg)wgs84_2.posn_long)] in
|
||||
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
|
||||
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"block_id", Pprz.Int glide_block ] in
|
||||
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
|
||||
|
||||
|
||||
let send_eight = fun ac_id p1 p2 r ->
|
||||
let wgs84_1 = geo_of p1
|
||||
and wgs84_2 = geo_of p2 in
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"wp_id", Pprz.Int 1; (* FIXME *)
|
||||
"alt", Pprz.Float (float p1.z);
|
||||
"lat", Pprz.Float ((Rad>>Deg)wgs84_1.posn_lat);
|
||||
"long", Pprz.Float ((Rad>>Deg)wgs84_1.posn_long)] in
|
||||
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
|
||||
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"wp_id", Pprz.Int 2; (* FIXME *)
|
||||
"alt", Pprz.Float (float p2.z);
|
||||
"lat", Pprz.Float ((Rad>>Deg)wgs84_2.posn_lat);
|
||||
"long", Pprz.Float ((Rad>>Deg)wgs84_2.posn_long)] in
|
||||
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
|
||||
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"index", Pprz.Int nav_radius_id;
|
||||
"value", Pprz.Float (float r) ] in
|
||||
GroundPprz.message_send "ihm" "DL_SETTING" vs;
|
||||
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"block_id", Pprz.Int eight_block ] in
|
||||
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
|
||||
|
||||
|
||||
let send_pattern_up = fun ac_id ->
|
||||
try
|
||||
let tl = Hashtbl.find timelines ac_id in
|
||||
begin
|
||||
match tl with
|
||||
Circle (p, r) :: _ -> send_circle ac_id p r
|
||||
| Eight (p1, p2, r) :: _ -> send_eight ac_id p1 p2 r
|
||||
| Line (p1, p2) :: _ -> send_line ac_id p1 p2
|
||||
| Nop :: _ ->
|
||||
let vs = [ "ac_id", Pprz.String ac_id;
|
||||
"block_id", Pprz.Int nop_block ] in
|
||||
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
|
||||
| [] -> failwith (Printf.sprintf "send_pattern_up: %s - empty list" ac_id)
|
||||
end
|
||||
with
|
||||
Not_found -> failwith (Printf.sprintf "send_pattern_up: %s" ac_id)
|
||||
|
||||
let get_ac = fun vs ->
|
||||
let ac_id = Pprz.string_assoc "ac_id" vs in
|
||||
try
|
||||
Hashtbl.find timelines ac_id
|
||||
with
|
||||
Not_found ->
|
||||
add_timeline ac_id
|
||||
|
||||
let insert_in_timeline values idx action =
|
||||
let t = get_ac values in
|
||||
let rec iter t i =
|
||||
if i = 0 then action :: t else
|
||||
match t with
|
||||
[] -> failwith "insert_in_timeline"
|
||||
| x :: xs -> x :: iter xs (i-1) in
|
||||
let newt = iter t idx in
|
||||
|
||||
(***)print_patterns newt;
|
||||
|
||||
let ac_id = Pprz.string_assoc "ac_id" values in
|
||||
Hashtbl.replace timelines ac_id newt
|
||||
|
||||
(** Bind to message while catching all the esceptions of the callback *)
|
||||
let safe_bind = fun msg cb bind ->
|
||||
let safe_cb = fun sender vs ->
|
||||
try cb sender vs with x -> prerr_endline (Printexc.to_string x) in
|
||||
ignore (bind msg safe_cb)
|
||||
|
||||
let safe_bind_up = fun msg cb ->
|
||||
safe_bind msg cb IhmUpPprz.message_bind
|
||||
|
||||
let safe_bind_ground = fun msg cb ->
|
||||
safe_bind msg cb GroundPprz.message_bind
|
||||
|
||||
|
||||
let point_assoc = fun x y z values ->
|
||||
{ x = Pprz.int_assoc x values;
|
||||
y = Pprz.int_assoc y values;
|
||||
z = Pprz.int_assoc z values }
|
||||
|
||||
|
||||
let ihm_circle_cb = fun _sender values ->
|
||||
let idx = Pprz.int_assoc "idx_timeline" values
|
||||
and p = point_assoc "x" "y" "z" values
|
||||
and r = Pprz.int_assoc "r" values in
|
||||
let action = Circle (p, r) in
|
||||
insert_in_timeline values idx action;
|
||||
if idx = 0 then
|
||||
send_pattern_up (Pprz.string_assoc "ac_id" values)
|
||||
|
||||
let ihm_line_cb = fun _sender values ->
|
||||
let idx = Pprz.int_assoc "idx_timeline" values
|
||||
and p1 = point_assoc "x1" "y1" "z1" values
|
||||
and p2 = point_assoc "x2" "y2" "z2" values in
|
||||
let action = Line (p1, p2) in
|
||||
insert_in_timeline values idx action;
|
||||
if idx = 0 then
|
||||
send_pattern_up (Pprz.string_assoc "ac_id" values)
|
||||
|
||||
let ihm_eight_cb = fun _sender values ->
|
||||
let idx = Pprz.int_assoc "idx_timeline" values
|
||||
and p1 = point_assoc "x1" "y1" "z" values
|
||||
and p2 = point_assoc "x2" "y2" "z" values
|
||||
and r = Pprz.int_assoc "r" values in
|
||||
let action = Eight (p1, p2, r) in
|
||||
insert_in_timeline values idx action;
|
||||
if idx = 0 then
|
||||
send_pattern_up (Pprz.string_assoc "ac_id" values)
|
||||
(*
|
||||
let delete = fun timeline idx values ->
|
||||
(* Shift left *)
|
||||
for i = max 0 idx to timeline_max_length - 2 do
|
||||
timeline.(i) <- timeline.(i+1)
|
||||
done;
|
||||
if idx = 0 then
|
||||
send_pattern_up (Pprz.string_assoc "ac_id" values)
|
||||
*)
|
||||
|
||||
let delete_in_timeline values idx =
|
||||
let rec iter t idx =
|
||||
if idx = 0 then
|
||||
match t with
|
||||
[] -> failwith "delete_in_timeline"
|
||||
| x :: xs -> xs
|
||||
else
|
||||
match t with
|
||||
[] -> failwith "delete_in_timeline"
|
||||
| x :: xs -> x :: iter xs (idx-1) in
|
||||
let t = get_ac values in
|
||||
let newt = iter t idx in
|
||||
let ac_id = Pprz.string_assoc "ac_id" values in
|
||||
Hashtbl.replace timelines ac_id newt;
|
||||
|
||||
print_patterns newt;
|
||||
|
||||
if idx = 0 then
|
||||
send_pattern_up (Pprz.string_assoc "ac_id" values)
|
||||
|
||||
let ihm_delete_cb = fun _sender values ->
|
||||
let idx = Pprz.int_assoc "idx_timeline" values in
|
||||
delete_in_timeline values idx
|
||||
|
||||
let nav_status_cb = fun _sender values ->
|
||||
let block = Pprz.int_assoc "cur_block" values in
|
||||
if block = end_glide_block then
|
||||
delete_in_timeline values 0
|
||||
|
||||
let fp_cb = fun _sender values ->
|
||||
let lat = Pprz.float_assoc "lat" values
|
||||
and lon = Pprz.float_assoc "long" values
|
||||
and alt = Pprz.float_assoc "alt" values in
|
||||
let wgs84 = { posn_lat=(Deg>>Rad)lat; posn_long=(Deg>>Rad)lon } in
|
||||
let utm = utm_of WGS84 wgs84 in
|
||||
let (x, y) = utm_sub utm utm_ref in
|
||||
let z = alt in
|
||||
let vs = ["x", Pprz.Int (truncate x);
|
||||
"y", Pprz.Int (truncate y);
|
||||
"z", Pprz.Int (truncate z)] in
|
||||
IhmDownPprz.message_send "ihm" "AC_POS" vs
|
||||
|
||||
|
||||
|
||||
let listen = fun () ->
|
||||
safe_bind_up "IHM_CIRCLE" ihm_circle_cb;
|
||||
safe_bind_up "IHM_LINE" ihm_line_cb;
|
||||
safe_bind_up "IHM_EIGHT" ihm_eight_cb;
|
||||
safe_bind_up "IHM_DELETE" ihm_delete_cb;
|
||||
safe_bind_ground "NAV_STATUS" nav_status_cb;
|
||||
safe_bind_ground "FLIGHT_PARAM" fp_cb
|
||||
|
||||
|
||||
|
||||
(*** Options ***)
|
||||
|
||||
let ivy_bus = Defivybus.default_ivy_bus
|
||||
|
||||
let options =
|
||||
[ "-b", Arg.String (fun x -> ivy_bus := x), (Printf.sprintf "Bus\tDefault is %s" !ivy_bus)]
|
||||
|
||||
|
||||
(************** Main ****************)
|
||||
let () =
|
||||
Arg.parse options
|
||||
(fun x -> Printf.fprintf stderr "%s: Warning: Don't do anything with '%s' argument\n" Sys.argv.(0) x)
|
||||
"Usage: ";
|
||||
|
||||
Ivy.init "Paparazzi gui_plug" "READY" (fun _ _ -> ());
|
||||
Ivy.start !ivy_bus;
|
||||
|
||||
listen ();
|
||||
|
||||
(* Main loop *)
|
||||
let loop = Glib.Main.create true in
|
||||
while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done
|
||||
Reference in New Issue
Block a user