From 593bad39b8d36f18eb0c80f1c225b3678b5c3d77 Mon Sep 17 00:00:00 2001
From: Antoine Drouin <poinix@gmail.com>
Date: Sat, 22 Nov 2008 18:59:52 +0000
Subject: [PATCH] *** empty log message ***

---
 conf/airframes/booz2_a1.xml   |  4 +-
 sw/in_progress/fdm/Makefile   | 33 ++++++++++++++
 sw/in_progress/fdm/fdm_step.c | 85 +++++++++++++++++++++++++++++++++++
 3 files changed, 120 insertions(+), 2 deletions(-)
 create mode 100644 sw/in_progress/fdm/Makefile
 create mode 100644 sw/in_progress/fdm/fdm_step.c

diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml
index e94e479d0b..af1e962dce 100644
--- a/conf/airframes/booz2_a1.xml
+++ b/conf/airframes/booz2_a1.xml
@@ -122,8 +122,8 @@
  </section>
 
  <section name="JOYSTICK" prefix="BOOZ2_JOYSTICK_">
-   <define name="SP_PHI_COEF" value="2860"/> <!-- MAX SP = 20 deg ( RadOfDeg(20 / 128) << 20 ) -->
-   <define name="SP_THETA_COEF" value="2860"/> <!-- MAX SP = 20 deg ( RadOfDeg(20 / 128) << 20 ) -->
+   <define name="SP_PHI_COEF" value="1400"/> <!-- MAX SP = 20 deg ( RadOfDeg(20 / 128) << 20 ) -->
+   <define name="SP_THETA_COEF" value="1400"/> <!-- MAX SP = 20 deg ( RadOfDeg(20 / 128) << 20 ) -->
  </section>
 
  <section name="BAT">
diff --git a/sw/in_progress/fdm/Makefile b/sw/in_progress/fdm/Makefile
new file mode 100644
index 0000000000..5e51b2186d
--- /dev/null
+++ b/sw/in_progress/fdm/Makefile
@@ -0,0 +1,33 @@
+# 
+#   $Id$
+#   Copyright (C) 2004 Pascal Brisset, Antoine Drouin
+#
+# This file is part of paparazzi.
+#
+# paparazzi 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, or (at your option)
+# any later version.
+#
+# paparazzi 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 paparazzi; see the file COPYING.  If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.  
+# 
+
+# Quiet compilation
+Q=@
+
+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
+
diff --git a/sw/in_progress/fdm/fdm_step.c b/sw/in_progress/fdm/fdm_step.c
new file mode 100644
index 0000000000..764c05ab1f
--- /dev/null
+++ b/sw/in_progress/fdm/fdm_step.c
@@ -0,0 +1,85 @@
+/*
+ * 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
+ * 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       1
+
+/* Options */
+static int aircraft_id       = DEFAULT_AC_ID;
+static int counter;
+
+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);
+}
+
+#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;
+}