[paparazzi-commits] [4762] add support for doubles in messages

[Pascal Brisset]

Revision: 4762
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4762
Author:   hecto
Date:     2010-03-29 17:05:48 +0000 (Mon, 29 Mar 2010)
Log Message:
-----------
 add support for doubles in messages

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/downlink.h
    paparazzi3/trunk/sw/airborne/pprz_transport.h
    paparazzi3/trunk/sw/airborne/sim/ivy_transport.h
    paparazzi3/trunk/sw/airborne/xbee.h
    paparazzi3/trunk/sw/lib/ocaml/convert.c
    paparazzi3/trunk/sw/lib/ocaml/pprz.ml
    paparazzi3/trunk/sw/tools/gen_messages.ml

Modified: paparazzi3/trunk/sw/airborne/downlink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/downlink.h     2010-03-29 15:57:49 UTC (rev 
4761)
+++ paparazzi3/trunk/sw/airborne/downlink.h     2010-03-29 17:05:48 UTC (rev 
4762)
@@ -95,6 +95,8 @@
 #define DownlinkPutUint32ByAddr(_chan, _x) Transport(_chan, 
PutUint32ByAddr(_x))
 #define DownlinkPutFloatByAddr(_chan, _x) Transport(_chan, PutFloatByAddr(_x))
 
+#define DownlinkPutDoubleByAddr(_chan, _x) Transport(_chan, 
PutDoubleByAddr(_x))
+
 #define DownlinkPutFloatArray(_chan, _n, _x) Transport(_chan, 
PutFloatArray(_n, _x))
 #define DownlinkPutInt16Array(_chan, _n, _x) Transport(_chan, 
PutInt16Array(_n, _x))
 #define DownlinkPutUint16Array(_chan, _n, _x) Transport(_chan, 
PutUint16Array(_n, _x))

Modified: paparazzi3/trunk/sw/airborne/pprz_transport.h
===================================================================
--- paparazzi3/trunk/sw/airborne/pprz_transport.h       2010-03-29 15:57:49 UTC 
(rev 4761)
+++ paparazzi3/trunk/sw/airborne/pprz_transport.h       2010-03-29 17:05:48 UTC 
(rev 4762)
@@ -94,7 +94,12 @@
     PprzTransportPut2ByteByAddr((const uint8_t*)_byte+2);      \
   }
 
+#define PprzTransportPut8ByteByAddr(_byte) { \
+    PprzTransportPut4ByteByAddr(_byte);        \
+    PprzTransportPut4ByteByAddr((const uint8_t*)_byte+4);      \
+  }
 
+
 #define PprzTransportPutInt8ByAddr(_x) PprzTransportPut1ByteByAddr(_x)
 #define PprzTransportPutUint8ByAddr(_x) PprzTransportPut1ByteByAddr((const 
uint8_t*)_x)
 #define PprzTransportPutInt16ByAddr(_x) PprzTransportPut2ByteByAddr((const 
uint8_t*)_x)
@@ -102,6 +107,7 @@
 #define PprzTransportPutInt32ByAddr(_x) PprzTransportPut4ByteByAddr((const 
uint8_t*)_x)
 #define PprzTransportPutUint32ByAddr(_x) PprzTransportPut4ByteByAddr((const 
uint8_t*)_x)
 #define PprzTransportPutFloatByAddr(_x) PprzTransportPut4ByteByAddr((const 
uint8_t*)_x)
+#define PprzTransportPutDoubleByAddr(_x) PprzTransportPut8ByteByAddr((const 
uint8_t*)_x)
 
 #define PprzTransportPutArray(_put, _n, _x) { \
   uint8_t _i; \

