diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml
index b056d2f95c..1091845db0 100644
--- a/sw/tools/gen_messages.ml
+++ b/sw/tools/gen_messages.ml
@@ -26,6 +26,9 @@
 
 open Printf
 
+let divide = fun a b -> b mod a = 0
+
+
 
 module Syntax = struct
   type format = string
@@ -95,21 +98,21 @@ module Syntax = struct
 	exit 1
 
   let of_xml = function
-    Element ("message", fields, l) ->
-      let name = assoc_or_fail "name" fields
-      and id = int_of_string (assoc_or_fail "id" fields) in
-      { id=id; name = name;
-	period = (try Some (float_of_string (List.assoc "period" fields)) with Not_found -> None);
-	fields=List.map (function
-	    Element ("field", fields, []) ->
-	      let id = assoc_or_fail "name" fields
-	      and type_name = assoc_or_fail "type" fields
-	      and fmt = try Some (List.assoc "format" fields) with _ -> None in
-	      let _type = try Array(type_name, int_of_string (List.assoc "len" fields)) with Not_found -> Basic type_name in
-		      
-	      (_type, id, fmt)
-	  | _ -> xml_error "field")
-	  l}
+      Element ("message", fields, l) ->
+	let name = assoc_or_fail "name" fields
+	and id = int_of_string (assoc_or_fail "id" fields) in
+	{ id=id; name = name;
+	  period = (try Some (float_of_string (List.assoc "period" fields)) with Not_found -> None);
+	  fields=List.map (function
+	      Element ("field", fields, []) ->
+		let id = assoc_or_fail "name" fields
+		and type_name = assoc_or_fail "type" fields
+		and fmt = try Some (List.assoc "format" fields) with _ -> None in
+		let _type = try Array(type_name, int_of_string (List.assoc "len" fields)) with Not_found -> Basic type_name in
+		
+		(_type, id, fmt)
+	    | _ -> xml_error "field")
+	    l}
     | _ -> xml_error "message with id"
 	  
 
@@ -150,7 +153,7 @@ module Gen_onboard = struct
     | (t, _, _)::fields -> size_fields fields (size + sizeof(t))
 
   let size_of_message = fun m -> size_fields m.fields 0    
-	  
+      
   let print_avr_macro = fun avr_h {name=s; fields = fields} ->
     fprintf avr_h "#define DOWNLINK_SEND_%s(" s;
     print_avr_macro_names avr_h fields;
@@ -187,60 +190,124 @@ module Gen_onboard = struct
     List.iter (print_null_avr_macro avr_h) messages
 
   let freq = 10
-  let buffer_length = 5
   let step = 1. /. float freq
   let nb_steps = (256 / freq) * freq
+  let byte_rate = 480 (** byte/s *)
+  let step_rate = byte_rate / freq
 
   let is_periodic = fun m -> m.period <> None
   let period_of = fun m ->
     match m.period with Some p -> p | None -> failwith "period_of"
   let morefrequent = fun m1 m2 -> compare (period_of m1) (period_of m2)
 
