formatted modes

conf/control_panel.xml

@@ -93,6 +93,7 @@
     <session name="soft sim 1">
       <program name="receive"/>     
       <program name="sim"><arg flag="-a" constant="Thon1"/></program>
+      <program name="cockpit"/>
       <program name="map 2d ml"/>
       <program name="messages"><arg flag="-c" constant="telemetry_ap:1"/><arg flag="-c" constant="ground"/></program>
     </session>

conf/messages.xml

@@ -292,7 +292,7 @@
     <field name="east"    type="float" unit="m"/>
     <field name="north"   type="float" unit="m"/>
     <field name="speed"   type="float" unit="m/s"/>
-    <field name="course" type="float" unit="deg"/>
+    <field name="course" type="float" unit="deg" format="%.1f"/>
     <field name="alt"     type="float" unit="m"/>
     <field name="climb"   type="float" unit="m/s"/>
   </message>
@@ -302,8 +302,8 @@
     <field name="ap_mode" type="string" values="MANUAL|AUTO1|AUTO2|HOME"/>
     <field name="lat_mode" type="string" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
     <field name="gaz_mode" type="string" values="MANUAL|GAZ|CLIMB|ALT"/>
-    <field name="gps_mode" type="uint8"/>
-    <field name="flight_time" type="uint32"/>
+    <field name="gps_mode" type="string" />
+    <field name="flight_time" type="uint32" values="NOFIX|DRO|2D|3D|GPSDRO"/>
   </message>
 
   <message name="NAV_STATUS" ID="13">
@@ -321,16 +321,16 @@
 
   <message name="CAM_STATUS" ID="14">
      <field name="ac_id" type="string"/>
-     <field name="cam_east" type="float" unit="m"/>
-     <field name="cam_north" type="float" unit="m"/>
+     <field name="cam_east" type="float" unit="m" format="%.1f"/>
+     <field name="cam_north" type="float" unit="m" format="%.1f"/>
      <field name="target_east" type="float" unit="m"/>
      <field name="target_north" type="float" unit="m"/>
   </message>
 
   <message name="ENGINE_STATUS" ID="15">
     <field name="ac_id" type="string"/>
-    <field name="throttle" type="float" unit="%"/>
-    <field name="rpm"      type="float" unit="rpm"/>
+    <field name="throttle" type="float" unit="%"  format="%.1f"/>
+    <field name="rpm"      type="float" unit="rpm"  format="%.1f"/>
     <field name="temp"     type="float" unit="celcius"/>
     <field name="bat"      type="float" unit="V"/>
     <field name="amp"      type="float" unit="A"/>
@@ -363,7 +363,7 @@
 
   <message name="INFLIGH_CALIB" id="19">
     <field name="ac_id" type="string"/>
-    <field name="if_mode" type="uint8" values="DOWN|OFF|UP"></field>
+    <field name="if_mode" type="string" values="OFF|DOWN|UP"></field>
     <field name="if_value1" type="float"></field>
     <field name="if_value2" type="float"></field>
   </message>

sw/ground_segment/tmtc/receive.ml

@@ -41,10 +41,20 @@ let logs_path = Env.paparazzi_home // "var" // "logs"
 let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml")
 let srtm_path = Env.paparazzi_home // "data" // "srtm"
 
-(** Should be read from messages.xml *)
+
+let rec norm_course =
+  let _2pi = 2. *. Latlong.pi in
+  fun c ->
+    if c < 0. then norm_course (c +. _2pi)
+    else if c >= _2pi then norm_course (c -. _2pi)
+    else c
+
+(** FIXME: Should be read from messages.xml *)
 let ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME"|]
 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 check_index = fun i t where ->
   if i < 0 || i >= Array.length t then begin
@@ -189,10 +199,10 @@ let log_and_parse = fun log ac_name a msg values ->
 		   utm_y = fvalue "utm_north" /. 100.;
 		   utm_zone = ivalue "utm_zone" };
 	a.gspeed  <- fvalue "speed";
