CIRCLE, SEGEMENT STATUS, id unicity control

conf/conf.xml

@@ -3,14 +3,14 @@
 <conf>
   <aircraft 
     name="Plaster1"
-    ac_id="1"
+    ac_id="4"
     airframe="airframes/plaster1.xml" 
     radio="radios/mc3030.xml"
     flight_plan="flight_plans/ricou_pla.xml"
   />
   <aircraft 
     name="Twinstar3"
-    ac_id="1"
+    ac_id="3"
     airframe="airframes/twinstar3.xml" 
     radio="radios/mc3030.xml" 
     flight_plan="flight_plans/ricou_pla.xml"
@@ -41,7 +41,7 @@
 
   <aircraft 
     name="Microjet"
-    ac_id="1"
+    ac_id="5"
     airframe="airframes/microjet1.xml" 
     radio="radios/mc3030.xml" 
     flight_plan="flight_plans/muret_mini.xml"
@@ -50,7 +50,7 @@
  
   <aircraft 
     name="Gorazoptere_brushless_3DMG"
-    ac_id="1"
+    ac_id="6"
     airframe="airframes/gorazoptere_brushless_3DMG.xml" 
     radio="radios/mc3030.xml" 
     flight_plan="flight_plans/muret_mini.xml"
@@ -58,7 +58,7 @@
 
   <aircraft 
     name="Gorazoptere_brushless_ANALOG"
-    ac_id="1"
+    ac_id="7"
     airframe="airframes/gorazoptere_brushless_ANALOG.xml" 
     radio="radios/mc3030.xml" 
     flight_plan="flight_plans/muret_mini.xml"

conf/control_panel.xml

@@ -110,7 +110,7 @@
     <session name="flight microjet modem">
       <program name="modem"><arg flag="-d" constant="/dev/ttyS0"/></program>
       <program name="receive"/>     
-      <program name="messages"><arg flag="-c" constant="telemetry_ap:1"/><arg flag="-c" constant="ground"/></program>
+      <program name="messages"><arg flag="-c" constant="telemetry_ap:5"/><arg flag="-c" constant="ground"/></program>
     </session>
 
    <session name="flight microjet-plaster">
@@ -121,14 +121,14 @@
       <program name="messages"><arg flag="-c" constant="telemetry_ap:1"/><arg flag="-c" constant="telemetry_ap:3"/><arg flag="-c" constant="ground"/></program>
     </session>
   
-   <session name="flight microjet multimon">
+   <session name="flight multimon">
       <variable name="map"  value="ricouUTM.xml"/>
       <variable name="ivy_bus" value="127:2010"/>
       <program name="http server"/>     
       <program name="multimon"><arg flag="-p" constant="/tmp/multimon"/></program>
       <program name="modem"><arg flag="-d" constant="/tmp/multimon"/></program>
       <program name="receive"/>     
-      <program name="messages"><arg flag="-c" constant="telemetry_ap:1"/><arg flag="-c" constant="ground"/></program>
+      <program name="messages"><arg flag="-c" constant="telemetry_ap:3"/><arg flag="-c" constant="ground"/></program>
       <program name="cockpit"/>
       <program name="map 2d ml"/>
     </session>

conf/messages.xml

@@ -142,16 +142,16 @@
   </message>
 	
   <message name="CIRCLE" ID="21">
-     <field name="center_x" type="int16" unit="m"/>
-     <field name="center_y" type="int16" unit="m"/>
+     <field name="center_east" type="int16" unit="m"/>
+     <field name="center_north" type="int16" unit="m"/>
      <field name="radius" type="int16" unit="m"/>
      </message>
 	
    <message name="SEGMENT" ID="22">
-      <field name="segment_x_1" type="int16" unit="m"/>
-      <field name="segment_y_1" type="int16" unit="m"/>
-      <field name="segment_x_2" type="int16" unit="m"/>
-      <field name="segment_y_2" type="int16" unit="m"/>
+      <field name="segment_east_1" type="int16" unit="m"/>
+      <field name="segment_north_1" type="int16" unit="m"/>
+      <field name="segment_east_2" type="int16" unit="m"/>
+      <field name="segment_north_2" type="int16" unit="m"/>
   </message>
 
   <message name="DOWNLINK_STATUS" ID="23">

sw/ground_segment/tmtc/receive.ml

@@ -55,6 +55,7 @@ let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
 let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
 let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|]
 let if_modes = [|"OFF";"DOWN";"UP"|]
+let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE"|]
 
 let check_index = fun i t where ->
   if i < 0 || i >= Array.length t then begin
@@ -112,13 +113,17 @@ let svinfo_init = {
     azim = 0;
   }
 
