*** empty log message ***

2026-05-21 20:04:09 +08:00 · 2006-12-12 06:28:10 +00:00
parent 09e3db7e9e
commit cd5c539bd1
24 changed files with 456 additions and 71 deletions
@@ -263,7 +263,7 @@ ap.srcs += gyro.c

 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DCONFIG=\"tiny.h\"
+sim.CFLAGS += -DCONFIG=\"tiny.h\" -DLOITER_TRIM


 # a test program to setup actuators 
@@ -108,6 +108,7 @@

    <define name="TRIGGER_DELAY" value="1."/>
    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+    <define name="MIN_CIRCLE_RADIUS" value="50."/>
  </section>
 
  <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -203,6 +204,10 @@
    <define name="DEVICE_ADDRESS" value="...."/>
  </section>

+  <section name="SIMU">
+    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+  </section>
+
 <makefile>
 include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile

@@ -262,7 +267,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN -DIR_360
 sim.srcs += traffic_info.c
-# sim.srcs += bomb.c
+sim.srcs += anemotaxis.c chemotaxis.c discsurvey.c


 # a test program to setup actuators 
@@ -184,6 +184,7 @@
 
  <section name="SIMU">
    <define name="WEIGHT" value="1."/>
+    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
  </section>

  <section name="FAILSAFE" prefix="FAILSAFE_">
@@ -0,0 +1,117 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan ground_alt="200" alt="250" lat0="43.4624" lon0="1.2727" max_dist_from_home="1500" name="Intelligent Surveillance" qfu="270" security_height="25">
+<header>
+#include "anemotaxis.h"
+#include "chemotaxis.h"
+#include "discsurvey.h"
+</header>
+  <waypoints>
+    <waypoint name="HOME" x="0" y="0"/>
+    <waypoint name="1" x="200" y="0"/>
+    <waypoint name="2" x="0" y="200"/>
+    <waypoint name="MOB" x="200" y="200"/>
+    <waypoint name="S1" x="-200" y="-200"/>
+    <waypoint name="S2" x="300" y="400"/>
+    <waypoint name="C" x="-250" y="-200"/>
+    <waypoint name="C1" x="-200" y="-200"/>
+    <waypoint name="C2" x="300" y="400"/>
+    <waypoint name="PLUME" x="400" y="400"/>
+  </waypoints>
+  <blocks>
+<!--
+    <block name="wait GPS">
+      <while cond="!GpsFixValid()"/>
+    </block>
+
+    <block name="init">
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call fun="NavSetGroundReferenceHere()"/>
+      <deroute block="circle 1"/>
+    </block>
+-->
+
+    <block name="circle 1">
+      <circle radius="75" wp="1"/>
+    </block>
+
+    <block name="eight 1">
+      <eight radius="75" center="1" turn_around="2"/>
+    </block>
+
+    <block name="oval" strip_button="Oval">
+      <oval p1="1" p2="2" radius="80"/>
+    </block>
+
+    <block name="MOB" strip_button="MOB">
+      <call fun="NavSetWaypointHere(WP_MOB)"/>
+      <circle wp="MOB" radius="100"/>
+    </block>
+
+    <block name="Anemotaxis" strip_button="Anemotaxis">
+      <call fun="nav_anemotaxis_downwind(WP_C)"/>
+      <go wp="C" hmode="route" approaching_time="0"/>
+      <call fun="nav_anemotaxis_init(WP_C)"/>
+      <call fun="nav_anemotaxis(WP_C, WP_C1, WP_C2, WP_PLUME)"/>
+    </block>
+
+    <block name="Chemotaxis" strip_button="Chemotaxis">
+      <call fun="nav_chemotaxis_init()"/>
+      <call fun="nav_chemotaxis(WP_C, WP_PLUME)"/>
+    </block>
+
+    <block name="Disc survey" strip_button="DS">
+      <call fun="disc_survey_init()"/>
+      <call fun="disc_survey(WP_HOME, 500)"/>
+    </block>
+
+    <block name="circle 1 auto pitch">
+      <circle radius="75" wp="1" pitch="auto" throttle="0.7"/>
+    </block>
+
+    <block name="circle left 1">
+      <circle radius="-75" wp="1"/>
+    </block>
+
+    <block name="climb 75">
+      <circle radius="50+(estimator_z-ground_alt)/2" wp="1" throttle="0.75" pitch="0.3" vmode="throttle" until="10 > PowerVoltage()"/>
+    </block>
+
+    <block name="climb 1">
+      <circle radius="50+(estimator_z-ground_alt)/2" wp="1" climb="1" pitch="0.1" vmode="climb" until="10 > PowerVoltage()"/>
+    </block>
+
+    <block name="descent 0" strip_button="Descent">
+      <circle radius="50+(estimator_z-ground_alt)/2" wp="1" throttle="0.0" pitch="-0.3" vmode="throttle" until="ground_alt+50 > estimator_z"/>
+      <deroute block="circle 1"/>
+    </block>
+
+    <block name="route12">
+      <go approaching_time="0" from="1" hmode="route" wp="2"/>
+    </block>
+
+    <block name="glide12">
+      <go from="1" hmode="route" vmode="glide" wp="2"/>
+    </block>
+
+    <block name="stack 2">
+      <circle radius="75" wp="2"/>
+    </block>
+
+    <block name="route21">
+      <go approaching_time="0" from="2" hmode="route" wp="1"/>
+    </block>
+
+    <block name="stack 1">
+      <circle radius="75" wp="1"/>
+    </block>
+
+    <block name="survey">
+      <survey_rectangle grid="150" wp1="S1" wp2="S2"/>
+    </block>
+
+    <block name="follow">
+      <follow ac_id="5" distance="25" height="10"/>
+    </block>
+  </blocks>
+</flight_plan>
@@ -624,6 +624,13 @@
    <field name="ac_id" type="string"/>
    <field name="message" type="string"/>
  </message>
