diff --git a/conf/airframes/microjet5.xml b/conf/airframes/microjet5.xml
index 989cd08d56..3c95cefaff 100644
--- a/conf/airframes/microjet5.xml
+++ b/conf/airframes/microjet5.xml
@@ -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
diff --git a/conf/airframes/microjet7.xml b/conf/airframes/microjet7.xml
index 6200a4d41f..da06eb410c 100644
--- a/conf/airframes/microjet7.xml
+++ b/conf/airframes/microjet7.xml
@@ -108,6 +108,7 @@
+
+
+
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
diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml
index 727ba9b8b6..8f2a450405 100644
--- a/conf/airframes/twinjet1.xml
+++ b/conf/airframes/twinjet1.xml
@@ -184,6 +184,7 @@
diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml
new file mode 100644
index 0000000000..9bb2929d79
--- /dev/null
+++ b/conf/flight_plans/IS.xml
@@ -0,0 +1,117 @@
+
+
+
+
+#include "anemotaxis.h"
+#include "chemotaxis.h"
+#include "discsurvey.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index 7f1ab802b7..dc710d4b98 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -624,6 +624,13 @@
+
+
+
+
+
+
+
diff --git a/sw/airborne/anemotaxis.c b/sw/airborne/anemotaxis.c
new file mode 100644
index 0000000000..e27e10f9e1
--- /dev/null
+++ b/sw/airborne/anemotaxis.c
@@ -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;
+}
diff --git a/sw/airborne/anemotaxis.h b/sw/airborne/anemotaxis.h
new file mode 100644
index 0000000000..dfda8f80eb
--- /dev/null
+++ b/sw/airborne/anemotaxis.h
@@ -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);
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 3e061f30f3..1952652164 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -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
diff --git a/sw/airborne/chemotaxis.c b/sw/airborne/chemotaxis.c
new file mode 100644
index 0000000000..98ccd0adf4
--- /dev/null
+++ b/sw/airborne/chemotaxis.c
@@ -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;
+}
diff --git a/sw/airborne/chemotaxis.h b/sw/airborne/chemotaxis.h
new file mode 100644
index 0000000000..b3fc5b8764
--- /dev/null
+++ b/sw/airborne/chemotaxis.h
@@ -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 );
diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index 5ed7658535..fa41454514 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -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();
}
diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h
index fa7f3e50e6..3238050b17 100644
--- a/sw/airborne/nav.h
+++ b/sw/airborne/nav.h
@@ -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 */
diff --git a/sw/airborne/sim/sim_ap.c b/sw/airborne/sim/sim_ap.c
index 8f3264d248..fba8c41b88 100644
--- a/sw/airborne/sim/sim_ap.c
+++ b/sw/airborne/sim/sim_ap.c
@@ -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;
+}
diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile
index 2d8400df60..7869a61a05 100644
--- a/sw/ground_segment/cockpit/Makefile
+++ b/sw/ground_segment/cockpit/Makefile
@@ -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)
diff --git a/sw/ground_segment/cockpit/editFP.mli b/sw/ground_segment/cockpit/editFP.mli
index 765d7afb94..d3d7bba007 100644
--- a/sw/ground_segment/cockpit/editFP.mli
+++ b/sw/ground_segment/cockpit/editFP.mli
@@ -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
diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml
index 45107127f5..37ca50392a 100644
--- a/sw/ground_segment/cockpit/gcs.ml
+++ b/sw/ground_segment/cockpit/gcs.ml
@@ -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 ()
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index cd23302a88..4086ab9401 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -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)
diff --git a/sw/ground_segment/cockpit/live.mli b/sw/ground_segment/cockpit/live.mli
index 5d6141312a..a577f610c2 100644
--- a/sw/ground_segment/cockpit/live.mli
+++ b/sw/ground_segment/cockpit/live.mli
@@ -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 *)
diff --git a/sw/include/std.h b/sw/include/std.h
index 3178798229..6d9743fb41 100644
--- a/sw/include/std.h
+++ b/sw/include/std.h
@@ -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 */
diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml
index a7dac5d57f..f298652ba9 100644
--- a/sw/lib/ocaml/mapCanvas.ml
+++ b/sw/lib/ocaml/mapCanvas.ml
@@ -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
diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index c3764d462f..9f5ce09955 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -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 () ->
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 1506bfe6ac..58a00eb229 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -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 $<
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index e8568c7267..d720ead913 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -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
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index 1aa235031e..6fba948429 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -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"