+type horiz_mode = 
+    Circle of utm * int
+  | Segment of utm * utm
+  | UnknownHorizMode
+
 type aircraft = { 
     id : string;
     mutable pos : Latlong.utm;
     mutable roll    : float;
     mutable pitch   : float;
-    mutable nav_ref_east    : float;
-    mutable nav_ref_north    : float;
+    mutable nav_ref    : Latlong.utm;
     mutable desired_east    : float;
     mutable desired_north    : float;
     mutable desired_altitude    : float;
@@ -139,6 +144,7 @@ type aircraft = {
     mutable ap_mode : int;
     mutable gaz_mode : int;
     mutable lateral_mode : int;
+    mutable horizontal_mode : int;
     cam : ac_cam;
     mutable gps_mode : int;
     inflight_calib : inflight_calib;
@@ -147,7 +153,8 @@ type aircraft = {
     svinfo : svinfo array;
     mutable flight_time : int;
     mutable stage_time : int;
-    mutable block_time : int
+    mutable block_time : int;
+    mutable horiz_mode : horiz_mode
   }
 
 (** The aircrafts store *)
@@ -242,8 +249,7 @@ let log_and_parse = fun log ac_name a msg values ->
 	a.desired_altitude <- fvalue "desired_altitude";
 	a.desired_climb <- fvalue "desired_climb"
     | "NAVIGATION_REF" ->
-	a.nav_ref_east <- fvalue "utm_east";
-	a.nav_ref_north <- fvalue "utm_north"
+	a.nav_ref <- { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = a.pos.utm_zone }
     | "ATTITUDE" ->
 	a.roll <- (Deg>>Rad) (fvalue "phi");
 	a.pitch <- (Deg>>Rad) (fvalue "theta")
@@ -262,6 +268,7 @@ let log_and_parse = fun log ac_name a msg values ->
 	a.ap_mode <- check_index (ivalue "ap_mode") ap_modes "AP_MODE";
 	a.gaz_mode <- check_index (ivalue "ap_gaz") gaz_modes "AP_GAZ";
 	a.lateral_mode <- check_index (ivalue "ap_lateral") lat_modes "AP_LAT";
+	a.horizontal_mode <- check_index (ivalue "ap_horizontal") horiz_modes "AP_HORIZ";
 	a.inflight_calib.if_mode <- check_index (ivalue "if_calib_mode") if_modes "IF_MODE";
 	let mcu1_status = ivalue "mcu1_status" in
 	(** c.f. link_autopilot.h *)
@@ -302,6 +309,12 @@ let log_and_parse = fun log ac_name a msg values ->
 	  elev = ivalue "Elev";
 	  azim = ivalue "Azim";
 	}
+    | "CIRCLE" ->
+	a.horiz_mode <- Circle (Latlong.utm_add a.nav_ref (fvalue "center_east") (fvalue "center_north"), ivalue "radius")
+    | "SEGMENT" ->
+	let p1 = Latlong.utm_add a.nav_ref (fvalue "segment_east_1") (fvalue "segment_north_1")
+	and p2 = Latlong.utm_add a.nav_ref (fvalue "segment_east_2") (fvalue "segment_north_2") in
+	a.horiz_mode <- Segment (p1, p2)
     | _ -> ()
 
 (** Callback for a message from a registered A/C *)
@@ -374,6 +387,22 @@ let send_svsinfo = fun a ->
 	    f "cno" cno; f "elev" elev; f "azim" azim] in
   Ground_Pprz.message_send my_id "SVSINFO" vs
 
+let send_horiz_status = fun a ->
+  match a.horiz_mode with
+    Circle (utm, r) ->
+      let vs = [ "ac_id", Pprz.String a.id; 
+		 "circle_east", Pprz.Float utm.utm_x;
+		 "circle_north", Pprz.Float utm.utm_y;
+		 "radius", Pprz.Int r ] in
+       Ground_Pprz.message_send my_id "CIRCLE_STATUS" vs
+  | Segment (u1, u2) ->
+      let vs = [ "ac_id", Pprz.String a.id; 
+		 "segment1_east", Pprz.Float u1.utm_x;
+		 "segment1_north", Pprz.Float u1.utm_y;
+		 "segment2_east", Pprz.Float u2.utm_x;
+		 "segment2_north", Pprz.Float u2.utm_y ] in
+      Ground_Pprz.message_send my_id "SEGMENT_STATUS" vs
+  | UnknownHorizMode -> ()
 
 
 let send_aircraft_msg = fun ac ->
@@ -397,8 +426,8 @@ let send_aircraft_msg = fun ac ->
 		  "cur_stage", Pprz.Int a.cur_stage;
 		  "stage_time", Pprz.Int a.stage_time;
 		  "block_time", Pprz.Int a.block_time;
-		  "target_east", f (a.nav_ref_east+.a.desired_east);
-		  "target_north", f (a.nav_ref_north+.a.desired_north);
+		  "target_east", f (a.nav_ref.utm_x+.a.desired_east);
+		  "target_north", f (a.nav_ref.utm_y+.a.desired_north);
 		  "target_alt", Pprz.Float a.desired_altitude;
 		  "target_climb", Pprz.Float a.desired_climb;
 		  "target_course", Pprz.Float ((Rad>>Deg)a.desired_course)