+
+  <message name="PLUMES" ID="100">
+    <field name="ids" type="string" format="csv"/>
+    <field name="lats" type="string" format="csv"/>
+    <field name="longs" type="string" format="csv"/>
+    <field name="values" type="string" format="csv"/>
+  </message>
 </class>


@@ -0,0 +1,100 @@
+#include "airframe.h"
+#include "estimator.h"
+#include "std.h"
+#include "nav.h"
+#include "flight_plan.h"
+#include "ap_downlink.h"
+
+uint8_t chemo_sensor;
+
+enum status { UTURN, CROSSWIND };
+static enum status status;
+static int8_t sign;
+static struct point last_plume;
+
+static void last_plume_was_here( void ) {
+  last_plume.x = estimator_x;
+  last_plume.y = estimator_y;
+}
+
+bool_t nav_anemotaxis_downwind( uint8_t c ) {
+  float wind_dir = atan2(wind_north, wind_east);
+  waypoints[c].x = waypoints[WP_HOME].x + MAX_DIST_FROM_HOME*0.9*cos(wind_dir);
+  waypoints[c].y = waypoints[WP_HOME].y + MAX_DIST_FROM_HOME*0.9*sin(wind_dir);
+  return FALSE;
+}
+
+bool_t nav_anemotaxis_init( uint8_t c ) {
+  status = UTURN;
+  sign = 1;
+  float wind_dir = atan2(wind_north, wind_east);
+  waypoints[c].x = estimator_x + MIN_CIRCLE_RADIUS*cos(wind_dir+M_PI);
+  waypoints[c].y = estimator_y + MIN_CIRCLE_RADIUS*sin(wind_dir+M_PI);
+  last_plume_was_here();
+  return FALSE;
+}
+
+bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
+  if (chemo_sensor) {
+    last_plume_was_here();
+    waypoints[plume].x = estimator_x;
+    waypoints[plume].y = estimator_y;
+    DownlinkSendWp(plume);
+  }
+
+  float wind_dir = atan2(wind_north, wind_east) + M_PI;
+
+  /** Not null even if wind_east=wind_north=0 */
+  float upwind_x = cos(wind_dir);
+  float upwind_y = sin(wind_dir);
+
+  switch (status) {
+  case UTURN:
+    NavCircleWaypoint(c, MIN_CIRCLE_RADIUS*sign);
+    if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
+      float crosswind_x = - upwind_y;
+      float crosswind_y = upwind_x;
+      waypoints[c1].x = waypoints[c].x + MIN_CIRCLE_RADIUS*upwind_x;
+      waypoints[c1].y = waypoints[c].y + MIN_CIRCLE_RADIUS*upwind_y;
+
+      float width = Max(2*ScalarProduct(upwind_x, upwind_y, estimator_x-last_plume.x, estimator_y-last_plume.y), MIN_CIRCLE_RADIUS);
+
+      waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
+      waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
+
+      DownlinkSendWp(c1);
+      DownlinkSendWp(c2);
+
+      status = CROSSWIND;
+      nav_init_stage();
+    }
+    break;
+
+  case CROSSWIND:
+    NavSegment(c1, c2);
+    if (NavApproaching(c2, CARROT)) {
+      waypoints[c].x = waypoints[c2].x + MIN_CIRCLE_RADIUS*upwind_x;
+      waypoints[c].y = waypoints[c2].y + MIN_CIRCLE_RADIUS*upwind_y;
+
+      DownlinkSendWp(c);
+     
+      sign = -sign;
+      status = UTURN;
+      nav_init_stage();
+    }
+
+    if (chemo_sensor) {
+      waypoints[c].x = estimator_x + MIN_CIRCLE_RADIUS*upwind_x;
+      waypoints[c].y = estimator_y + MIN_CIRCLE_RADIUS*upwind_y;
+
+      DownlinkSendWp(c);
+     
+      sign = -sign;
+      status = UTURN;
+      nav_init_stage();
+    }
+    break;
+  }
+  chemo_sensor = 0;
+  return TRUE;
+}
@@ -0,0 +1,7 @@
+#include "std.h"
+
+extern uint8_t chemo_sensor;
+extern bool_t nav_anemotaxis_downwind(uint8_t c);
+extern bool_t nav_anemotaxis_init(uint8_t c);
+extern bool_t nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume);
+
@@ -83,14 +83,20 @@

 #define PERIODIC_SEND_NAVIGATION_REF()  DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);