Modified: paparazzi3/trunk/sw/airborne/sim/ivy_transport.h
===================================================================
--- paparazzi3/trunk/sw/airborne/sim/ivy_transport.h    2010-03-29 15:57:49 UTC 
(rev 4761)
+++ paparazzi3/trunk/sw/airborne/sim/ivy_transport.h    2010-03-29 17:05:48 UTC 
(rev 4762)
@@ -28,6 +28,7 @@
 #define IvyTransportPutInt16ByAddr(x) IvyTransportPutIntByAddr(x) Space()
 #define IvyTransportPutInt32ByAddr(x) IvyTransportPutIntByAddr(x) Space()
 #define IvyTransportPutFloatByAddr(x) ivy_p += sprintf(ivy_p, "%f ", *x);
+#define IvyTransportPutDoubleByAddr(x) ivy_p += sprintf(ivy_p, "%f ", *x);
 
 #define IvyTransportPutArray(_put, _n, _x) { \
   int i; \

Modified: paparazzi3/trunk/sw/airborne/xbee.h
===================================================================
--- paparazzi3/trunk/sw/airborne/xbee.h 2010-03-29 15:57:49 UTC (rev 4761)
+++ paparazzi3/trunk/sw/airborne/xbee.h 2010-03-29 17:05:48 UTC (rev 4762)
@@ -95,9 +95,14 @@
 
 #define XBeeTransportPut4ByteByAddr(_byte) { \
     XBeeTransportPut2ByteByAddr(_byte);        \
-    XBeeTransportPut2ByteByAddr((const uint8_t*)_byte+2);      \
+    XBeeTransportPut2ByteByAddr((const uint8_t*)_byte+2); \
   }
 
+#define XBeeTransportPut8ByteByAddr(_byte) { \
+    XBeeTransportPut4ByteByAddr(_byte);        \
+    XBeeTransportPut4ByteByAddr((const uint8_t*)_byte+4); \
+  }
+
   
 #define XBeeTransportPutInt8ByAddr(_x) XBeeTransportPut1ByteByAddr(_x)
 #define XBeeTransportPutUint8ByAddr(_x) XBeeTransportPut1ByteByAddr((const 
uint8_t*)_x)
@@ -106,6 +111,7 @@
 #define XBeeTransportPutInt32ByAddr(_x) XBeeTransportPut4ByteByAddr((const 
uint8_t*)_x)
 #define XBeeTransportPutUint32ByAddr(_x) XBeeTransportPut4ByteByAddr((const 
uint8_t*)_x)
 #define XBeeTransportPutFloatByAddr(_x) XBeeTransportPut4ByteByAddr((const 
uint8_t*)_x)
+#define XBeeTransportPutDoubleByAddr(_x) XBeeTransportPut8ByteByAddr((const 
uint8_t*)_x)
 #define XBeeTransportPutNamedUint8(_name, _byte) XBeeTransportPutUint8(_byte)
 
 #define XBeeTransportPutArray(_put, _n, _x) { \

Modified: paparazzi3/trunk/sw/lib/ocaml/convert.c
===================================================================
--- paparazzi3/trunk/sw/lib/ocaml/convert.c     2010-03-29 15:57:49 UTC (rev 
4761)
+++ paparazzi3/trunk/sw/lib/ocaml/convert.c     2010-03-29 17:05:48 UTC (rev 
4762)
@@ -48,6 +48,13 @@
 }
 
 value
