diff --git a/sw/in_progress/fdm/Makefile b/sw/in_progress/fdm/Makefile
index 5e51b2186d..0d620ca127 100644
--- a/sw/in_progress/fdm/Makefile
+++ b/sw/in_progress/fdm/Makefile
@@ -28,6 +28,12 @@ all: fdm_step
 fdm_step: fdm_step.c
 	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
 
-clean:
-	rm -f *~ core *.o *.bak .depend fdm_step
+
+fms_steps_attitude: fms_steps_attitude.c
+	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
+
+
+
+clean:
+	rm -f *~ core *.o *.bak .depend fdm_step fms_steps_attitude
 
diff --git a/sw/in_progress/fdm/booz2_fms_extern.h b/sw/in_progress/fdm/booz2_fms_extern.h
new file mode 100644
index 0000000000..fa14dca5fb
--- /dev/null
+++ b/sw/in_progress/fdm/booz2_fms_extern.h
@@ -0,0 +1,18 @@
+#ifndef BOOZ2_FMS_EXTERN_H
+#define BOOZ2_FMS_EXTERN_H
+
+#define FMS_H_MODE_RATE     0
+#define FMS_H_MODE_ATTITUDE 1
+#define FMS_H_MODE_SPEED    2
+#define FMS_H_MODE_POS      3
+
+#define FMS_ATTITUDE_OF_DEG(_d) ((int32_t)((_d)*0.0000546))
+
+#define FMS_V_MODE_DIRECT    0 
+#define FMS_V_MODE_CLIMB     1 
+#define FMS_V_MODE_HEIGHT    2 
+
+//#define BOOZ2_SEND_IVY_FMS_COMMAND
+
+
+#endif /* BOOZ2_FMS_EXTERN_H */
diff --git a/sw/in_progress/fdm/fdm_step.c b/sw/in_progress/fdm/fdm_step.c
index 764c05ab1f..1109bec08f 100644
--- a/sw/in_progress/fdm/fdm_step.c
+++ b/sw/in_progress/fdm/fdm_step.c
@@ -1,10 +1,6 @@
 /*
  * fdm_step
  *
- * send joystick control to paparazzi through ivy 
- *
- * based on Force Feedback: Constant Force Stress Test
- * Copyright (C) 2001 Oliver Hamann
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
diff --git a/sw/in_progress/fdm/fms_steps_attitude.c b/sw/in_progress/fdm/fms_steps_attitude.c
new file mode 100644
index 0000000000..b9af9fa5f5
--- /dev/null
+++ b/sw/in_progress/fdm/fms_steps_attitude.c
@@ -0,0 +1,92 @@
+/*
+ * fms_steps_attitude
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <glib.h>
+#include <Ivy/ivy.h>
+#include <Ivy/ivyglibloop.h>
+
+#include <inttypes.h>
+#include "booz2_fms_extern.h"
+
+
+#define TIMEOUT_PERIOD  100
+#define DEFAULT_AC_ID   150
+#define STEP_VAL 2
+#define STEP_PERIOD 3
+
+static int aircraft_id = DEFAULT_AC_ID;
+static int counter;
+
+static gboolean periodic(gpointer data __attribute__ ((unused)));
+
+
+void parse_args(int argc, char * argv[])
+{
+  int i;
+
+  for (i = 1; i < argc; i++) {
+    if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
+    else if (!strcmp(argv[i],"-h")) goto l_help;
+  }
+  return;
+
+l_help:
+  printf("Usage:\n");
+  printf("  %s <option> [<option>...]\n",argv[0]);
+  printf("Options:\n");
+  printf("  -a <int>     aircraft id (default: %d)\n",DEFAULT_AC_ID);
+  printf("  -h           display this help\n");
+  exit(1);
+}
+
+static gboolean periodic(gpointer data __attribute__ ((unused))) {
+  
+  counter++;
+
+  uint8_t h_mode = FMS_H_MODE_ATTITUDE; 
+  uint8_t v_mode = FMS_V_MODE_DIRECT; 
+  int32_t pitch  = FMS_ATTITUDE_OF_DEG(0);
+  int32_t roll   = FMS_ATTITUDE_OF_DEG(STEP_VAL);
+  int32_t yaw    = FMS_ATTITUDE_OF_DEG(0);
+  int32_t unused = 0;
+  if ((counter%(2*STEP_PERIOD)) >= STEP_PERIOD) roll = -roll;
+
+  IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d,%d,%d %d", aircraft_id, h_mode, v_mode, roll, pitch, yaw, unused);
+  return TRUE;
+}
+
+
+int main ( int argc, char** argv) {
+
+  GMainLoop *ml =  g_main_loop_new(NULL, FALSE);
+  
+  parse_args(argc, argv);
+  
+  IvyInit ("IvyBooz2FmsAttStep", "IvyBooz2FmsAttStep READY", NULL, NULL, NULL, NULL);
+  IvyStart("127.255.255.255");
+
+  g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
+  
+  g_main_loop_run(ml);
+
+  return 0;
+}
diff --git a/sw/in_progress/fdm/fms_steps_position.c b/sw/in_progress/fdm/fms_steps_position.c
new file mode 100644
index 0000000000..eee6e4499e
--- /dev/null
+++ b/sw/in_progress/fdm/fms_steps_position.c
@@ -0,0 +1,89 @@
+/*
+ * fdm_step_position
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <glib.h>
+#include <Ivy/ivy.h>
+#include <Ivy/ivyglibloop.h>
+
+#define TIMEOUT_PERIOD  100
+#define DEFAULT_AC_ID   150
+
+static int aircraft_id = DEFAULT_AC_ID;
+static int counter;
+
+static gboolean periodic(gpointer data __attribute__ ((unused)));
+static void on_FMS_info(IvyClientPtr app, void *user_data, int argc, char *argv[]);
+
+void parse_args(int argc, char * argv[])
+{
+  int i;
+
+  for (i = 1; i < argc; i++) {
+    if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
+    else if (!strcmp(argv[i],"-h")) goto l_help;
+  }
+  return;
+
+l_help:
+  printf("Usage:\n");
+  printf("  %s <option> [<option>...]\n",argv[0]);
+  printf("Options:\n");
+  printf("  -a <int>     aircraft id (default: %d)\n",DEFAULT_AC_ID);
+  printf("  -h           display this help\n");
+  exit(1);
+}
+
+static void on_FMS_state(IvyClientPtr app, void *user_data, int argc, char *argv[]){
+  //  guint ac_id = atoi(argv[0]);
+  //  float estimator_phi = atof(argv[1]);
+}
+
+
+
+#define STEP_VAL 32
+static gboolean periodic(gpointer data __attribute__ ((unused))) {
+  int roll, pitch;
+  counter++;
+  pitch = 0;
+  roll = STEP_VAL;
+  if ((counter%6) >=3) roll = -roll;
+    
+  IvySendMsg("dl COMMANDS_RAW %d %d,%d", aircraft_id, roll, pitch);
+  return TRUE;
+}
+
+
+int main ( int argc, char** argv) {
+
+  GMainLoop *ml =  g_main_loop_new(NULL, FALSE);
+  
+  parse_args(argc, argv);
+  
+  IvyInit ("IvyFdmStep", "IvyFdmStep READY", NULL, NULL, NULL, NULL);
+  IvyStart("127.255.255.255");
+
+  g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
+  
+  g_main_loop_run(ml);
+
+  return 0;
+}