-#define PERIODIC_SEND_WP_MOVED() { \
-  static uint8_t i; \
-  i++; if (i > nb_waypoint) i = 0; \
+
+#define DownlinkSendWp(i) { \
  float x = nav_utm_east0 +  waypoints[i].x; \
  float y = nav_utm_north0 + waypoints[i].y; \
  DOWNLINK_SEND_WP_MOVED(&i, &x, &y, &(waypoints[i].a)); \
 }

+
+#define PERIODIC_SEND_WP_MOVED() { \
+  static uint8_t i; \
+  i++; if (i > nb_waypoint) i = 0; \
+  DownlinkSendWp(i); \
+}
+
 #ifdef RADIO_CONTROL_SETTINGS
 #define PERIODIC_SEND_SETTINGS() if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(&slider_1_val, &slider_2_val);
 #else
@@ -0,0 +1,48 @@
+#include "airframe.h"
+#include "estimator.h"
+#include "std.h"
+#include "nav.h"
+#include "flight_plan.h"
+#include "ap_downlink.h"
+
+uint8_t chemo_sensor;
+
+#define MAX_RADIUS 500
+#define ALPHA 0.5
+
+static uint8_t last_plume_value;
+
+static float radius;
+static int8_t sign;
+
+bool_t nav_chemotaxis_init( void ) {
+  radius = MAX_RADIUS;
+  last_plume_value = 0;
+  sign = 1;
+  return FALSE;
+}
+
+bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {
+
+  if (chemo_sensor > last_plume_value) {
+    /* Store this plume */
+    waypoints[plume].x = estimator_x;
+    waypoints[plume].y = estimator_y;
+    DownlinkSendWp(plume);
+    last_plume_value = chemo_sensor;
+
+    /* Reduce the radius */
+    sign = - sign;
+    radius = sign * (MIN_CIRCLE_RADIUS+(255-chemo_sensor)/256.*(MAX_RADIUS-MIN_CIRCLE_RADIUS));
+
+    /* Move the circle in this direction */
+    float x = waypoints[c].x - waypoints[plume].x;
+    float y = waypoints[c].y - waypoints[plume].y;
+    waypoints[c].x = waypoints[plume].x + ALPHA * x;
+    waypoints[c].y = waypoints[plume].y + ALPHA * y;
+    DownlinkSendWp(c);
+  }
+
+  NavCircleWaypoint(c, radius);
+  return TRUE;
+}
@@ -0,0 +1,4 @@
+#include "std.h"
+
+extern bool_t nav_chemotaxis_init( void );
+extern bool_t nav_chemotaxis( uint8_t c, uint8_t plume );
@@ -58,10 +58,7 @@ float carrot_x, carrot_y;

 /** Status on the current circle */
 static float nav_circle_radians; /* Cumulated */
-static float nav_circle_trigo_qdr; /* Angle from center to mobile */
-
-#define NavCircleCount() (fabs(nav_circle_radians) / (2*M_PI))
-#define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; })
+float nav_circle_trigo_qdr; /* Angle from center to mobile */


 /** Status on the current leg (percentage, 0. < < 1.) in route mode */
@@ -82,6 +79,14 @@ float flight_altitude;

 float nav_glide_pitch_trim;

+void nav_init_stage( void ) {
+  last_x = estimator_x; last_y = estimator_y;
+  stage_time = 0;
+  stage_time_ds = 0;
+  nav_circle_radians = 0;
+  nav_in_circle = FALSE;
+  nav_in_segment = FALSE;
+}

 #define PowerVoltage() (vsupply/10.)
 #define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
@@ -95,7 +100,6 @@ float nav_glide_pitch_trim;
 #define Return() { nav_block=last_block; nav_stage=last_stage; block_time=0; return;}

 #define Stage(s) case s: nav_stage=s;