-	a.course  <- fvalue "course";
+	a.course  <- norm_course (fvalue "course");
 	a.alt     <- fvalue "alt";
 	a.climb   <- fvalue "climb";
-	a.gps_mode <- ivalue "mode"
+	a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE"
     | "DESIRED" ->
 	a.desired_east <- fvalue "desired_x";
 	a.desired_north <- fvalue "desired_y";
@@ -207,7 +217,7 @@ let log_and_parse = fun log ac_name a msg values ->
     | "NAVIGATION" -> 
 	a.cur_block <- ivalue "cur_block";
 	a.cur_stage <- ivalue "cur_stage";
-	a.desired_course <- fvalue "desired_course" /. 10.
+	a.desired_course <- norm_course ((Deg>>Rad)(fvalue "desired_course" /. 10.))
     | "BAT" ->
 	a.throttle <- fvalue "desired_gaz" /. 9600. *. 100.;
 	a.flight_time <- ivalue "flight_time";
@@ -219,7 +229,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.inflight_calib.if_mode <- ivalue "if_calib_mode";
+	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 *)
 	a.fbw.rc_status <- 
@@ -282,8 +292,9 @@ let send_cam_status = fun a ->
     Ground_Pprz.message_send my_id "CAM_STATUS" values
 
 let send_if_calib = fun a ->
+  let if_mode = get_indexed_value if_modes a.inflight_calib.if_mode in
   let values = ["ac_id", Pprz.String a.id;
-		"if_mode", Pprz.Int a.inflight_calib.if_mode;
+		"if_mode", Pprz.String if_mode;
 		"if_value1", Pprz.Float a.inflight_calib.if_val1;
 		"if_value2", Pprz.Float a.inflight_calib.if_val2] in
   Ground_Pprz.message_send my_id "INFLIGH_CALIB" values
@@ -353,7 +364,7 @@ let send_aircraft_msg = fun ac ->
 		  "target_north", f (a.nav_ref_north+.a.desired_north);
 		  "target_alt", Pprz.Float a.desired_altitude;
 		  "target_climb", Pprz.Float a.desired_climb;
-		  "target_course", Pprz.Float a.desired_course
+		  "target_course", Pprz.Float ((Rad>>Deg)a.desired_course)
 		] in
     Ground_Pprz.message_send my_id "NAV_STATUS" values;
 
@@ -369,12 +380,13 @@ 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 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;
-		  "gps_mode", Pprz.Int a.gps_mode] in
+		  "gps_mode", Pprz.String gps_mode] in
     Ground_Pprz.message_send my_id "AP_STATUS" values;
 
     send_cam_status a;

sw/lib/ocaml/pprz.ml

@@ -93,6 +93,15 @@ let string_of_value = function
   | Int32 x -> Int32.to_string x
   | String s -> s
 
+
+let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format)
+
+
+let formatted_string_of_value = fun format v ->
+  match v with
+    Float x -> sprintf (magic format) x
+  | v -> string_of_value v
+
 let size_of_field = fun f -> (List.assoc f._type types).size
 let default_format = fun x -> try (List.assoc x types).format with Not_found -> failwith (sprintf "Unknwon format '%s'" x)
 let default_value = fun x -> (List.assoc x types).value
@@ -149,10 +158,7 @@ let lazy_classes =
 
 let classes = fun () -> Lazy.force lazy_classes
     
-let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format)
-
 let value_field = fun buffer index (field:field) ->
-  let format = field.fformat in
   match field._type with
     "uint8" -> Int (Char.code buffer.[index])
   | "int8" -> Int (int8_of_bytes buffer index)
@@ -258,8 +264,11 @@ module Protocol(Class:CLASS) = struct
       (msg.name::
        List.map 
 	 (fun (field_name, field) ->
-	   try string_of_value (List.assoc field_name values) with
-	     Not_found -> string_of_value (default_value field._type))
+	   let v =
+	     try List.assoc field_name values with
+	       Not_found ->
+		 default_value field._type in
+	   formatted_string_of_value field.fformat v)
 	 msg.fields)
 
   let message_send = fun sender msg_name values ->