+c_sprint_double(value s, value index, value f) {
+  double *p = (double*) (String_val(s) + Int_val(index));
+  *p = Double_val(f);
+  return Val_unit;
+}
+
+value
 c_sprint_int16(value s, value index, value f) {
   int16_t *p = (int16_t*) (String_val(s) + Int_val(index));
   *p = (int16_t)Int_val(f);

Modified: paparazzi3/trunk/sw/lib/ocaml/pprz.ml
===================================================================
--- paparazzi3/trunk/sw/lib/ocaml/pprz.ml       2010-03-29 15:57:49 UTC (rev 
4761)
+++ paparazzi3/trunk/sw/lib/ocaml/pprz.ml       2010-03-29 17:05:48 UTC (rev 
4762)
@@ -79,6 +79,7 @@
 external int8_of_bytes : string -> int -> int = "c_int8_of_indexed_bytes"
 external int16_of_bytes : string -> int -> int = "c_int16_of_indexed_bytes"
 external sprint_float : string -> int -> float -> unit = "c_sprint_float"
+external sprint_double : string -> int -> float -> unit = "c_sprint_double"
 external sprint_int32 : string -> int -> int32 -> unit = "c_sprint_int32"
 external sprint_int16 : string -> int -> int -> unit = "c_sprint_int16"
 external sprint_int8 : string -> int -> int -> unit = "c_sprint_int8"
@@ -91,6 +92,7 @@
   ("int16",  { format = "%d";  glib_type = "gint16"; inttype = "int16_t";  
size = 2; value= Int 42 });
   ("int32",  { format = "%ld" ;  glib_type = "gint32"; inttype = "int32_t";  
size = 4; value=Int 42 });
   ("float",  { format = "%f" ;  glib_type = "gfloat"; inttype = "float";  size 
= 4; value=Float 4.2 });
+  ("double",  { format = "%f" ;  glib_type = "gdouble"; inttype = "double";  
size = 8; value=Float 4.2 });
   ("string",  { format = "%s" ;  glib_type = "gchar*"; inttype = "char*";  
size = max_int; value=String "42" })
 ]
 
@@ -110,7 +112,7 @@
   match t with 
     Scalar ("uint8" | "uint16" | "int8" | "int16") -> Int (int_of_string v)
   | Scalar ("uint32" | "int32") -> Int32 (Int32.of_string v)
-  | Scalar "float" -> Float (float_of_string v)
+  | Scalar ("float" | "double") -> Float (float_of_string v)
   | Scalar "string" -> String v
   | ArrayType t' ->
       Array (Array.map (value (Scalar t')) (Array.of_list (split_array v)))
@@ -267,6 +269,7 @@
        failwith (sprintf "Value too large to fit in a int8: %d" x);
       sprint_int8 buf i x; sizeof _type
   | Scalar "float", Float f -> sprint_float buf i f; sizeof _type
+  | Scalar "double", Float f -> sprint_double buf i f; sizeof _type
   | Scalar ("int32"|"uint32"), Int32 x -> sprint_int32 buf i x; sizeof _type
   | Scalar "int16", Int x -> sprint_int16 buf i x; sizeof _type
   | Scalar ("int32" | "uint32"), Int value ->

Modified: paparazzi3/trunk/sw/tools/gen_messages.ml
===================================================================
--- paparazzi3/trunk/sw/tools/gen_messages.ml   2010-03-29 15:57:49 UTC (rev 
4761)
+++ paparazzi3/trunk/sw/tools/gen_messages.ml   2010-03-29 17:05:48 UTC (rev 
4762)
@@ -59,7 +59,7 @@
       List.assoc t Pprz.types
     with
       Not_found -> 
-       failwith (sprintf "Error: '%s' unknown type\n" t)
+       failwith (sprintf "Error: '%s' unknown type" t)
 
   let rec sizeof = function
       Basic t -> string_of_int (assoc_types t).Pprz.size
@@ -221,6 +221,13 @@
        | 2 -> sprintf 
"(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8)" 
pprz_type.Pprz.inttype o o
        | 4 when pprz_type.Pprz.inttype = "float" -> 
            sprintf "({ union { uint32_t u; float f; } _f; _f.u = 
(uint32_t)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24);
 _f.f; })" o o o o
+       | 8 when pprz_type.Pprz.inttype = "double" ->
+           let s = ref (sprintf "*((uint8_t*)_payload+%d)" o) in
+           for i = 1 to 7 do
+             s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_payload+%d+%d))<<%d" 
o i (8*i)
+           done;
+
+           sprintf "({ union { uint64_t u; double f; } _f; _f.u = 
(uint64_t)(%s); _f.f; })" !s
        | 4 -> 
            sprintf 
"(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24)"
 pprz_type.Pprz.inttype o o o o
        | _ -> failwith "unexpected size in Gen_messages.print_get_macros" in