-#define InitStage() { last_x = estimator_x; last_y = estimator_y; stage_time = 0; stage_time_ds = 0; nav_circle_radians = 0; nav_in_circle = FALSE; nav_in_segment = FALSE; return; }
 #define NextStage() { nav_stage++; InitStage() }
 #define NextStageFrom(wp) { last_wp = wp; NextStage() }
 #define GotoStage(s) { nav_stage = s; InitStage() }
@@ -103,20 +107,14 @@ float nav_glide_pitch_trim;
 #define Label(x) label_ ## x:
 #define Goto(x) { goto label_ ## x; }

-static bool_t approaching(uint8_t, float) __attribute__ ((unused));
 static inline void fly_to_xy(float x, float y);
-static void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y);

 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))

 #define DegOfRad(x) ((x) / M_PI * 180.)
 #define RadOfDeg(x) ((x)/180. * M_PI)
-#define NormCourse(x) { \
-  while (x < 0) x += 360; \
-  while (x >= 360) x -= 360; \
-}

-static inline void nav_circle_XY(float x, float y, float radius) {
+void nav_circle_XY(float x, float y, float radius) {
  float last_trigo_qdr = nav_circle_trigo_qdr;
  nav_circle_trigo_qdr = atan2(estimator_y - y, estimator_x - x);

@@ -158,12 +156,6 @@ static inline void nav_circle_XY(float x, float y, float radius) {
  fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
 }

-#define NavCircleWaypoint(wp, radius) \
-  nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)
-
-#define NavSegment(_start, _end) \
-  nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y)
-
 #define NavSurveyRectangleInit(_wp1, _wp2, _grid) nav_survey_rectangle_init(_wp1, _wp2, _grid)
 #define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle(_wp1, _wp2)

@@ -237,7 +229,6 @@ static inline void nav_circle_XY(float x, float y, float radius) {
 #define Or(x, y) ((x) || (y))
 #define Min(x,y) (x < y ? x : y)
 #define Max(x,y) (x > y ? x : y)
-#define NavQdrCloseTo(x) ({ float _course = x; NormCourse(_course); float circle_qdr = NavCircleQdr(); (Min(_course, 350) < circle_qdr && circle_qdr < _course+10); })
 #define NavBlockTime() (block_time)
 #define LessThan(_x, _y) ((_x) < (_y))

@@ -249,7 +240,7 @@ static inline void nav_circle_XY(float x, float y, float radius) {
  fly_to_xy(ac->east - _distance*cos(alpha), ac->north - _distance*sin(alpha)); \
 }

-#define NavSetGroundReferenceHere() { nav_reset_reference(); nav_update_waypoints_alt(); }
+#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })

 /** Automatic survey of a sector (south-north sweep) */
 static struct point survey_from;
@@ -403,7 +394,7 @@ struct point waypoints[NB_WAYPOINT+1] = WAYPOINTS;
 *  uav has not gone past waypoint.
 *  Return true if it is the case.
 */
-static bool_t approaching_xy(float x, float y, float approaching_time) {
+bool_t nav_approaching_xy(float x, float y, float approaching_time) {
  /** distance to waypoint in x */
  float pw_x = x - estimator_x;
  /** distance to waypoint in y */
@@ -419,11 +410,6 @@ static bool_t approaching_xy(float x, float y, float approaching_time) {
  return (scal_prod < 0.);
 }

-static bool_t approaching(uint8_t wp, float approaching_time) {
-  return approaching_xy(waypoints[wp].x, waypoints[wp].y, approaching_time);
-}
-
-

 /** static inline void fly_to_xy(float x, float y)
 *  \brief Computes \a desired_x, \a desired_y and \a desired_course.
@@ -435,7 +421,7 @@ static inline void fly_to_xy(float x, float y) {
 }


-static void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
+void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
  float leg_x = wp_x - last_wp_x;
  float leg_y = wp_y - last_wp_y;
  float leg2 = leg_x * leg_x + leg_y * leg_y;
@@ -614,7 +600,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {

  case R12:
    nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
-    if (approaching_xy(c2_in.x, c2_in.y,CARROT)) { 
+    if (nav_approaching_xy(c2_in.x, c2_in.y,CARROT)) { 
      eight_status = C2;
      InitStage();
    }
@@ -630,7 +616,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {

  case R21:
    nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
-    if (approaching_xy(c1_in.x, c1_in.y,CARROT)) { 
+    if (nav_approaching_xy(c1_in.x, c1_in.y,CARROT)) { 
      eight_status = C1;
      InitStage();
    }
@@ -697,7 +683,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) {

  case OR12:
    nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
-    if (approaching_xy(p2_in.x, p2_in.y,CARROT)) { 
+    if (nav_approaching_xy(p2_in.x, p2_in.y,CARROT)) { 
      oval_status = OC2;
      InitStage();
    }
@@ -713,7 +699,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) {

  case OR21:
    nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
-    if (approaching_xy(waypoints[p1].x, waypoints[p1].y,CARROT)) { 
+    if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y,CARROT)) { 
      oval_status = OC1;
      InitStage();
    }
@@ -117,11 +117,37 @@ void nav_without_gps(void);

 extern void nav_goto_block(uint8_t block_id);

-#define NavSetWaypointHere(_wp) { \
+#define NavSetWaypointHere(_wp) ({ \
  waypoints[_wp].x = estimator_x; \
  waypoints[_wp].y = estimator_y; \
+  FALSE; \
+})
+
+extern float nav_circle_trigo_qdr; /** Angle from center to mobile */
+extern void nav_circle_XY(float x, float y, float radius);
+
+#define NavCircleWaypoint(wp, radius) \
+  nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)
+
+#define NormCourse(x) { \
+  while (x < 0) x += 360; \
+  while (x >= 360) x -= 360; \
 }

+#define NavCircleCount() (fabs(nav_circle_radians) / (2*M_PI))
+#define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; })
+#define NavQdrCloseTo(x) ({ float _course = x; NormCourse(_course); float circle_qdr = NavCircleQdr(); (Min(_course, 350) < circle_qdr && circle_qdr < _course+10); })

+extern void nav_init_stage( void );
+#define InitStage() { nav_init_stage(); return; }
+
+
+/*********** Navigation along a line *************************************/
+extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y);
+#define NavSegment(_start, _end) \
+  nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y)
+
+bool_t nav_approaching_xy(float x, float y, float approaching_time);
+#define NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, time)

 #endif /* NAV_H */