-  let gen_periodic = fun avr_h messages ->
-    let periodic_messages = List.filter is_periodic messages in
-    let periodic_messages = List.sort morefrequent periodic_messages in
+  type modulos = node list list (** Or list of Xor lists *)
+  and node = N of int * int * message list * modulos
+      (** [(p, s, msgs, c)] stands for messages [msg] to be send on a period [p]
+	 shifted [s] from the start of the [p] period (i.e. 
+	 when [time == s mod p]). Children [c] are messages for which the same
+	 time condition is needed *)
+      
+  let rec insert_in_or = fun ((s, p, m) as msg) t ->
+    match t with
+      [] -> [[N (s,p,[m], [])]]
+    | l::ls ->
+	let rec insert_in_xor = fun previous exclusive l ->
+	  match l with
+            [] -> 
+              if exclusive then
+		(N (s,p,[m], [])::previous)::ls
+              else
+		previous::insert_in_or msg ls
+	  | (N (s',p',ms',t') as x)::ls' ->
+              if s = s' && p == p' then
+		(previous@N (s', p', m::ms', t')::ls')::ls
+              else
+		let b = divide p' p in
+		if b && divide p' (abs (s-s')) then
+		  (previous@N (s', p', ms', insert_in_or msg t')::ls')::ls
+		else
+		  insert_in_xor (x::previous) (exclusive && b) ls' in
+	insert_in_xor [] true l;;
 
-    let load = Array.create nb_steps 0 in
-    let buffer_load = Array.create nb_steps 0 in
-    
-    let scheduled_messages = 
-      List.map
-	(fun m ->
-	  let p = period_of m in
-	  let period_steps = truncate (p /. step) in
-	  let start_step = ref 0 in
-	  for i = 1 to period_steps - 1 do
-	    if (load.(i), buffer_load.(i)) < (load.(!start_step), buffer_load.(!start_step)) then start_step := i
-	  done;
-	  let s = size_of_message m in
-	  for j = 0 to nb_steps/period_steps - 1 do
-	    let i = !start_step+j*period_steps in
-	    load.(i) <- load.(i) + s;
-	    for k = i to i + buffer_length - 1 do
-	      let k = (k + nb_steps) mod nb_steps in
-	      buffer_load.(k) <- buffer_load.(k) + s
-	    done
-	  done;
-	  (!start_step, period_steps, m))
-	periodic_messages in
-    
-    fprintf avr_h "// Load: intant(buffer)";
-    for i = 0 to nb_steps - 1 do
-      fprintf avr_h " %d(%d)" load.(i) buffer_load.(i)
-    done;
-    fprintf avr_h "\n";
- 
-    fprintf avr_h "#define PeriodicSend() {  /* %dHz */ \\\n" freq;
-    fprintf avr_h "  static uint8_t i;\\\n";
-    fprintf avr_h "  i++; if (i == %d) i = 0;\\\n" nb_steps;
+let gen_periodic = fun avr_h messages ->
+  let indent = fun x -> fprintf avr_h "%s" (String.make x ' ') in
+
+  let periodic_messages = List.filter is_periodic messages in
+  let periodic_messages = List.sort morefrequent periodic_messages in
+  
+  let load = Array.create nb_steps 0 in
+  
+  let scheduled_messages = 
+    List.map
+      (fun m ->
+	let p = period_of m in
+	let period_steps = truncate (p /. step) in
+	let start_step = ref 0 in
+	for i = 1 to period_steps - 1 do
+	  if load.(i) < load.(!start_step) then start_step := i
+	done;
+	for j = 0 to nb_steps/period_steps - 1 do
+	  let s = ref (size_of_message m) in
+	  let i = ref (!start_step+j*period_steps) in
+	  while !s > 0 do
+	    let rest = step_rate - load.(!i) in
+	    let m = min rest !s in
+	    load.(!i) <- load.(!i) + m;
+	    incr i;
+	    s := !s - m
+	  done
+	done;
+	(!start_step, period_steps, m))
+      periodic_messages in
+  
+  fprintf avr_h "/* Periodic messages are sent over %d timeframes. A greedy algorithm is used to smooth the load according to the size of the messages. speed=%d byte/timeframe */\n" nb_steps step_rate;
+  for i = 0 to nb_steps - 1 do
+    fprintf avr_h "//%d:" i; indent load.(i); fprintf avr_h "%d\n" load.(i)
+  done;
+  fprintf avr_h "\n";
+  
+  fprintf avr_h "#define PeriodicSend() {  /* %dHz */ \\\n" freq;
+  fprintf avr_h "  static uint8_t periodic_i;\\\n";
+  fprintf avr_h "  periodic_i++; if (periodic_i == %d) periodic_i = 0;\\\n" nb_steps;
+
+  (** Sorting messages by increasing periods *)
+  let scheduled_messages = List.sort (fun (_,p,_) (_,p',_) -> compare p' p) scheduled_messages in
+
+  (** Hierarchize messages to minimize tests *)
+  let tree = List.fold_right insert_in_or scheduled_messages [] in
+  
+  let rec gen_or_code = fun tab modulos xors ->
     List.iter
-      (fun (s, p, m) ->
-	fprintf avr_h "  if (i %% %d == %d) PERIODIC_SEND_%s();\\\n" p s m.name)
-      scheduled_messages;
-    fprintf avr_h "}\n"
-
-    
-	
+      (fun xor ->
+	let required_modulos = List.map (fun (N (s,p,ms,t)) -> p) xor in
+	let rec add_new_modulos = fun modulos l ->
+	  match l with
+	    [] -> modulos
+	  | p::ps ->
+	      if not (List.mem p modulos) then begin
+		indent tab;
+		fprintf avr_h "uint8_t i_mod_%d = periodic_i %% %d; \\\n" p p;
+		add_new_modulos (p::modulos) ps
+	      end else
+		add_new_modulos modulos ps in
+	let new_modulos = add_new_modulos modulos required_modulos in
+	List.iter 
+	  (fun (N (s,p,ms,t)) ->
+	    indent tab;
+	    fprintf avr_h "if (i_mod_%d == %d) { \\\n" p s;
+	    List.iter (fun m -> 
+	      indent tab;
+	      fprintf avr_h "  PERIODIC_SEND_%s(); \\\n" m.name) 
+	      ms;
+	    gen_or_code (tab+2) new_modulos t;
+	    indent tab;
+	    fprintf avr_h "} else")
+	  xor;
+	indent tab;
+	fprintf avr_h " {} \\\n")
+      xors in
+  gen_or_code 2 [] tree;
+  fprintf avr_h "}\n"
 end
 
 let _ =