@@ -417,12 +446,14 @@ let send_aircraft_msg = fun ac ->
     let ap_mode = get_indexed_value ap_modes a.ap_mode in
     let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
     let lat_mode = get_indexed_value lat_modes a.lateral_mode in
+    let horiz_mode = get_indexed_value horiz_modes a.horizontal_mode in
     let gps_mode = get_indexed_value gps_modes a.gps_mode in
     let values = ["ac_id", Pprz.String ac; 
 		  "flight_time", Pprz.Int a.flight_time;
 		  "ap_mode", Pprz.String ap_mode; 
 		  "gaz_mode", Pprz.String gaz_mode;
 		  "lat_mode", Pprz.String lat_mode;
+		  "horiz_mode", Pprz.String horiz_mode;
 		  "gps_mode", Pprz.String gps_mode] in
     Ground_Pprz.message_send my_id "AP_STATUS" values;
 
@@ -430,13 +461,14 @@ let send_aircraft_msg = fun ac ->
     send_if_calib a;
     send_fbw a;
     send_infrared a;
-    send_svsinfo a
+    send_svsinfo a;
+    send_horiz_status a
   with
     Not_found -> prerr_endline ac
   | x -> prerr_endline (Printexc.to_string x)
       
 let new_aircraft = fun id ->
-    { id = id ; roll = 0.; pitch = 0.; nav_ref_east = 0.; nav_ref_north = 0.; desired_east = 0.; desired_north = 0.; 
+    { id = id ; roll = 0.; pitch = 0.; desired_east = 0.; desired_north = 0.; 
       desired_course = 0.;
 	gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode= -1;
       gaz_mode= -1; lateral_mode= -1;
@@ -444,12 +476,15 @@ let new_aircraft = fun id ->
       desired_altitude = 0.;
       desired_climb = 0.;
       pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
+      nav_ref = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
       cam = { phi = 0.; theta = 0. };
       inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
       infrared = infrared_init;
       fbw = { rc_status = "???"; rc_mode = "???" };
       svinfo = Array.create gps_nb_channels svinfo_init;
-      flight_time = 0; stage_time = 0; block_time = 0
+      flight_time = 0; stage_time = 0; block_time = 0;
+      horiz_mode = UnknownHorizMode;
+      horizontal_mode = 0
     }
     
 let register_aircraft = fun name a ->

sw/lib/ocaml/latlong.ml

@@ -334,6 +334,9 @@ let of_cartesian ellips {x=x;y=y;z=z} =
 let utm_distance = fun utm1 utm2 ->
   if utm1.utm_zone <> utm2.utm_zone then invalid_arg "utm_distance";
   sqrt ((utm1.utm_x -. utm2.utm_x)**2. +. (utm1.utm_y -. utm2.utm_y)**2.)
+
+let utm_add = fun u x y ->
+  {utm_x = u.utm_x +. x; utm_y = u.utm_y +. y; utm_zone = u.utm_zone }
   
 let wgs84_of_lambertIIe = fun x y -> (WGS84<<NTF)(of_lambert lambertIIe {lbt_x = x; lbt_y = y})
 

sw/lib/ocaml/latlong.mli

@@ -107,6 +107,9 @@ geographic coordinates and altitude expressed in geodesic referential
 
 val utm_distance : utm -> utm -> fmeter 
 
+val utm_add : utm -> fmeter -> fmeter -> utm
+(** [add_utm utm east north] *)
+
 val wgs84_of_lambertIIe : meter -> meter -> geographic
 
 val of_string : string -> geographic

sw/tools/gen_aircraft.ml

@@ -9,11 +9,23 @@ let mkdir = fun d ->
   if not (Sys.file_exists d) then
     Unix.mkdir d 0o755
 
+let check_unique_id = fun conf ->
+  let ids = Hashtbl.create 5 in
+  List.iter
+    (fun x -> 
+      if String.lowercase (Xml.tag x) = "aircraft" then 
+	let id = ExtXml.attrib x "ac_id" in
+	if Hashtbl.mem ids id then
+	  failwith (sprintf "Error: A/C Id '%s' duplicated in %s" id conf_xml);
+	Hashtbl.add ids id ())
+    (Xml.children conf)
+
 let _ =
   if Array.length Sys.argv <> 2 then
     failwith (sprintf "Usage: %s <A/C ident (conf.xml)>" Sys.argv.(0));
   let aircraft = Sys.argv.(1) in
   let conf = Xml.parse_file conf_xml in
+  check_unique_id conf;
   let aircraft_xml =
     try
       ExtXml.child conf ~select:(fun x -> Xml.attrib x "name" = aircraft) "aircraft"