@@ -146,3 +146,9 @@ value dl_setting(value index __attribute__ ((unused)),
  DOWNLINK_SEND_DL_VALUE(&i, &var);
  return Val_unit;
 }
+
+value set_wind(value east, value north) {
+  wind_east = Double_val(east);
+  wind_north = Double_val(north);
+  return Val_unit;
+}
@@ -31,7 +31,7 @@ INCLUDES=-I +lablgtk2 -I ../../lib/ocaml -I +xml-light
 LIBS=glibivy-ocaml.cma lablgtk.cma lib-pprz.cma lablgnomecanvas.cma xlib-pprz.cma
 CMXA=$(LIBS:.cma=.cmxa)

-ML= horizon.ml strip.ml pages.ml speech.ml plugin.ml sectors.ml map2d.ml editFP.ml live.ml gcs.ml
+ML= horizon.ml strip.ml pages.ml speech.ml plugin.ml sectors.ml map2d.ml editFP.ml live.ml particules.ml gcs.ml
 MAIN=gcs
 CMO=$(ML:.ml=.cmo)
 CMX=$(ML:.ml=.cmx)
@@ -1,9 +1,9 @@
 val path_button :
-  MapCanvas.widget -> MapCanvas.LL.fmeter * MapCanvas.LL.fmeter -> unit
+  MapCanvas.widget -> MapCanvas.world -> unit
 val path_notify : MapCanvas.widget -> float * float -> bool
 val path_close : unit -> unit
 val path_change_radius : [> `DOWN | `UP ] -> unit
-val create_wp : MapCanvas.widget -> MapCanvas.LL.geographic -> MapWaypoints.waypoint
+val create_wp : MapCanvas.widget -> Latlong.geographic -> MapWaypoints.waypoint
 val calibrate_map : MapCanvas.widget -> GBin.frame -> Gtk.accel_group -> unit -> unit
 val new_fp : MapCanvas.widget -> GBin.frame -> Gtk.accel_group -> unit -> unit
 val load_fp : MapCanvas.widget -> GBin.frame -> Gtk.accel_group -> unit -> unit
@@ -336,6 +336,7 @@ and mplayer = ref ""
 and plugin_window = ref ""
 and layout_file = ref "horizontal.xml"
 and edit = ref false
+and display_particules = ref false

 let options =
  [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010";
@@ -359,6 +360,7 @@ let options =
    "-auto_ortho", Arg.Set auto_ortho, "IGN tiles path";
    "-google_fill", Arg.Set GM.auto, "Google maps auto fill";
    "-speech", Arg.Set Speech.active, "Active vocal messages";
+    "-particules", Arg.Set display_particules, "Display particules";
    "-m", Arg.String (fun x -> map_files := x :: !map_files), "Map description file"]


@@ -597,5 +599,8 @@ let _main =

  Speech.say "Welcome to papa ratsi";

+  if !display_particules then
+    Particules.listen geomap ;
+
  (** Threaded main loop (map tiles loaded concurently) *)
  GtkThread.main ()
@@ -85,20 +85,20 @@ type aircraft = {
    mutable ground_prox : bool;
  }

-let live_aircrafts = Hashtbl.create 3
+let aircrafts = Hashtbl.create 3
 let active_ac = ref ""
 let get_ac = fun vs ->
  let ac_id = Pprz.string_assoc "ac_id" vs in
-  Hashtbl.find live_aircrafts ac_id
+  Hashtbl.find aircrafts ac_id

 let select_ac = fun acs_notebook ac_id ->
  if !active_ac <> ac_id then
-    let ac = Hashtbl.find live_aircrafts ac_id in
+    let ac = Hashtbl.find aircrafts ac_id in

    (* Show the buttons in the active strip and hide the previous active one *)
    ac.strip#show_buttons ();
    if !active_ac <> "" then begin
-      let ac' = Hashtbl.find live_aircrafts !active_ac in
+      let ac' = Hashtbl.find aircrafts !active_ac in
      ac'.strip#hide_buttons ();
      ac'.notebook_label#set_width_chars (String.length ac'.notebook_label#text)  
    end;
@@ -124,7 +124,7 @@ let log =
 let log_and_say = fun a ac_id s -> log ~say:true a ac_id s

 let show_mission = fun ac on_off ->
-  let a = Hashtbl.find live_aircrafts ac in
+  let a = Hashtbl.find aircrafts ac in
  if on_off then
    a.fp_group#show ()
  else
@@ -149,7 +149,7 @@ let send_move_waypoint_msg = fun ac i w ->
  Ground_Pprz.message_send "map2d" "MOVE_WAYPOINT" vs

 let commit_changes = fun ac ->
-  let a = Hashtbl.find live_aircrafts ac in
+  let a = Hashtbl.find aircrafts ac in
  List.iter 
    (fun w ->
      let (i, w) = a.fp_group#index w in
@@ -460,7 +460,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
 	       pages = ac_frame#coerce;
 	       notebook_label = _label
 	     } in
-    Hashtbl.add live_aircrafts ac_id ac;
+    Hashtbl.add aircrafts ac_id ac;
    select_ac acs_notebook ac_id;

  (** Periodically send the wind estimation through
@@ -510,10 +510,10 @@ let ok_color = "green"
 let warning_color = "orange"
 let alert_color = "red"

-    (** Bind to message while catching all the esceptions of the callback *)
+(** Bind to message while catching all the esceptions of the callback *)
 let safe_bind = fun msg cb ->
  let safe_cb = fun sender vs ->
-    try cb sender vs with _ -> () in
+    try cb sender vs with x -> prerr_endline (Printexc.to_string x) in
  ignore (Ground_Pprz.message_bind msg safe_cb)

 let alert_bind = fun msg cb ->
@@ -523,7 +523,7 @@ let alert_bind = fun msg cb ->

 let ask_config = fun alert geomap fp_notebook ac ->
  let get_config = fun _sender values ->
-    if not (Hashtbl.mem live_aircrafts ac) then
+    if not (Hashtbl.mem aircrafts ac) then
      create_ac alert geomap fp_notebook ac values
  in
  Ground_Pprz.message_req "map2d" "CONFIG" ["ac_id", Pprz.String ac] get_config
@@ -531,7 +531,7 @@ let ask_config = fun alert geomap fp_notebook ac ->
    

 let one_new_ac = fun alert (geomap:G.widget) fp_notebook ac ->
-  if not (Hashtbl.mem live_aircrafts ac) then
+  if not (Hashtbl.mem aircrafts ac) then
    ask_config alert geomap fp_notebook ac
      

@@ -593,7 +593,7 @@ let aircrafts_msg = fun alert (geomap:G.widget) fp_notebook acs ->
 let listen_dl_value = fun () ->
  let get_dl_value = fun _sender vs ->
    let ac_id = Pprz.string_assoc "ac_id" vs in
-    let ac = Hashtbl.find live_aircrafts ac_id in
+    let ac = Hashtbl.find aircrafts ac_id in
    match ac.dl_settings_page with
      Some settings ->
 	let csv = Pprz.string_assoc "values" vs in
@@ -699,7 +699,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->

  let get_cam_status = fun _sender vs ->
    let ac_id = Pprz.string_assoc "ac_id" vs in
-    let ac = Hashtbl.find live_aircrafts ac_id in
+    let ac = Hashtbl.find aircrafts ac_id in
    let a = fun s -> Pprz.float_assoc s vs in
    let cam_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_lat"); posn_long = (Deg>>Rad)(a "cam_long") }
    and target_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_target_lat"); posn_long = (Deg>>Rad)(a "cam_target_long") } in
@@ -718,7 +718,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert ->

  let get_segment_status = fun _sender vs ->
    let ac_id = Pprz.string_assoc "ac_id" vs in
-    let ac = Hashtbl.find live_aircrafts ac_id in
+    let ac = Hashtbl.find aircrafts ac_id in
    let a = fun s -> Pprz.float_assoc s vs in
    let geo1 = { posn_lat = (Deg>>Rad)(a "segment1_lat"); posn_long = (Deg>>Rad)(a "segment1_long") }
    and geo2 = { posn_lat = (Deg>>Rad)(a "segment2_lat"); posn_long = (Deg>>Rad)(a "segment2_long") } in
@@ -783,13 +783,16 @@ let listen_waypoint_moved = fun () ->
    and altitude = a "alt" in

    (** FIXME: No indexed access to waypoints: iter and compare: *)
-    List.iter (fun w ->
-      let (i, w) = ac.fp_group#index w in
-      if i = wp_id then begin
-	w#set ~if_not_moved:true ~altitude ~update:true geo;
-	raise Exit (** catched by safe_bind *)
-      end)
-      ac.fp_group#waypoints
+    try
+      List.iter (fun w ->
+	let (i, w) = ac.fp_group#index w in
+	if i = wp_id then begin
+	  w#set ~if_not_moved:true ~altitude ~update:true geo;
+	  raise Exit
+	end)
+	ac.fp_group#waypoints
+    with
+      Exit -> ()
  in
  safe_bind "WAYPOINT_MOVED" get_values
    
@@ -804,7 +807,7 @@ let listen_alert = fun a ->

 let get_infrared = fun _sender vs ->
  let ac_id = Pprz.string_assoc "ac_id" vs in
-  let ac = Hashtbl.find live_aircrafts ac_id in
+  let ac = Hashtbl.find aircrafts ac_id in
  let ir_page = ac.ir_page in
  let gps_hybrid_mode = Pprz.string_assoc "gps_hybrid_mode" vs in
  let gps_hybrid_factor = Pprz.float_assoc "gps_hybrid_factor" vs in
@@ -820,7 +823,7 @@ let listen_infrared = fun () -> safe_bind "INFRARED" get_infrared

 let get_svsinfo = fun _sender vs ->
  let ac_id = Pprz.string_assoc "ac_id" vs in
-  let ac = Hashtbl.find live_aircrafts ac_id in
+  let ac = Hashtbl.find aircrafts ac_id in
  let gps_page = ac.gps_page in
  let svid = Str.split list_separator (Pprz.string_assoc "svid" vs)
  and cn0 = Str.split list_separator (Pprz.string_assoc "cno" vs)
@@ -881,5 +884,5 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac ->
      (fun ac_id ac -> 
 	if ac.pages#get_oid = ac_page#get_oid
 	then select_ac ac_notebook ac_id) 
-      live_aircrafts in
+      aircrafts in
  ignore (ac_notebook#connect#switch_page ~callback)
@@ -24,6 +24,44 @@
 *
 *)

+
+type color = string
+type aircraft = {
+    ac_name : string;
+    config : Pprz.values;
+    track : MapTrack.track;
+    color: color;
+    fp_group : MapFP.flight_plan;
+    fp : Xml.xml;
+    blocks : (int * string) list;
+    mutable last_ap_mode : string;
+    mutable last_stage : int * int;
+    ir_page : Pages.infrared;
+    gps_page : Pages.gps;
+    pfd_page : Pages.pfd;
+    misc_page : Pages.misc;
+    dl_settings_page : Pages.settings option;
+    rc_settings_page : Pages.rc_settings option;
+    pages : GObj.widget;
+    notebook_label : GMisc.label;
+    strip : Strip.t;
+    mutable first_pos : bool;
+    mutable last_block_name : string;
+    mutable in_kill_mode : bool;
+    mutable speed : float;
+    mutable alt : float;
+    mutable target_alt : float;
+    mutable flight_time : int;
+    mutable wind_speed : float;
+    mutable wind_dir : float; (* Rad *)
+    mutable ground_prox : bool;
+  }
+
+val aircrafts : (string, aircraft) Hashtbl.t
+
+
+val safe_bind : string -> (string -> Pprz.values -> unit) -> unit
+
 val track_size : int ref
 (** Default length for A/C tracks on the 2D view *)

@@ -76,4 +76,6 @@ typedef uint8_t unit_t;

 #define Blend(a, b, rho) (((rho)*(a))+(1-(rho))*(b))

+#define ScalarProduct(x1,y1,x2,y2) ((x1)*(x2)+(y1)*(y2))
+
 #endif /* STD_H */
@@ -29,6 +29,8 @@ open LL
 module G2D = Geometry_2d
 open Printf

+type world = float * float
+
 let zoom_factor = 1.5 (* Mouse wheel zoom action *)
 let pan_step = 50 (* Pan keys speed *)

@@ -44,10 +46,10 @@ let distance = fun (x1,y1) (x2,y2) -> sqrt ((x1-.x2)**2.+.(y1-.y2)**2.)
      
 let _ = Srtm.add_path "SRTM"

-let affine_pos_and_angle xw yw angle =
+let affine_pos_and_angle ?(z = 1.) xw yw angle =
  let cos_a = cos angle in
  let sin_a = sin angle in
-  [| cos_a ; sin_a ; ~-. sin_a; cos_a; xw ; yw |]
+  [| cos_a /. z; sin_a /. z; ~-. sin_a /. z; cos_a /. z; xw ; yw |]

 type projection = 
    Mercator (* 1e-6 = 1 world unit, y axis reversed *)
@@ -123,7 +125,6 @@ class type geographic = object
 end
 	    

-
 (** basic canvas with menubar **************************************
 * (the vertical display in map2.ml is an instance of basic_widget)*
 *******************************************************************)
@@ -318,6 +319,10 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
    method geo_string = fun wgs84 ->
      LL.string_degrees_of_geographic wgs84

+
+    method move_item = fun (item:GnomeCanvas.re_p GnoCanvas.item) wgs84 ->
+      let (xw,yw) = self#world_of wgs84 in
+      item#affine_absolute (affine_pos_and_angle ~z:self#zoom_adj#value xw yw 0.);
    	    
    method moveto = fun wgs84 ->
      let (xw, yw) = self#world_of wgs84 in
@@ -166,6 +166,7 @@ class waypoint = fun (wpts_group:group) (name :string) ?(alt=0.) wgs84 ->
 	dialog#destroy () in
      ignore(cancel#connect#clicked ~callback:destroy);

+      (** Delete button for editable waypoints *)
      if editable then begin
 	let delete = GButton.button ~stock:`DELETE ~packing: dhbx#add () in 
 	let delete_callback = fun () ->
@@ -57,6 +57,10 @@ gaia : gaia.cmo
 	@echo OL $@
 	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $<

+diffusion : stdlib.cmo diffusion.cmo
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $^
+
 %.cmo : %.ml ../lib/ocaml/lib-pprz.cma
 	@echo OC $<
 	$(Q)$(OCAMLC) $(INCLUDES) -c $<
@@ -31,6 +31,8 @@ module Ground_Pprz = Pprz.Messages(struct let name = "ground" end)
 let ios = int_of_string
 let fos = float_of_string

+let raw_datalink_msg_separator = Str.regexp ";"
+
 module Make(A:Data.MISSION) = struct

  let servos_period = 1./.40. (* s *)
@@ -185,6 +187,17 @@ module Make(A:Data.MISSION) = struct
    if ac_id = !my_id then
      dl_setting (Pprz.int_assoc "index" vs) (Pprz.float_assoc "value" vs)

+  external set_wind : float -> float -> unit = "set_wind"
+  let get_raw_datalink = fun _sender vs ->
+    let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+    if ac_id = !my_id then
+      match Str.split raw_datalink_msg_separator (Pprz.string_assoc "message" vs) with
+	"WIND_INFO"::_::wind_east::wind_north::_ ->
+	  set_wind (fos wind_east) (fos wind_north)
+      | x::_ -> fprintf stderr "Sim: Warning, ingoring RAW_DATALINK '%s' message" x
+      | [] -> ()
+
+

  let boot = fun time_scale ->
    Stdlib.timer ~scale:time_scale servos_period (update_servos bat_button);
@@ -193,7 +206,8 @@ module Make(A:Data.MISSION) = struct
    ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" get_move_waypoint);
    ignore (Ground_Pprz.message_bind "SEND_EVENT" get_send_event);
    ignore (Ground_Pprz.message_bind "JUMP_TO_BLOCK" get_jump_to_block);
-    ignore (Ground_Pprz.message_bind "DL_SETTING" get_dl_setting)
+    ignore (Ground_Pprz.message_bind "DL_SETTING" get_dl_setting);
+    ignore (Ground_Pprz.message_bind "RAW_DATALINK" get_raw_datalink)

 (* Functions called by the simulator *)
  let commands = fun s -> rcommands := s
@@ -342,7 +342,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	      "0"
 	in
 	let at = try ExtXml.attrib x "approaching_time" with _ -> "CARROT" in
-	lprintf "if (approaching(%s,%s)) NextStageFrom(%s) else {\n" wp at wp;
+	lprintf "if (NavApproaching(%s,%s)) NextStageFrom(%s) else {\n" wp at wp;
 	right ();
 	let last_wp =
 	  try
@@ -427,8 +427,8 @@ let rec print_stage = fun index_of_waypoints x ->
    | "call" ->
 	stage ();
 	let statement = ExtXml.attrib  x "fun" in
-	lprintf "%s;\n" statement;
-	lprintf "NextStage();\n";
+	lprintf "if (! (%s))\n" statement;
+	lprintf "  NextStage();\n";
 	lprintf "return;\n"
    | "survey_rectangle" ->
 	let grid = parsed_attrib x "grid"