From 5705689bb3e7fd62270048b189eba4682108b9f9 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 8 Oct 2012 18:43:39 +0200
Subject: [PATCH 01/26] [actuators] start to rework actuators

- use multiple drivers at the same time
- all actuators firmware independant
- default actuators can still be defined for some boards
- only lpc part modified yet
- for now microjet example is compiling...
---
 conf/airframes/airframe.dtd                   | 24 ++------
 .../subsystems/shared/actuators_4015.makefile |  4 +-
 .../subsystems/shared/actuators_4017.makefile |  4 +-
 .../shared/actuators_direct.makefile          |  4 +-
 .../subsystems/shared/actuators_ppm.makefile  |  4 +-
 .../arch/lpc21/mcu_periph/sys_time_arch.c     | 10 +++-
 sw/airborne/arch/lpc21/servos_csc.h           | 20 -------
 .../subsystems/actuators/actuators_pwm_arch.c |  2 +-
 .../subsystems/actuators/actuators_pwm_arch.h |  7 ++-
 .../actuators}/servos_4015_MAT_hw.c           |  6 +-
 .../actuators}/servos_4015_MAT_hw.h           |  6 +-
 .../actuators}/servos_4015_hw.c               | 25 +++++++-
 .../actuators}/servos_4015_hw.h               |  6 +-
 .../actuators}/servos_4015_hw_new.c           | 25 +++++++-
 .../actuators}/servos_4017_hw.c               | 25 +++++++-
 .../actuators}/servos_4017_hw.h               |  6 +-
 .../actuators}/servos_ppm_hw.c                |  4 +-
 .../actuators}/servos_ppm_hw.h                |  6 +-
 sw/airborne/boards/tiny_0.99.h                |  6 ++
 sw/airborne/boards/tiny_1.1.h                 |  6 ++
 sw/airborne/boards/tiny_2.0.h                 |  6 ++
 sw/airborne/boards/tiny_2.1.h                 |  6 ++
 sw/airborne/boards/tiny_sim.h                 |  6 ++
 sw/airborne/boards/umarim_1.0.h               |  6 ++
 sw/airborne/boards/umarim_lite_2.0.h          |  6 ++
 .../firmwares/fixedwing/fbw_downlink.h        |  4 +-
 sw/airborne/firmwares/fixedwing/main_fbw.c    |  2 +-
 sw/airborne/firmwares/setup/setup_actuators.c |  6 +-
 sw/airborne/{ => subsystems}/actuators.c      | 11 +++-
 .../fixedwing => subsystems}/actuators.h      | 26 ++++----
 .../subsystems/actuators/actuators_4015.h     | 27 +++++++++
 .../subsystems/actuators/actuators_4017.h     | 27 +++++++++
 .../actuators/actuators_asctec.c              |  0
 .../actuators/actuators_asctec.h              |  0
 .../actuators/actuators_default.h}            | 30 ++++------
 .../actuators/actuators_dummy.c               |  0
 .../actuators/actuators_heli.c                |  0
 .../actuators/actuators_mkk.c                 |  0
 .../actuators/actuators_mkk.h                 |  0
 .../subsystems/actuators/actuators_ppm.h      | 27 +++++++++
 .../actuators/actuators_pwm_supervision.c     |  0
 .../actuators/actuators_skiron.c              |  0
 .../actuators/actuators_skiron.h              |  0
 .../actuators/supervision.c                   |  0
 .../actuators/supervision.h                   |  0
 sw/tools/gen_airframe.ml                      | 59 +++++--------------
 46 files changed, 294 insertions(+), 155 deletions(-)
 delete mode 100644 sw/airborne/arch/lpc21/servos_csc.h
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_4015_MAT_hw.c (96%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_4015_MAT_hw.h (92%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_4015_hw.c (77%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_4015_hw.h (83%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_4015_hw_new.c (82%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_4017_hw.c (64%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_4017_hw.h (93%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_ppm_hw.c (96%)
 rename sw/airborne/arch/lpc21/{ => subsystems/actuators}/servos_ppm_hw.h (94%)
 rename sw/airborne/{ => subsystems}/actuators.c (84%)
 rename sw/airborne/{firmwares/fixedwing => subsystems}/actuators.h (71%)
 create mode 100644 sw/airborne/subsystems/actuators/actuators_4015.h
 create mode 100644 sw/airborne/subsystems/actuators/actuators_4017.h
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_asctec.c (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_asctec.h (100%)
 rename sw/airborne/{firmwares/rotorcraft/actuators.h => subsystems/actuators/actuators_default.h} (51%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_dummy.c (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_heli.c (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_mkk.c (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_mkk.h (100%)
 create mode 100644 sw/airborne/subsystems/actuators/actuators_ppm.h
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_pwm_supervision.c (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_skiron.c (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/actuators_skiron.h (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/supervision.c (100%)
 rename sw/airborne/{firmwares/rotorcraft => subsystems}/actuators/supervision.h (100%)

diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd
index 9a1e5c36b6..5950084218 100644
--- a/conf/airframes/airframe.dtd
+++ b/conf/airframes/airframe.dtd
@@ -1,21 +1,18 @@
 <!-- Paparazzi airframe DTD -->
 
-<!ELEMENT airframe (include|servos|commands|csc_boards|rc_commands|auto_rc_commands|ap_only_commands|command_laws|section|makefile|modules|firmware)*>
+<!ELEMENT airframe (include|servos|commands|rc_commands|auto_rc_commands|ap_only_commands|command_laws|section|makefile|modules|firmware)*>
 <!ELEMENT include EMPTY>
 <!ELEMENT servos (servo)*>
 <!ELEMENT commands (axis)*>
-<!ELEMENT csc_boards (board)*>
-<!ELEMENT board (msg)*>
-<!ELEMENT msg (field_map)*>
-<!ELEMENT field_map EMPTY>
 <!ELEMENT rc_commands (set)*>
 <!ELEMENT auto_rc_commands (set)*>
 <!ELEMENT ap_only_commands (copy)*>
-<!ELEMENT command_laws (let|set|ratelimit)*>
+<!ELEMENT command_laws (let|set|call|ratelimit)*>
 <!ELEMENT section (define|linear)*>
 <!ELEMENT servo EMPTY>
 <!ELEMENT axis EMPTY>
 <!ELEMENT set EMPTY>
+<!ELEMENT call EMPTY>
 <!ELEMENT ratelimit EMPTY>
 <!ELEMENT copy EMPTY>
 <!ELEMENT let EMPTY>
@@ -54,7 +51,6 @@ driver CDATA #IMPLIED>
 <!ATTLIST rc_commands>
 <!ATTLIST ap_only_commands>
 <!ATTLIST command_laws>
-<!ATTLIST csc_boards>
 
 <!ATTLIST section
 name CDATA #IMPLIED
@@ -67,17 +63,6 @@ min CDATA #REQUIRED
 neutral CDATA #REQUIRED
 max CDATA #REQUIRED>
 
-<!ATTLIST board
-id CDATA #REQUIRED>
-
-<!ATTLIST msg
-id CDATA #REQUIRED
-type CDATA #REQUIRED>
-
-<!ATTLIST field_map
-field CDATA #REQUIRED
-servo_id CDATA #REQUIRED>
-
 <!ATTLIST axis
 name CDATA #REQUIRED
 failsafe_value CDATA #REQUIRED>
@@ -87,6 +72,9 @@ value CDATA #REQUIRED
 command CDATA #IMPLIED
 servo CDATA #IMPLIED>
 
+<!ATTLIST call
+fun CDATA #REQUIRED>
+
 <!ATTLIST copy
 command CDATA #REQUIRED>
 
diff --git a/conf/firmwares/subsystems/shared/actuators_4015.makefile b/conf/firmwares/subsystems/shared/actuators_4015.makefile
index 6451c757b0..cc1951717a 100644
--- a/conf/firmwares/subsystems/shared/actuators_4015.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_4015.makefile
@@ -1,4 +1,4 @@
 # for Tiny v1.1
 
-$(TARGET).CFLAGS 	+= -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
-$(TARGET).srcs 	+= $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
+$(TARGET).CFLAGS 	+= -DACTUATORS -DSERVOS_4015_MAT
+$(TARGET).srcs 	+= $(SRC_ARCH)/subsystems/actuators/servos_4015_MAT_hw.c subsystems/actuators.c
diff --git a/conf/firmwares/subsystems/shared/actuators_4017.makefile b/conf/firmwares/subsystems/shared/actuators_4017.makefile
index 471e3c3433..7750d2a992 100644
--- a/conf/firmwares/subsystems/shared/actuators_4017.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_4017.makefile
@@ -1,4 +1,4 @@
 # for Tiny v2 or Twog v1
 
-$(TARGET).CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017
-$(TARGET).srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
+$(TARGET).CFLAGS += -DACTUATORS -DSERVOS_4017
+$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/servos_4017_hw.c subsystems/actuators.c
diff --git a/conf/firmwares/subsystems/shared/actuators_direct.makefile b/conf/firmwares/subsystems/shared/actuators_direct.makefile
index 019bb6fac2..21ed2f6fb2 100644
--- a/conf/firmwares/subsystems/shared/actuators_direct.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_direct.makefile
@@ -1,5 +1,5 @@
 
 
-$(TARGET).CFLAGS += -DACTUATORS=\"subsystems/actuators/actuators_pwm.h\" -DSERVOS_DIRECT
-$(TARGET).srcs   += subsystems/actuators/actuators_pwm.c actuators.c
+$(TARGET).CFLAGS += -DACTUATORS -DSERVOS_DIRECT
+$(TARGET).srcs   += subsystems/actuators/actuators_pwm.c subsystems/actuators.c
 $(TARGET).srcs   += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
diff --git a/conf/firmwares/subsystems/shared/actuators_ppm.makefile b/conf/firmwares/subsystems/shared/actuators_ppm.makefile
index 99171e4448..f88eb3e3c0 100644
--- a/conf/firmwares/subsystems/shared/actuators_ppm.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_ppm.makefile
@@ -5,6 +5,6 @@
 # The PPM is output on the SERV_CLK pin. The PPM frame rate, pulse width, and number of channels
 # can be adjusted in the "servos_ppm_hw.h" file to suit your particular receiver.
 
-$(TARGET).CFLAGS += -DACTUATORS=\"servos_ppm_hw.h\" -DSERVOS_PPM_MAT
-$(TARGET).srcs += $(SRC_ARCH)/servos_ppm_hw.c actuators.c
+$(TARGET).CFLAGS += -DACTUATORS -DSERVOS_PPM_MAT
+$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/servos_ppm_hw.c subsystems/actuators.c
 
diff --git a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c
index 1b9a020303..ed2335ff66 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c
@@ -37,7 +37,15 @@
 #define SYS_TICK_IT TIR_MR0I
 
 #if defined ACTUATORS && ( defined SERVOS_4017 || defined SERVOS_4015_MAT || defined SERVOS_PPM_MAT)
-#include ACTUATORS
+#ifdef SERVOS_4015_MAT
+#include "subsystems/actuators/servos_4015_MAT_hw.h"
+#endif
+#ifdef SERVOS_4017
+#include "subsystems/actuators/servos_4017_hw.h"
+#endif
+#ifdef SERVOS_PPM
+#include "subsystems/actuators/servos_ppm_hw.h"
+#endif
 #else
 #define ACTUATORS_IT 0x00
 #endif /* ACTUATORS */
diff --git a/sw/airborne/arch/lpc21/servos_csc.h b/sw/airborne/arch/lpc21/servos_csc.h
deleted file mode 100644
index b7d8acb6b3..0000000000
--- a/sw/airborne/arch/lpc21/servos_csc.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef SERVOS_CSC_H
-#define SERVOS_CSC_H
-
-#include "LPC21xx.h"
-#include "generated/airframe.h"
-#include "actuators.h"
-#include "mcu_periph/sys_time.h"
-
-#define SERVOS_TICS_OF_USEC(s) CPU_TICKS_OF_USEC(s)
-#define ChopServo(x,a,b) Chop(x, a, b)
-#define SERVO_COUNT 4
-
-#define Actuator(i) actuators[i]
-
-static inline void ActuatorsCommit(void)
-{
-
-}
-
-#endif /* SERVOS_CSC_H */
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
index 2dfd68cdc8..ae0a10361b 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
@@ -28,7 +28,7 @@ const uint8_t pwm_latch_value = 0
 #endif
   ;
 
-void actuators_init ( void ) {
+void actuators_pwm_init ( void ) {
 
   /* configure pins for PWM */
 #if defined PWM_SERVO_0
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
index 0032b075dc..181f0d5b0c 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
@@ -15,7 +15,7 @@
 #define SERVO_REG_5 PWMMR2
 
 #define COMMAND_(i) SERVO_REG_ ## i
-#define Actuator(i) COMMAND_(i)
+#define ActuatorPwm(i) COMMAND_(i)
 
 
 /*
@@ -76,8 +76,11 @@ PWM6        PWM2  SSEL0  EINT2   P0.7
 
 extern const uint8_t pwm_latch_value;
 
-#define ActuatorsCommit() { \
+#define ActuatorsPwmCommit() { \
     PWMLER = pwm_latch_value;   \
   }
 
+extern void actuators_pwm_init(void);
+#define ActuatorsPwmInit() actuators_pwm_init()
+
 #endif /* ACTUATORS_PWM_ARCH_H */
diff --git a/sw/airborne/arch/lpc21/servos_4015_MAT_hw.c b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.c
similarity index 96%
rename from sw/airborne/arch/lpc21/servos_4015_MAT_hw.c
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.c
index cf254ffc59..5c777f8235 100644
--- a/sw/airborne/arch/lpc21/servos_4015_MAT_hw.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.c
@@ -1,6 +1,4 @@
 /*
- * $Id$
- *
  * Copyright (C) 2006  Antoine Drouin
  *
  * This file is part of paparazzi.
@@ -27,7 +25,7 @@
  */
 
 
-#include "actuators.h"
+#include "subsystems/actuators.h"
 #include "paparazzi.h"
 #include "generated/airframe.h"
 
@@ -36,7 +34,7 @@ uint32_t servos_delay = SERVO_REFRESH_TICS;
 
 #define START_TIMEOUT 0xFFFF;
 
-void actuators_init ( void ) {
+void actuators_4015_init ( void ) {
   /* select clock pin as MAT0.1 output */
   IO0DIR |= _BV(SERVO_CLOCK_PIN);
   SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT;
diff --git a/sw/airborne/arch/lpc21/servos_4015_MAT_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
similarity index 92%
rename from sw/airborne/arch/lpc21/servos_4015_MAT_hw.h
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
index d157e86bda..2e03368e5f 100644
--- a/sw/airborne/arch/lpc21/servos_4015_MAT_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
@@ -42,9 +42,11 @@
 
 #define _4015_NB_CHANNELS 8
 extern uint16_t servos_values[_4015_NB_CHANNELS];
-#define Actuator(i) servos_values[i]
+#define Actuator4015(i) servos_values[i]
 
-#define ActuatorsCommit() {}
+extern void actuators_4015_init(void);
+#define Actuators4015Commit() {}
+#define Actuators4015Init() actuators_4015_init()
 extern uint8_t servos_4015_idx;
 extern uint32_t servos_delay;
 
diff --git a/sw/airborne/arch/lpc21/servos_4015_hw.c b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.c
similarity index 77%
rename from sw/airborne/arch/lpc21/servos_4015_hw.c
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.c
index 60eeb6af5e..836d37829a 100644
--- a/sw/airborne/arch/lpc21/servos_4015_hw.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.c
@@ -1,4 +1,25 @@
-#include "actuators.h"
+/*
+ * Copyright (C) 2006  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.
+ *
+ */
+#include "subsystems/actuators.h"
 #include "armVIC.h"
 
 #include "generated/airframe.h"
@@ -19,7 +40,7 @@ uint16_t servos_values[_4015_NB_CHANNELS];
 #define PWMIR_MRI_SERV1 PWMIR_MR2I
 
 
-void actuators_init ( void ) {
+void actuators_4015_init ( void ) {
   /* PWM selected as IRQ */
   VICIntSelect &= ~VIC_BIT(VIC_PWM);
   /* PWM interrupt enabled */
diff --git a/sw/airborne/arch/lpc21/servos_4015_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h
similarity index 83%
rename from sw/airborne/arch/lpc21/servos_4015_hw.h
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h
index 60fe1593a0..eab191954b 100644
--- a/sw/airborne/arch/lpc21/servos_4015_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h
@@ -35,9 +35,11 @@
 
 #define _4015_NB_CHANNELS 8
 extern uint16_t servos_values[_4015_NB_CHANNELS];
-#define Actuator(i) servos_values[i]
+#define Actuator4015(i) servos_values[i]
 
-#define ActuatorsCommit() {}
+extern void actuators_4015_init(void);
+#define Actuators4015Commit() {}
+#define Actuators4015Init() actuators_4015_init()
 
 void PWM_ISR ( void ) __attribute__((naked));
 
diff --git a/sw/airborne/arch/lpc21/servos_4015_hw_new.c b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw_new.c
similarity index 82%
rename from sw/airborne/arch/lpc21/servos_4015_hw_new.c
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw_new.c
index 8a31b359b5..fdbc695682 100644
--- a/sw/airborne/arch/lpc21/servos_4015_hw_new.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw_new.c
@@ -1,4 +1,25 @@
-#include "actuators.h"
+/*
+ * Copyright (C) 2006  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.
+ *
+ */
+#include "subsystems/actuators.h"
 #include "armVIC.h"
 
 #include "generated/airframe.h"
@@ -19,7 +40,7 @@ uint16_t servos_values[_4015_NB_CHANNELS];
 #define PWMIR_MRI_SERV1 PWMIR_MR2I
 
 
-void actuators_init ( void ) {
+void actuators_4015_init ( void ) {
   /* PWM selected as IRQ */
   VICIntSelect &= ~VIC_BIT(VIC_PWM);
   /* PWM interrupt enabled */
diff --git a/sw/airborne/arch/lpc21/servos_4017_hw.c b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.c
similarity index 64%
rename from sw/airborne/arch/lpc21/servos_4017_hw.c
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.c
index 72022c414e..20a7d63b16 100644
--- a/sw/airborne/arch/lpc21/servos_4017_hw.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.c
@@ -1,4 +1,25 @@
-#include "actuators.h"
+/*
+ * Copyright (C) 2006  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.
+ *
+ */
+#include "subsystems/actuators.h"
 
 #include "paparazzi.h"
 #include "generated/airframe.h"
@@ -7,7 +28,7 @@ uint8_t servos_4017_idx;
 
 #define START_TIMEOUT 0xFFFF;
 
-void actuators_init ( void ) {
+void actuators_4017_init ( void ) {
   /* select clock pin as MAT0.1 output */
   IO0DIR |= _BV(SERVO_CLOCK_PIN);
   SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT;
diff --git a/sw/airborne/arch/lpc21/servos_4017_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
similarity index 93%
rename from sw/airborne/arch/lpc21/servos_4017_hw.h
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
index 9f2989c791..865fa7cf79 100644
--- a/sw/airborne/arch/lpc21/servos_4017_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
@@ -19,9 +19,11 @@
 #endif
 
 extern uint16_t servos_values[_4017_NB_CHANNELS];
-#define Actuator(i) servos_values[i]
+#define Actuator4017(i) servos_values[i]
 
-#define ActuatorsCommit() {}
+extern void actuators_4017_init(void);
+#define Actuators4017Commit() {}
+#define Actuators4017Init() actuators_4017_init()
 
 extern uint8_t servos_4017_idx;
 
diff --git a/sw/airborne/arch/lpc21/servos_ppm_hw.c b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.c
similarity index 96%
rename from sw/airborne/arch/lpc21/servos_ppm_hw.c
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.c
index 6162f48f9f..5a5271e504 100644
--- a/sw/airborne/arch/lpc21/servos_ppm_hw.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.c
@@ -26,7 +26,7 @@
  *  \ for a R/C receiver which has a microcontroller to drive the servos
  *  \(not a 4015 or 4017 decade counter chip).
  */
-#include "actuators.h"
+#include "subsystems/actuators.h"
 #include "paparazzi.h"
 #include "generated/airframe.h"
 
@@ -36,7 +36,7 @@ uint32_t servos_delay = SERVO_REFRESH_TICS;
 
 #define START_TIMEOUT 0xFFFF;
 
-void actuators_init ( void ) {
+void actuators_ppm_init ( void ) {
   /* select ppm output pin as MAT0.1 output */
   SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT;
 
diff --git a/sw/airborne/arch/lpc21/servos_ppm_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
similarity index 94%
rename from sw/airborne/arch/lpc21/servos_ppm_hw.h
rename to sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
index eb40c18d25..13e8b5e382 100644
--- a/sw/airborne/arch/lpc21/servos_ppm_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
@@ -40,9 +40,11 @@
 
 #define _PPM_NB_CHANNELS 8
 extern uint16_t servos_values[_PPM_NB_CHANNELS];
-#define Actuator(i) servos_values[i]
+#define ActuatorPpm(i) servos_values[i]
 
-#define ActuatorsCommit() {}
+extern void actuators_ppm_init(void);
+#define ActuatorsPpmCommit() {}
+#define ActuatorsPpmInit() actuators_ppm_init()
 extern uint8_t servos_PPM_idx;
 extern uint32_t servos_delay;
 extern uint8_t ppm_pulse; /* 1=start of pulse, 0=end of pulse */
diff --git a/sw/airborne/boards/tiny_0.99.h b/sw/airborne/boards/tiny_0.99.h
index b6915b33eb..f7055626c0 100644
--- a/sw/airborne/boards/tiny_0.99.h
+++ b/sw/airborne/boards/tiny_0.99.h
@@ -35,6 +35,12 @@
 #define LED_2_BANK 1
 #define LED_2_PIN 19
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4015.h"
+#define ActuatorDefault(_x) Actuator4015(_x)
+#define ActuatorsDefaultInit() Actuators4015Init()
+#define ActuatorsDefaultCommit() Actuators4015Commit()
+
 /* P0.5 aka MAT0.1  */
 #define SERVO_CLOCK_PIN  5
 #define SERVO_CLOCK_PINSEL PINSEL0
diff --git a/sw/airborne/boards/tiny_1.1.h b/sw/airborne/boards/tiny_1.1.h
index d76d4aa61e..21f806732f 100644
--- a/sw/airborne/boards/tiny_1.1.h
+++ b/sw/airborne/boards/tiny_1.1.h
@@ -46,6 +46,12 @@
 
 #define POWER_SWITCH_LED 3
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4015.h"
+#define ActuatorDefault(_x) Actuator4015(_x)
+#define ActuatorsDefaultInit() Actuators4015Init()
+#define ActuatorsDefaultCommit() Actuators4015Commit()
+
 /* P0.5 aka MAT0.1  */
 #define SERVO_CLOCK_PIN  5
 #define SERVO_CLOCK_PINSEL PINSEL0
diff --git a/sw/airborne/boards/tiny_2.0.h b/sw/airborne/boards/tiny_2.0.h
index ac512c8ea9..c48148f6bd 100644
--- a/sw/airborne/boards/tiny_2.0.h
+++ b/sw/airborne/boards/tiny_2.0.h
@@ -49,6 +49,12 @@
 
 #define POWER_SWITCH_LED 4
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
+#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorsDefaultInit() Actuators4017Init()
+#define ActuatorsDefaultCommit() Actuators4017Commit()
+
 /* P0.5 aka MAT0.1  */
 #define SERVO_CLOCK_PIN  5
 #define SERVO_CLOCK_PINSEL PINSEL0
diff --git a/sw/airborne/boards/tiny_2.1.h b/sw/airborne/boards/tiny_2.1.h
index 572f2b6cf0..598f679c9f 100644
--- a/sw/airborne/boards/tiny_2.1.h
+++ b/sw/airborne/boards/tiny_2.1.h
@@ -86,6 +86,12 @@
 #define Set_GPS_RESET_Pin_LOW() LED_ON(GPS_RESET)
 #define Open_GPS_RESET_Pin() ClearBit(LED_DIR(GPS_RESET), LED_PIN(GPS_RESET))
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
+#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorsDefaultInit() Actuators4017Init()
+#define ActuatorsDefaultCommit() Actuators4017Commit()
+
 /* P0.5 aka MAT0.1  */
 #define SERVO_CLOCK_PIN  5
 #define SERVO_CLOCK_PINSEL PINSEL0
diff --git a/sw/airborne/boards/tiny_sim.h b/sw/airborne/boards/tiny_sim.h
index c3cb2f3b91..c020bada14 100644
--- a/sw/airborne/boards/tiny_sim.h
+++ b/sw/airborne/boards/tiny_sim.h
@@ -29,6 +29,12 @@
 #define LED_2_BANK 1
 #define LED_2_PIN 19
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
+#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorsDefaultInit() Actuators4017Init()
+#define ActuatorsDefaultCommit() Actuators4017Commit()
+
 /* p0.21 aka PWM5 */
 #define SERV0_CLOCK_PIN 21
 #define SERV0_CLOCK_PINSEL PINSEL1
diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h
index f17d5df75d..52d2c68688 100644
--- a/sw/airborne/boards/umarim_1.0.h
+++ b/sw/airborne/boards/umarim_1.0.h
@@ -30,6 +30,12 @@
 #define LED_2_BANK 1
 #define LED_2_PIN 24
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
+#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorsDefaultInit() Actuators4017Init()
+#define ActuatorsDefaultCommit() Actuators4017Commit()
+
 /* P0.5 aka MAT0.1  */
 #define SERVO_CLOCK_PIN  5
 #define SERVO_CLOCK_PINSEL PINSEL0
diff --git a/sw/airborne/boards/umarim_lite_2.0.h b/sw/airborne/boards/umarim_lite_2.0.h
index 2690f73c0c..e3d8527cc8 100644
--- a/sw/airborne/boards/umarim_lite_2.0.h
+++ b/sw/airborne/boards/umarim_lite_2.0.h
@@ -30,6 +30,12 @@
 #define LED_2_BANK 1
 #define LED_2_PIN 24
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
+#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorsDefaultInit() Actuators4017Init()
+#define ActuatorsDefaultCommit() Actuators4017Commit()
+
 /* P0.5 aka MAT0.1  */
 #define SERVO_CLOCK_PIN  5
 #define SERVO_CLOCK_PINSEL PINSEL0
diff --git a/sw/airborne/firmwares/fixedwing/fbw_downlink.h b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
index 38609e9082..0bc8cd48ab 100644
--- a/sw/airborne/firmwares/fixedwing/fbw_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
@@ -40,7 +40,7 @@
 #include "generated/periodic_telemetry.h"
 #include "generated/airframe.h"
 #include "commands.h"
-#include "actuators.h"
+#include "subsystems/actuators.h"
 
 #include "mcu_periph/uart.h"
 #include "firmwares/fixedwing/main_fbw.h"
@@ -78,7 +78,7 @@
 #endif // RADIO_CONTROL
 
 #ifdef ACTUATORS
-#define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, SERVOS_NB, actuators)
+#define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, ACTUATORS_NB, actuators)
 #else
 #define PERIODIC_SEND_ACTUATORS(_trans, _dev) {}
 #endif
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 0c7b22060a..60e008d7d6 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -37,7 +37,7 @@
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
 #include "commands.h"
-#include "firmwares/fixedwing/actuators.h"
+#include "subsystems/actuators.h"
 #include "subsystems/electrical.h"
 #include "subsystems/radio_control.h"
 #include "firmwares/fixedwing/autopilot.h"
diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c
index 875e9dc2fb..1b72f44107 100644
--- a/sw/airborne/firmwares/setup/setup_actuators.c
+++ b/sw/airborne/firmwares/setup/setup_actuators.c
@@ -30,7 +30,7 @@ void dl_parse_msg( void ) {
     uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer);
     uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
     LED_TOGGLE(2);
-    if (servo_no < SERVOS_NB)
+    if (servo_no < ACTUATORS_NB)
       SetServo(servo_no, servo_value);
   }
 #ifdef DlSetting
@@ -58,7 +58,7 @@ void init_fbw( void ) {
   actuators_init();
 
   uint8_t i;
-  for(i = 0; i < SERVOS_NB; i++) {
+  for(i = 0; i < ACTUATORS_NB; i++) {
     SetServo(i, 1500);
   }
 
@@ -83,7 +83,7 @@ void periodic_task_fbw(void) {
    /* SetServo(SERVO_THROTTLE, servo_value); */
 
   RunOnceEvery(300, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM));
-  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, SERVOS_NB, actuators ));
+  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators ));
 }
 
 void event_task_fbw(void) {
diff --git a/sw/airborne/actuators.c b/sw/airborne/subsystems/actuators.c
similarity index 84%
rename from sw/airborne/actuators.c
rename to sw/airborne/subsystems/actuators.c
index 0d2a660b22..ead30dea2d 100644
--- a/sw/airborne/actuators.c
+++ b/sw/airborne/subsystems/actuators.c
@@ -24,6 +24,13 @@
  *
  */
 
-#include "firmwares/fixedwing/actuators.h"
+#include "subsystems/actuators.h"
 
-uint16_t actuators[SERVOS_NB];
+uint16_t actuators[ACTUATORS_NB];
+
+void actuators_init(void) {
+
+  // Init macro from generated airframe.h
+  AllActuatorsInit();
+
+}
diff --git a/sw/airborne/firmwares/fixedwing/actuators.h b/sw/airborne/subsystems/actuators.h
similarity index 71%
rename from sw/airborne/firmwares/fixedwing/actuators.h
rename to sw/airborne/subsystems/actuators.h
index 5f310f7a3c..9d3ee02fdb 100644
--- a/sw/airborne/firmwares/fixedwing/actuators.h
+++ b/sw/airborne/subsystems/actuators.h
@@ -1,5 +1,6 @@
 /*
  * (c) 2003-2005 Pascal Brisset, Antoine Drouin
+ * (c) 2012 Gautier Hattenberger
  *
  * This file is part of paparazzi.
  *
@@ -26,32 +27,29 @@
 #ifndef ACTUATORS_H
 #define ACTUATORS_H
 
-#if defined ACTUATORS
-
 #include "paparazzi.h"
 
-/** Defines SetActuatorsFromCommands() macro */
+/** Defines SetActuatorsFromCommands() macro
+ *  Defines ACTUATORS_NB to 0 if no servo
+ *  Include servos drivers
+ */
 #include "generated/airframe.h"
 
-/** Must be defined by specific hardware implementation */
+#if ACTUATORS_NB
+
 extern void actuators_init( void );
 
 /** Temporary storage (for debugging purpose, downlinked via telemetry) */
-extern uint16_t actuators[SERVOS_NB];
+extern uint16_t actuators[ACTUATORS_NB];
 
-#include ACTUATORS
+#define SetServo(x, v) { actuators[x##_IDX] = v; }
 
-#define SetServo(x, v) {                                        \
-    Actuator(x) = SERVOS_TICS_OF_USEC(ChopServo(v,700,2400));   \
-    actuators[x] = v;                                           \
-  }
+#else /* ACTUATORS_NB */
 
-#else /* ACTUATORS */
-
-// define empty SetServo makro for sim
+// define empty SetServo makro for sim (really needed ?)
 #define SetServo(x, v) {}
 
-#endif /* ACTUATORS */
+#endif /* ACTUATORS_NB */
 
 
 #endif /* ACTUATORS_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_4015.h b/sw/airborne/subsystems/actuators/actuators_4015.h
new file mode 100644
index 0000000000..07cd56875d
--- /dev/null
+++ b/sw/airborne/subsystems/actuators/actuators_4015.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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.
+ */
+
+#ifndef ACTUATORS_4015_H
+#define ACTUATORS_4015_H
+
+#include "subsystems/actuators/servos_4015_MAT_hw.h"
+
+#endif /* ACTUATORS_4015_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_4017.h b/sw/airborne/subsystems/actuators/actuators_4017.h
new file mode 100644
index 0000000000..315382d628
--- /dev/null
+++ b/sw/airborne/subsystems/actuators/actuators_4017.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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.
+ */
+
+#ifndef ACTUATORS_4017_H
+#define ACTUATORS_4017_H
+
+#include "subsystems/actuators/servos_4017_hw.h"
+
+#endif /* ACTUATORS_4017_H */
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c b/sw/airborne/subsystems/actuators/actuators_asctec.c
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
rename to sw/airborne/subsystems/actuators/actuators_asctec.c
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.h b/sw/airborne/subsystems/actuators/actuators_asctec.h
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.h
rename to sw/airborne/subsystems/actuators/actuators_asctec.h
diff --git a/sw/airborne/firmwares/rotorcraft/actuators.h b/sw/airborne/subsystems/actuators/actuators_default.h
similarity index 51%
rename from sw/airborne/firmwares/rotorcraft/actuators.h
rename to sw/airborne/subsystems/actuators/actuators_default.h
index e8d0c056d1..f3a0e750f0 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators.h
+++ b/sw/airborne/subsystems/actuators/actuators_default.h
@@ -1,36 +1,30 @@
 /*
- * Copyright (C) 2010-2012 The Paparazzi Team
+ * Copyright (C) 2012 Gautier Hattenberger
  *
- * This file is part of paparazzi.
+ * This file is part of Paparazzi.
  *
- * paparazzi is free software; you can redistribute it and/or modify
+ * 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,
+ * 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
+ * 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.
  */
 
-/** @file actuators.h
- *  Generic Actuators API.
+#ifndef ACTUATORS_DEFAULT_H
+#define ACTUATORS_DEFAULT_H
+
+/** Dummy include of default actuators header
  */
+#include BOARD_CONFIG
+#include DEFAULT_ACTUATORS
 
-#ifndef ACTUATORS_H
-#define ACTUATORS_H
-
-//#include ACTUATORS
-
-#include "std.h"
-
-extern void actuators_init(void);
-extern void actuators_set(bool_t motors_on);
-
-#endif /* ACTUATORS_H */
+#endif /* ACTUATORS_DEFAULT_H */
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c b/sw/airborne/subsystems/actuators/actuators_dummy.c
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c
rename to sw/airborne/subsystems/actuators/actuators_dummy.c
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c b/sw/airborne/subsystems/actuators/actuators_heli.c
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c
rename to sw/airborne/subsystems/actuators/actuators_heli.c
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/subsystems/actuators/actuators_mkk.c
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
rename to sw/airborne/subsystems/actuators/actuators_mkk.c
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.h b/sw/airborne/subsystems/actuators/actuators_mkk.h
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.h
rename to sw/airborne/subsystems/actuators/actuators_mkk.h
diff --git a/sw/airborne/subsystems/actuators/actuators_ppm.h b/sw/airborne/subsystems/actuators/actuators_ppm.h
new file mode 100644
index 0000000000..eba3f6915e
--- /dev/null
+++ b/sw/airborne/subsystems/actuators/actuators_ppm.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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.
+ */
+
+#ifndef ACTUATORS_PPM_H
+#define ACTUATORS_PPM_H
+
+#include "subsystems/actuators/servos_ppm_hw.h"
+
+#endif /* ACTUATORS_PPM_H */
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c b/sw/airborne/subsystems/actuators/actuators_pwm_supervision.c
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c
rename to sw/airborne/subsystems/actuators/actuators_pwm_supervision.c
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.c b/sw/airborne/subsystems/actuators/actuators_skiron.c
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.c
rename to sw/airborne/subsystems/actuators/actuators_skiron.c
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.h b/sw/airborne/subsystems/actuators/actuators_skiron.h
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.h
rename to sw/airborne/subsystems/actuators/actuators_skiron.h
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c b/sw/airborne/subsystems/actuators/supervision.c
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/supervision.c
rename to sw/airborne/subsystems/actuators/supervision.c
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/supervision.h b/sw/airborne/subsystems/actuators/supervision.h
similarity index 100%
rename from sw/airborne/firmwares/rotorcraft/actuators/supervision.h
rename to sw/airborne/subsystems/actuators/supervision.h
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index 3ce7e5ad18..677ffbd8a2 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -150,7 +150,10 @@ let preprocess_value = fun s v prefix ->
   let s = Str.global_replace pprz_value (sprintf "%s[%s_\\1]" v prefix) s in
   Str.global_replace var_value "_var_\\1" s
 
-
+let print_actuators_idx = fun () ->
+  let nb = Hashtbl.fold (fun s _d i -> printf "#define SERVO_%s_IDX %d\n" s i; i+1) servos_drivers 0 in
+  define "ACTUATORS_NB" (string_of_int nb);
+  nl ()
 
 let parse_command_laws = fun command ->
   let a = fun s -> ExtXml.attrib command s in
@@ -162,15 +165,18 @@ let parse_command_laws = fun command ->
        printf "  command_value = %s;\\\n" v;
        printf "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP : SERVO_%s_TRAVEL_DOWN;\\\n" servo servo;
        printf "  servo_value = SERVO_%s_NEUTRAL + (int32_t)(command_value);\\\n" servo;
-       printf "  actuators[SERVO_%s] = ChopServo(servo_value, SERVO_%s_MIN, SERVO_%s_MAX);\\\n\\\n" servo servo servo;
+       printf "  actuators[SERVO_%s_IDX] = ChopServo(servo_value, SERVO_%s_MIN, SERVO_%s_MAX);\\\n\\\n" servo servo servo;
 
        let driver = get_servo_driver servo in
-       printf "  Actuator%s(SERVO_%s) = SERVOS_TICS_OF_USEC(actuators[SERVO_%s]);\\\n\\\n" driver servo servo
+       printf "  Actuator%s(SERVO_%s) = SERVOS_TICS_OF_USEC(actuators[SERVO_%s_IDX]);\\\n\\\n" driver servo servo
    | "let" ->
        let var = a "var"
        and value = a "value" in
        let v = preprocess_value value "values" "COMMAND" in
        printf "  int16_t _var_%s = %s;\\\n" var v
+   | "call" ->
+       let f = a "fun" in
+       printf "  %s;\\\n" f
    | "ratelimit" ->
        let var = a "var"
        and value = a "value"
@@ -182,40 +188,6 @@ let parse_command_laws = fun command ->
        parse_element "" command
    | _ -> xml_error "set|let"
 
-let parse_csc_fields = fun csc_fields ->
-  let a = fun s -> ExtXml.attrib csc_fields s in
-   match Xml.tag csc_fields with
-     "field_map" ->
-       let servo_id = a "servo_id"
-       and field = a "field" in
-       printf "  temp.%s = actuators[%s]; \\\n" field servo_id;
-      | _ -> xml_error "field_map"
-
-let parse_csc_messages = (let msg_index_ref = ref 0 in fun csc_id csc_messages ->
-  let a = fun s -> ExtXml.attrib csc_messages s in
-   match Xml.tag csc_messages with
-     "msg" ->
-       let msg_id = a "id"
-       and msg_type = a "type"
-       and msg_index = msg_index_ref.contents in
-       msg_index_ref.contents <- msg_index + 1;
-       printf "{\\\n  struct Csc%s temp; \\\n" msg_type;
-       List.iter parse_csc_fields (Xml.children csc_messages);
-       printf "  can_write_csc(%s, CSC_%s, (uint8_t *)&temp, sizeof(struct Csc%s)); \\\n" csc_id msg_id msg_type;
-       printf "} \\\n"
-      | _ -> xml_error "msg"
-  )
-
-let parse_csc_boards = fun csc_board ->
-  let a = fun s -> ExtXml.attrib csc_board s in
-   match Xml.tag csc_board with
-     "board" ->
-       let csc_id = a "id" in
-       List.iter (parse_csc_messages csc_id) (Xml.children csc_board);
-   | "define" ->
-       parse_element "" csc_board
-   | _ -> xml_error "board"
-
 let parse_rc_commands = fun rc ->
   let a = fun s -> ExtXml.attrib rc s in
   match Xml.tag rc with
@@ -255,11 +227,12 @@ let rec parse_section = fun s ->
       List.iter (parse_element prefix) (Xml.children s);
       nl ()
   | "servos" ->
-      let driver = ExtXml.attrib_or_default s "driver" "" in
+      let driver = ExtXml.attrib_or_default s "driver" "Default" in
       let servos = Xml.children s in
       let nb_servos = List.fold_right (fun s m -> Pervasives.max (int_of_string (ExtXml.attrib s "no")) m) servos min_int + 1 in
 
-      define "SERVOS_NB" (string_of_int nb_servos);
+      define (sprintf "SERVOS_%s_NB" (String.uppercase driver)) (string_of_int nb_servos);
+      printf "#include \"subsystems/actuators/actuators_%s.h\"\n" (String.lowercase driver);
       nl ();
       List.iter (parse_servo driver) servos;
       nl ()
@@ -282,6 +255,8 @@ let rec parse_section = fun s ->
       List.iter parse_ap_only_commands (Xml.children s);
       printf "}\n\n"
   | "command_laws" ->
+      print_actuators_idx ();
+
       printf "#define SetActuatorsFromCommands(values) { \\\n";
       printf "  uint32_t servo_value;\\\n";
       printf "  float command_value;\\\n";
@@ -295,12 +270,6 @@ let rec parse_section = fun s ->
       printf "#define AllActuatorsInit() { \\\n";
       List.iter (fun d -> printf "  Actuators%sInit();\\\n" d) drivers;
       printf "}\n\n";
-  | "csc_boards" ->
-      let boards = Array.of_list (Xml.children s) in
-      define "CSC_BOARD_NB" (string_of_int (Array.length boards));
-      printf "#define SendCscFromActuators() { \\\n";
-      List.iter parse_csc_boards (Xml.children s);
-      printf "}\n"
   | "include" ->
       let filename = ExtXml.attrib s "href" in
       let subxml = Xml.parse_file filename in

From 184b41633d1a3d9bbe97b9f61c1c0555871600e8 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 10 Oct 2012 21:30:20 +0200
Subject: [PATCH 02/26] [actuators] more actuator drivers with new interface

---
 conf/airframes/ENAC/quadrotor/blender.xml     | 62 ++++++++++++-------
 conf/airframes/ENAC/quadrotor/booz2_g1.xml    | 46 ++++++++------
 conf/firmwares/rotorcraft.makefile            |  1 +
 .../subsystems/fixedwing/autopilot.makefile   |  1 +
 .../subsystems/shared/actuators_4015.makefile |  2 +-
 .../subsystems/shared/actuators_4017.makefile |  2 +-
 .../actuators_asctec.makefile                 |  4 +-
 .../shared/actuators_direct.makefile          |  4 +-
 .../subsystems/shared/actuators_ppm.makefile  |  2 +-
 .../actuators_skiron.makefile                 | 18 +++---
 .../subsystems/actuators/actuators_pwm_arch.c | 25 +++++++-
 .../subsystems/actuators/actuators_pwm_arch.h |  3 +-
 .../subsystems/actuators/servos_4017_hw.h     |  2 +-
 sw/airborne/boards/booz_1.0.h                 | 27 +++-----
 sw/airborne/boards/navgo_1.0.h                | 21 +++----
 sw/airborne/boards/tiny_2.1.h                 |  2 +-
 sw/airborne/firmwares/rotorcraft/main.c       |  5 +-
 sw/airborne/firmwares/rotorcraft/telemetry.h  |  2 +-
 sw/airborne/subsystems/actuators.c            |  5 ++
 sw/airborne/subsystems/actuators.h            |  9 +--
 .../subsystems/actuators/actuators_asctec.c   | 20 +++---
 .../subsystems/actuators/actuators_asctec.h   | 24 +++++--
 .../subsystems/actuators/actuators_pwm.c      |  7 +--
 .../subsystems/actuators/actuators_pwm.h      |  7 +--
 .../subsystems/actuators/actuators_skiron.c   | 27 +++-----
 .../subsystems/actuators/actuators_skiron.h   | 11 +++-
 .../subsystems/actuators/supervision.c        |  5 +-
 sw/tools/gen_airframe.ml                      |  4 +-
 28 files changed, 190 insertions(+), 158 deletions(-)
 rename conf/firmwares/subsystems/{rotorcraft => shared}/actuators_asctec.makefile (74%)
 rename conf/firmwares/subsystems/{rotorcraft => shared}/actuators_skiron.makefile (69%)

diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index 209b6518e6..ed812e9dac 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -1,11 +1,13 @@
+<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
+
 <airframe name="Blender">
 
   <modules main_freq="512">
-    <load name="booz_pwm.xml">
+    <!--load name="booz_pwm.xml">
       <define name="USE_PWM0"/>
     </load>
     <load name="booz_drop.xml"/>
-    <load name="booz_cam.xml"/>
+    <load name="booz_cam.xml"/-->
     <load name="sonar_maxbotix_booz.xml"/>
     <!--load name="adc_generic.xml">
       <configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
@@ -30,38 +32,19 @@
     <subsystem name="actuators"     type="skiron">
       <define name="SKIRON_I2C_SCL_TIME" value="25"/>
     </subsystem>
+    <subsystem name="actuators"     type="direct">
+      <define name="USE_PWM0"/>
+    </subsystem>
     <subsystem name="imu"           type="navgo"/>
     <subsystem name="gps"           type="ublox">
       <configure name="GPS_BAUD" value="B57600"/>
     </subsystem>
     <subsystem name="stabilization" type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_quat"/>
-    <subsystem name="ins"           type="extended"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>
 
-  <servos>
-    <servo name="FRONT"  no="0"    min="0" neutral="0" max="255"/>
-    <servo name="BACK"   no="2"    min="0" neutral="0" max="255"/>
-    <servo name="RIGHT"  no="1"    min="0" neutral="0" max="255"/>
-    <servo name="LEFT"   no="3"    min="0" neutral="0" max="255"/>
-  </servos>
-
-  <commands>
-    <axis name="PITCH"  failsafe_value="0"/>
-    <axis name="ROLL"   failsafe_value="0"/>
-    <axis name="YAW"    failsafe_value="0"/>
-    <axis name="THRUST" failsafe_value="0"/>
-  </commands>
-
-  <section name="ACTUATORS_SKIRON" prefix="ACTUATORS_SKIRON_">
-    <define name="NB" value="4"/>
-    <define name="IDX" value="{ SERVO_FRONT, SERVO_BACK, SERVO_RIGHT, SERVO_LEFT }"/>
-  </section>
-
   <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="MIN_MOTOR" value="20"/>
-    <define name="MAX_MOTOR" value="255"/>
     <define name="TRIM_A" value="-50"/>
     <define name="TRIM_E" value="-150"/>
     <define name="TRIM_R" value="-100"/>
@@ -73,6 +56,37 @@
     <define name="THRUST_COEF" value="{ 256,  256,  256,  256}"/>
   </section>
 
+  <servos driver="Skiron">
+    <servo name="FRONT"  no="0"    min="20" neutral="20" max="255"/>
+    <servo name="BACK"   no="2"    min="20" neutral="20" max="255"/>
+    <servo name="RIGHT"  no="1"    min="20" neutral="20" max="255"/>
+    <servo name="LEFT"   no="3"    min="20" neutral="20" max="255"/>
+  </servos>
+
+  <servos driver="Pwm">
+    <servo name="DROP" no="5" min="1000" neutral="1500" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+
+  <!--section name="ACTUATORS_SKIRON" prefix="ACTUATORS_SKIRON_">
+    <define name="NB" value="4"/>
+    <define name="IDX" value="{ SERVO_FRONT, SERVO_BACK, SERVO_RIGHT, SERVO_LEFT }"/>
+  </section-->
+
+  <command_laws>
+    <call fun="supervision_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT" value="supervision.commands[SERVO_FRONT]"/>
+    <set servo="BACK" value="supervision.commands[SERVO_BACK]"/>
+    <set servo="RIGHT" value="supervision.commands[SERVO_RIGHT]"/>
+    <set servo="LEFT" value="supervision.commands[SERVO_LEFT]"/>
+  </command_laws>
+
   <section name="IMU" prefix="IMU_">
 
     <define name="GYRO_P_NEUTRAL" value="10"/>
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 743a71c620..bfdaa7420a 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -1,11 +1,11 @@
 <airframe name="BOOZ2_G1">
 
   <modules main_freq="512">
-    <load name="booz_pwm.xml">
+    <!--load name="booz_pwm.xml">
       <define name="USE_PWM1"/>
     </load>
     <load name="booz_drop.xml"/>
-    <load name="booz_cam.xml"/>
+    <load name="booz_cam.xml"/-->
     <!--load name="sonar_maxbotix_booz.xml"/-->
     <!--load name="adc_generic_booz.xml"/-->
     <!--load name="sys_mon.xml"/-->
@@ -27,28 +27,35 @@
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="telemetry"     type="transparent"/>
     <subsystem name="actuators"     type="asctec"/>
+    <subsystem name="actuators"     type="direct">
+      <define name="USE_PWM0"/>
+      <define name="USE_PWM1"/>
+    </subsystem>
     <subsystem name="imu"           type="b2_v1.1"/>
     <subsystem name="gps"           type="ublox">
       <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
     <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_euler"/>
-    <subsystem name="ins"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>
 
-  <firmware name="booz_test_progs">
-    <target name="test_telemetry"   board="booz_1.0"/>
-    <target name="test_baro"        board="booz_1.0"/>
-    <target name="test_rc_spektrum" board="booz_1.0"/>
-    <target name="test_rc_ppm"      board="booz_1.0"/>
-  </firmware>
+  <section name="TRIM">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="6"/>
+    <define name="TRIM_YAW" value="0"/>
+  </section>
 
-  <servos>
-    <servo name="PITCH"  no="0" min="0" neutral="0" max="255"/>
-    <servo name="ROLL"   no="1" min="0" neutral="0" max="255"/>
-    <servo name="YAW"    no="2" min="0" neutral="0" max="255"/>
-    <servo name="THRUST" no="3" min="0" neutral="0" max="255"/>
+  <servos driver="Asctec">
+    <servo name="PITCH"  no="0" min="-100" neutral="0" max="100"/>
+    <servo name="ROLL"   no="1" min="-100" neutral="0" max="100"/>
+    <servo name="YAW"    no="2" min="-100" neutral="0" max="100"/>
+    <servo name="THRUST" no="3" min="0"    neutral="0" max="200"/>
+  </servos>
+
+  <servos driver="Pwm">
+    <servo name="CAM" no="0" min="1000" neutral="1500" max="2000"/>
+    <servo name="DROP" no="5" min="1000" neutral="1500" max="2000"/>
   </servos>
 
   <commands>
@@ -58,11 +65,12 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="TRIM_A" value="0"/>
-    <define name="TRIM_E" value="6"/>
-    <define name="TRIM_R" value="0"/>
-  </section>
+  <command_laws>
+    <set servo="PITCH" value="@PITCH"/>
+    <set servo="ROLL" value="@ROLL"/>
+    <set servo="YAW" value="@YAW"/>
+    <set servo="THRUST" value="@THRUST"/>
+  </command_laws>
 
   <section name="IMU" prefix="IMU_">
 
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index d25efa24d1..c46f2c7715 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -105,6 +105,7 @@ ifeq ($(TARGET), ap)
 endif
 
 ap.srcs += $(SRC_FIRMWARE)/commands.c
+ap.srcs += subsystems/actuators.c
 
 #
 # Radio control choice
diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index c3db971434..cc61ae07d8 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -153,6 +153,7 @@ fbw_CFLAGS		+= -DFBW
 fbw_srcs 		+= $(SRC_FIRMWARE)/main_fbw.c
 fbw_srcs 		+= subsystems/electrical.c
 fbw_srcs 		+= $(SRC_FIXEDWING)/commands.c
+fbw_srcs 		+= subsystems/actuators.c
 fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 
 ######################################################################
diff --git a/conf/firmwares/subsystems/shared/actuators_4015.makefile b/conf/firmwares/subsystems/shared/actuators_4015.makefile
index cc1951717a..2ae0e976a8 100644
--- a/conf/firmwares/subsystems/shared/actuators_4015.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_4015.makefile
@@ -1,4 +1,4 @@
 # for Tiny v1.1
 
 $(TARGET).CFLAGS 	+= -DACTUATORS -DSERVOS_4015_MAT
-$(TARGET).srcs 	+= $(SRC_ARCH)/subsystems/actuators/servos_4015_MAT_hw.c subsystems/actuators.c
+$(TARGET).srcs 	+= $(SRC_ARCH)/subsystems/actuators/servos_4015_MAT_hw.c
diff --git a/conf/firmwares/subsystems/shared/actuators_4017.makefile b/conf/firmwares/subsystems/shared/actuators_4017.makefile
index 7750d2a992..db89c10474 100644
--- a/conf/firmwares/subsystems/shared/actuators_4017.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_4017.makefile
@@ -1,4 +1,4 @@
 # for Tiny v2 or Twog v1
 
 $(TARGET).CFLAGS += -DACTUATORS -DSERVOS_4017
-$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/servos_4017_hw.c subsystems/actuators.c
+$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/servos_4017_hw.c
diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile b/conf/firmwares/subsystems/shared/actuators_asctec.makefile
similarity index 74%
rename from conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile
rename to conf/firmwares/subsystems/shared/actuators_asctec.makefile
index d1d1c1137d..38032aff04 100644
--- a/conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_asctec.makefile
@@ -1,5 +1,5 @@
 # asctec controllers
-ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
+ap.srcs += subsystems/actuators/actuators_asctec.c
 
 ifeq ($(ARCH), lpc21)
 ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0
@@ -13,6 +13,6 @@ endif
 
 
 # Simulator
-nps.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
+nps.srcs += subsystems/actuators/actuators_asctec.c
 nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0
 
diff --git a/conf/firmwares/subsystems/shared/actuators_direct.makefile b/conf/firmwares/subsystems/shared/actuators_direct.makefile
index 21ed2f6fb2..a47ef259c4 100644
--- a/conf/firmwares/subsystems/shared/actuators_direct.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_direct.makefile
@@ -1,5 +1,5 @@
 
 
-$(TARGET).CFLAGS += -DACTUATORS -DSERVOS_DIRECT
-$(TARGET).srcs   += subsystems/actuators/actuators_pwm.c subsystems/actuators.c
+$(TARGET).CFLAGS += -DACTUATORS
+$(TARGET).srcs   += subsystems/actuators/actuators_pwm.c
 $(TARGET).srcs   += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
diff --git a/conf/firmwares/subsystems/shared/actuators_ppm.makefile b/conf/firmwares/subsystems/shared/actuators_ppm.makefile
index f88eb3e3c0..8509ec1de1 100644
--- a/conf/firmwares/subsystems/shared/actuators_ppm.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_ppm.makefile
@@ -6,5 +6,5 @@
 # can be adjusted in the "servos_ppm_hw.h" file to suit your particular receiver.
 
 $(TARGET).CFLAGS += -DACTUATORS -DSERVOS_PPM_MAT
-$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/servos_ppm_hw.c subsystems/actuators.c
+$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/servos_ppm_hw.c
 
diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_skiron.makefile b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
similarity index 69%
rename from conf/firmwares/subsystems/rotorcraft/actuators_skiron.makefile
rename to conf/firmwares/subsystems/shared/actuators_skiron.makefile
index 6365fbbfbb..254152a481 100644
--- a/conf/firmwares/subsystems/rotorcraft/actuators_skiron.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
@@ -12,14 +12,8 @@
 #
 #
 # required xml configuration:
-#  <section name="ACTUATORS_SKIRON" prefix="ACTUATORS_SKIRON_">
-#    <define name="NB" value="4"/>
-#    <define name="IDX" value="{ 0, 1, 2, 3 }"/>
-#  </section>
 #
 #  <section name="SUPERVISION" prefix="SUPERVISION_">
-#    <define name="MIN_MOTOR" value="20"/>
-#    <define name="MAX_MOTOR" value="255"/>
 #    <define name="TRIM_A" value="0"/>
 #    <define name="TRIM_E" value="0"/>
 #    <define name="TRIM_R" value="0"/>
@@ -31,6 +25,8 @@
 #    <define name="THRUST_COEF" value="{ 256,  256,  256,  256}"/>
 #  </section>
 #
+#  servo section with driver="Skiron"
+#  command_laws section to map supervision commands to servos
 #
 
 # set default i2c timing if not already configured
@@ -38,8 +34,8 @@ ifeq ($(SKIRON_I2C_SCL_TIME), )
 SKIRON_I2C_SCL_TIME=150
 endif
 
-ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_skiron.c
+ap.srcs += subsystems/actuators/supervision.c
+ap.srcs += subsystems/actuators/actuators_skiron.c
 
 ifeq ($(ARCH), lpc21)
 ap.CFLAGS += -DACTUATORS_SKIRON_DEVICE=i2c0
@@ -47,7 +43,7 @@ ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_
 endif
 
 # Simulator
-nps.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-nps.srcs += $(SRC_FIRMWARE)/actuators/actuators_skiron.c
-nps.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME) -DI2C0_VIC_SLOT=10 -DACTUATORS_MKK_DEVICE=i2c0
+nps.srcs += subsystems/actuators/supervision.c
+nps.srcs += subsystems/actuators/actuators_skiron.c
+nps.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME) -DI2C0_VIC_SLOT=10 -DACTUATORS_SKIRON_DEVICE=i2c0
 
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
index ae0a10361b..61eca3c1e0 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
@@ -1,6 +1,27 @@
+/*
+ * Copyright (C) 2010-2012 The Paparazzi Team
+ *
+ * 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.
+ */
+
 #include "std.h"
-#include "actuators.h"
-#include "actuators_pwm_arch.h"
+#include "subsystems/actuators.h"
+#include "subsystems/actuators/actuators_pwm_arch.h"
 
 /* 40 Hz */
 #ifndef SERVOS_PERIOD
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
index 181f0d5b0c..3a23cedbaf 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
@@ -5,7 +5,6 @@
 #include "mcu_periph/sys_time.h"
 
 #define SERVOS_TICS_OF_USEC(s) CPU_TICKS_OF_USEC(s)
-#define ChopServo(x,a,b) Chop(x, a, b)
 
 #define SERVO_REG_0 PWMMR5
 #define SERVO_REG_1 PWMMR3
@@ -15,7 +14,7 @@
 #define SERVO_REG_5 PWMMR2
 
 #define COMMAND_(i) SERVO_REG_ ## i
-#define ActuatorPwm(i) COMMAND_(i)
+#define ActuatorPwmSet(_i, _v) { COMMAND_(_i) = SERVOS_TICS_OF_USEC(_v); }
 
 
 /*
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
index 865fa7cf79..9a1c83e61b 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
@@ -19,7 +19,7 @@
 #endif
 
 extern uint16_t servos_values[_4017_NB_CHANNELS];
-#define Actuator4017(i) servos_values[i]
+#define Actuator4017Set(_i, _v) { servos_values[i] = SERVOS_TICS_OF_USEC(_v); }
 
 extern void actuators_4017_init(void);
 #define Actuators4017Commit() {}
diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h
index be7ef13afe..3516c091d4 100644
--- a/sw/airborne/boards/booz_1.0.h
+++ b/sw/airborne/boards/booz_1.0.h
@@ -147,26 +147,19 @@
 #define MS2100_DRDY_EINT 3
 #define MS2100_DRDY_VIC_IT VIC_EINT3
 
-/* PWM5 on CAM connector */
-/* P0.21 */
-#define PWM0_PINSEL PINSEL1
-#define PWM0_PINSEL_VAL 0x01
-#define PWM0_PINSEL_BIT 10
+/* BOOZ PWM0 = PWM_SERVO_0 (driver) = PWM5 (lpc)
+ * on CAM connector */
+#if USE_PWM0
+#define PWM_SERVO_0 1
+#endif
 
-/* PWM2 on SPI connector */
-/* P0.7 */
-#define PWM1_PINSEL PINSEL0
-#define PWM1_PINSEL_VAL 0x02
-#define PWM1_PINSEL_BIT 14
+/* BOOZ PWM1 = PWM_SERVO_5 (driver) = PWM2 (lpc)
+ * on SPI connector */
+#if USE_PWM1
+#define PWM_SERVO_5 1
+#endif
 
 
 #define BOARD_HAS_BARO 1
 
-/*
- * Modem
- */
-//#define MODEM_DEVICE Uart1
-//#define MODEM_UART_FLAG
-
-
 #endif /* CONFIG_BOOZ2_V1_0_H */
diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h
index c3d4c28d72..f409143362 100644
--- a/sw/airborne/boards/navgo_1.0.h
+++ b/sw/airborne/boards/navgo_1.0.h
@@ -54,7 +54,7 @@
 
 /* ADC */
 
-/* not compatible with PWM0 */
+/* not compatible with PWM1 */
 #define ADC_0 AdcBank1(6)
 #ifdef USE_ADC_0
 #ifndef USE_AD1
@@ -88,18 +88,15 @@
 #define SPI1_DRDY_EINT         0
 #define SPI1_DRDY_VIC_IT       VIC_EINT0
 
-/* PWM0 (internal PWM2) */
-/* P0.7 */
-#define PWM0_PINSEL PINSEL0
-#define PWM0_PINSEL_VAL 0x02
-#define PWM0_PINSEL_BIT 14
+/* NAVGO PWM0 = PWM_SERVO_5 (driver) = PWM2 (lpc) */
+#if USE_PWM0
+#define PWM_SERVO_5 1
+#endif
 
-/* PWM1 (internal PWM5) */
-/* not compatible with ADC_0 */
-/* P0.21 */
-#define PWM1_PINSEL PINSEL1
-#define PWM1_PINSEL_VAL 0x01
-#define PWM1_PINSEL_BIT 10
+/* NAVGO PWM1 = PWM_SERVO_0 (driver) = PWM5 (lpc) */
+#if USE_PWM1
+#define PWM_SERVO_0 1
+#endif
 
 #define BOARD_HAS_BARO 1
 
diff --git a/sw/airborne/boards/tiny_2.1.h b/sw/airborne/boards/tiny_2.1.h
index 598f679c9f..bef470ba3a 100644
--- a/sw/airborne/boards/tiny_2.1.h
+++ b/sw/airborne/boards/tiny_2.1.h
@@ -88,7 +88,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
-#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorDefaultSet(_x,_y) Actuator4017Set(_x,_y)
 #define ActuatorsDefaultInit() Actuators4017Init()
 #define ActuatorsDefaultCommit() Actuators4017Commit()
 
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 9d7e2a37c4..ec47388565 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -36,7 +36,7 @@
 #include "subsystems/datalink/xbee.h"
 
 #include "firmwares/rotorcraft/commands.h"
-#include "firmwares/rotorcraft/actuators.h"
+#include "subsystems/actuators.h"
 
 #include "subsystems/imu.h"
 #include "subsystems/gps.h"
@@ -160,7 +160,8 @@ STATIC_INLINE void main_periodic( void ) {
   /* run control loops */
   autopilot_periodic();
   /* set actuators     */
-  actuators_set(autopilot_motors_on);
+  //actuators_set(autopilot_motors_on);
+  SetActuatorsFromCommands(commands);
 
   modules_periodic_task();
 
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 1a29c1d6a4..12161a08fe 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -40,7 +40,7 @@
 #include "firmwares/rotorcraft/autopilot.h"
 #include "firmwares/rotorcraft/guidance.h"
 
-#include "firmwares/rotorcraft/actuators.h"
+#include "subsystems/actuators.h"
 
 #include "mcu_periph/sys_time.h"
 #include "subsystems/electrical.h"
diff --git a/sw/airborne/subsystems/actuators.c b/sw/airborne/subsystems/actuators.c
index ead30dea2d..907e24ad9d 100644
--- a/sw/airborne/subsystems/actuators.c
+++ b/sw/airborne/subsystems/actuators.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2006 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2012 Gautier Hattenberger
  *
  * This file is part of paparazzi.
  *
@@ -26,6 +27,8 @@
 
 #include "subsystems/actuators.h"
 
+#if ACTUATORS_NB
+
 uint16_t actuators[ACTUATORS_NB];
 
 void actuators_init(void) {
@@ -34,3 +37,5 @@ void actuators_init(void) {
   AllActuatorsInit();
 
 }
+
+#endif
diff --git a/sw/airborne/subsystems/actuators.h b/sw/airborne/subsystems/actuators.h
index 9d3ee02fdb..7a71c96bc6 100644
--- a/sw/airborne/subsystems/actuators.h
+++ b/sw/airborne/subsystems/actuators.h
@@ -37,18 +37,11 @@
 
 #if ACTUATORS_NB
 
-extern void actuators_init( void );
+extern void actuators_init(void);
 
 /** Temporary storage (for debugging purpose, downlinked via telemetry) */
 extern uint16_t actuators[ACTUATORS_NB];
 
-#define SetServo(x, v) { actuators[x##_IDX] = v; }
-
-#else /* ACTUATORS_NB */
-
-// define empty SetServo makro for sim (really needed ?)
-#define SetServo(x, v) {}
-
 #endif /* ACTUATORS_NB */
 
 
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec.c b/sw/airborne/subsystems/actuators/actuators_asctec.c
index e49eba07d5..920779e13b 100644
--- a/sw/airborne/subsystems/actuators/actuators_asctec.c
+++ b/sw/airborne/subsystems/actuators/actuators_asctec.c
@@ -23,12 +23,8 @@
  *  Actuators driver for Asctec motor controllers.
  */
 
-#include "firmwares/rotorcraft/actuators.h"
-#include "firmwares/rotorcraft/actuators/actuators_asctec.h"
-
-#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
-#include "firmwares/rotorcraft/actuators/supervision.h"
-#endif
+#include "subsystems/actuators.h"
+#include "subsystems/actuators/actuators_asctec.h"
 
 #include "firmwares/rotorcraft/commands.h"
 #include "mcu_periph/i2c.h"
@@ -45,7 +41,7 @@ struct ActuatorsAsctec actuators_asctec;
 uint32_t actuators_delay_time;
 bool_t   actuators_delay_done;
 
-void actuators_init(void) {
+void actuators_asctec_init(void) {
   actuators_asctec.cmd = NONE;
   actuators_asctec.cur_addr = FRONT;
   actuators_asctec.new_addr = FRONT;
@@ -74,7 +70,7 @@ void actuators_init(void) {
 }
 
 #ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
-void actuators_set(bool_t motors_on) {
+void actuators_asctec_set(bool_t motors_on) {
 #if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
     if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
@@ -102,10 +98,10 @@ void actuators_set(bool_t motors_on) {
   actuators_asctec.cmds[YAW]    = 0;
   actuators_asctec.cmds[THRUST] = 0;
 #else /* ! KILL_MOTORS */
-  actuators_asctec.cmds[PITCH]  = ((commands[COMMAND_PITCH]  + SUPERVISION_TRIM_E) * ASCTEC_MAX_CMD) / MAX_PPRZ;
-  actuators_asctec.cmds[ROLL]   = ((commands[COMMAND_ROLL]   + SUPERVISION_TRIM_A) * ASCTEC_MAX_CMD) / MAX_PPRZ;
-  actuators_asctec.cmds[YAW]    = ((commands[COMMAND_YAW]    + SUPERVISION_TRIM_R) * ASCTEC_MAX_CMD) / MAX_PPRZ;
-  actuators_asctec.cmds[THRUST] = (commands[COMMAND_THRUST] * ASCTEC_MAX_THROTTLE) / MAX_PPRZ;
+//  actuators_asctec.cmds[PITCH]  = ((commands[COMMAND_PITCH]  + SUPERVISION_TRIM_E) * ASCTEC_MAX_CMD) / MAX_PPRZ;
+//  actuators_asctec.cmds[ROLL]   = ((commands[COMMAND_ROLL]   + SUPERVISION_TRIM_A) * ASCTEC_MAX_CMD) / MAX_PPRZ;
+//  actuators_asctec.cmds[YAW]    = ((commands[COMMAND_YAW]    + SUPERVISION_TRIM_R) * ASCTEC_MAX_CMD) / MAX_PPRZ;
+//  actuators_asctec.cmds[THRUST] = (commands[COMMAND_THRUST] * ASCTEC_MAX_THROTTLE) / MAX_PPRZ;
   Bound(actuators_asctec.cmds[PITCH],ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
   Bound(actuators_asctec.cmds[ROLL], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
   Bound(actuators_asctec.cmds[YAW],  ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec.h b/sw/airborne/subsystems/actuators/actuators_asctec.h
index 8a7ddddeef..fc6aae40e0 100644
--- a/sw/airborne/subsystems/actuators/actuators_asctec.h
+++ b/sw/airborne/subsystems/actuators/actuators_asctec.h
@@ -28,27 +28,32 @@
 
 #include "mcu_periph/i2c.h"
 
-enum actuators_astec_cmd { NONE,
+#include "generated/airframe.h"
+#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
+#include "subsystems/actuators/supervision.h"
+#endif
+
+enum actuators_asctec_cmd { NONE,
                            TEST,
                            REVERSE,
                            SET_ADDR };
 
-enum actuators_astec_addr { FRONT,
+enum actuators_asctec_addr { FRONT,
                             BACK,
                             LEFT,
                             RIGHT };
 
 /* this is for the v1 protocol which does its own mixing */
-enum actuators_astec_cmds { PITCH,
+enum actuators_asctec_cmds { PITCH,
                             ROLL,
                             YAW,
                             THRUST,
                             CMD_NB };
 
 struct ActuatorsAsctec {
-  enum actuators_astec_cmd cmd;
-  enum actuators_astec_addr cur_addr;
-  enum actuators_astec_addr new_addr;
+  enum actuators_asctec_cmd cmd;
+  enum actuators_asctec_addr cur_addr;
+  enum actuators_asctec_addr new_addr;
   int32_t cmds[CMD_NB];
   struct i2c_transaction i2c_trans;
   volatile uint32_t nb_err;
@@ -69,5 +74,12 @@ extern struct ActuatorsAsctec actuators_asctec;
     actuators_asctec.cur_addr = _v;        \
   }
 
+extern void actuators_asctec_init(void);
+extern void actuators_asctec_set(bool_t motors_on);
+
+#define ActuatorAsctecSet(_i, _v) { actuators_asctec.cmds[_i] = _v; }
+#define ActuatorsAsctecInit() actuators_asctec_init()
+#define ActuatorsAsctecCommit() actuators_asctec_set(autopilot_motors_on)
+
 
 #endif /* ACTUATORS_ASCTEC_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.c b/sw/airborne/subsystems/actuators/actuators_pwm.c
index 8bde49ea92..a5c432d3fe 100644
--- a/sw/airborne/subsystems/actuators/actuators_pwm.c
+++ b/sw/airborne/subsystems/actuators/actuators_pwm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 The Paparazzi Team
+ * Copyright (C) 2010-2012 The Paparazzi Team
  *
  * This file is part of Paparazzi.
  *
@@ -21,8 +21,5 @@
 
 #include "actuators_pwm.h"
 
-int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
+int32_t actuators_pwm_values[SERVOS_PWM_NB];
 
-void actuators_init(void) {
-  actuators_pwm_arch_init();
-}
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.h b/sw/airborne/subsystems/actuators/actuators_pwm.h
index abd83f7df4..cae834ffea 100644
--- a/sw/airborne/subsystems/actuators/actuators_pwm.h
+++ b/sw/airborne/subsystems/actuators/actuators_pwm.h
@@ -23,14 +23,11 @@
 #define ACTUATORS_PWM_H
 
 #include "std.h"
-#include "actuators.h"
+#include "generated/airframe.h"
 
 #include BOARD_CONFIG
 #include "subsystems/actuators/actuators_pwm_arch.h"
 
-extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
-
-#define Actuator(_x)  actuators_pwm_values[_x]
-#define ChopServo(_x,_a,_b) Chop(_x, _a, _b)
+extern int32_t actuators_pwm_values[SERVOS_PWM_NB];
 
 #endif /* ACTUATORS_PWM_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_skiron.c b/sw/airborne/subsystems/actuators/actuators_skiron.c
index b7a3aec3fa..79182faa6e 100644
--- a/sw/airborne/subsystems/actuators/actuators_skiron.c
+++ b/sw/airborne/subsystems/actuators/actuators_skiron.c
@@ -23,31 +23,25 @@
  *  Skiron motor speed controller by Michel.
  */
 
-#include "firmwares/rotorcraft/actuators.h"
-#include "firmwares/rotorcraft/actuators/actuators_skiron.h"
+#include "subsystems/actuators.h"
+#include "subsystems/actuators/actuators_skiron.h"
 
-#include "firmwares/rotorcraft/commands.h"
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/sys_time.h"
 
-
 struct ActuatorsSkiron actuators_skiron;
 
 
 uint32_t actuators_delay_time;
 bool_t   actuators_delay_done;
 
-void actuators_init(void) {
+void actuators_skiron_init(void) {
 
   supervision_init();
   actuators_skiron.trans.type = I2CTransTx;
-  actuators_skiron.trans.len_w = ACTUATORS_SKIRON_NB;
+  actuators_skiron.trans.len_w = SERVOS_SKIRON_NB;
   actuators_skiron.trans.slave_addr = ACTUATORS_SKIRON_I2C_ADDR;
   actuators_skiron.trans.status = I2CTransDone;
-  const uint8_t actuators_idx[ACTUATORS_SKIRON_NB] = ACTUATORS_SKIRON_IDX;
-  for (uint8_t i=0; i<ACTUATORS_SKIRON_NB; i++) {
-    actuators_skiron.actuators_idx[i] = actuators_idx[i];
-  }
 
 #if defined ACTUATORS_START_DELAY && ! defined SITL
   actuators_delay_done = FALSE;
@@ -59,7 +53,7 @@ void actuators_init(void) {
 
 }
 
-void actuators_set(bool_t motors_on) {
+void actuators_skiron_set(void) {
 #if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
     if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
@@ -67,14 +61,11 @@ void actuators_set(bool_t motors_on) {
   }
 #endif
 
-  supervision_run(motors_on, FALSE, commands);
-  for (uint8_t i=0; i<ACTUATORS_SKIRON_NB; i++) {
-    uint8_t idx = actuators_skiron.actuators_idx[i];
 #ifdef KILL_MOTORS
-    actuators_skiron.trans.buf[idx] = 0;
-#else
-    actuators_skiron.trans.buf[idx] = supervision.commands[i];
-#endif
+  for (uint8_t i=0; i<ACTUATORS_SKIRON_NB; i++) {
+    actuators_skiron.trans.buf[i] = 0;
   }
+#endif
+
   i2c_submit(&ACTUATORS_SKIRON_DEVICE, &actuators_skiron.trans);
 }
diff --git a/sw/airborne/subsystems/actuators/actuators_skiron.h b/sw/airborne/subsystems/actuators/actuators_skiron.h
index 04c559b6c7..b1a6b2906f 100644
--- a/sw/airborne/subsystems/actuators/actuators_skiron.h
+++ b/sw/airborne/subsystems/actuators/actuators_skiron.h
@@ -30,17 +30,24 @@
 #include "mcu_periph/i2c.h"
 
 #include "generated/airframe.h"
+#include "subsystems/actuators/supervision.h"
+
 
 // Use I2C broadcast adderss
 #define ACTUATORS_SKIRON_I2C_ADDR 0x00
 
 struct ActuatorsSkiron {
   struct i2c_transaction trans;
-  uint8_t actuators_idx[ACTUATORS_SKIRON_NB];
+  //uint8_t actuators_idx[ACTUATORS_SKIRON_NB];
 };
 
 extern struct ActuatorsSkiron actuators_skiron;
 
-#include "firmwares/rotorcraft/actuators/supervision.h"
+extern void actuators_skiron_init(void);
+extern void actuators_skiron_set(void);
+
+#define ActuatorSkironSet(_i, _v) { actuators_skiron.trans.buf[_i] = _v; }
+#define ActuatorsSkironInit() actuators_skiron_init()
+#define ActuatorsSkironCommit() actuators_skiron_set()
 
 #endif /* ACTUATORS_SKIRON_H */
diff --git a/sw/airborne/subsystems/actuators/supervision.c b/sw/airborne/subsystems/actuators/supervision.c
index 6a020617ec..dde7e1f7b7 100644
--- a/sw/airborne/subsystems/actuators/supervision.c
+++ b/sw/airborne/subsystems/actuators/supervision.c
@@ -25,7 +25,7 @@
  *  to actual motor commands.
  */
 
-#include "firmwares/rotorcraft/actuators/supervision.h"
+#include "subsystems/actuators/supervision.h"
 #include "paparazzi.h"
 
 //#include <stdint.h>
@@ -37,6 +37,9 @@
 #define INT32_MAX (2147483647)
 #endif
 
+#define SUPERVISION_MIN_MOTOR 0
+#define SUPERVISION_MAX_MOTOR MAX_PPRZ
+
 #ifndef SUPERVISION_STOP_MOTOR
 #define SUPERVISION_STOP_MOTOR 0
 #endif
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index 677ffbd8a2..90fe069733 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -165,10 +165,10 @@ let parse_command_laws = fun command ->
        printf "  command_value = %s;\\\n" v;
        printf "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP : SERVO_%s_TRAVEL_DOWN;\\\n" servo servo;
        printf "  servo_value = SERVO_%s_NEUTRAL + (int32_t)(command_value);\\\n" servo;
-       printf "  actuators[SERVO_%s_IDX] = ChopServo(servo_value, SERVO_%s_MIN, SERVO_%s_MAX);\\\n\\\n" servo servo servo;
+       printf "  actuators[SERVO_%s_IDX] = Chop(servo_value, SERVO_%s_MIN, SERVO_%s_MAX);\\\n\\\n" servo servo servo;
 
        let driver = get_servo_driver servo in
-       printf "  Actuator%s(SERVO_%s) = SERVOS_TICS_OF_USEC(actuators[SERVO_%s_IDX]);\\\n\\\n" driver servo servo
+       printf "  Actuator%sSet(SERVO_%s,actuators[SERVO_%s_IDX]);\\\n\\\n" driver servo servo
    | "let" ->
        let var = a "var"
        and value = a "value" in

From 38d3e71acad4f974eeb8bb88c5b323270b618192 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 15 Oct 2012 18:17:43 +0200
Subject: [PATCH 03/26] [actuators] first test with 2 drivers

- actuators are now in signed int16 (need to check size again) (int32 ?)
- servo_switch module replaces booz_drop
- booz_pwm is now handled by actuators_direct
---
 conf/airframes/ENAC/quadrotor/booz2_g1.xml    | 25 +++-----
 conf/messages.xml                             |  2 +-
 conf/modules/booz_drop.xml                    | 24 --------
 conf/modules/booz_pwm.xml                     | 22 -------
 conf/modules/servo_switch.xml                 | 12 ++--
 .../arch/lpc21/modules/core/booz_pwm_arch.c   | 60 -------------------
 .../arch/lpc21/modules/core/booz_pwm_arch.h   | 46 --------------
 sw/airborne/firmwares/rotorcraft/telemetry.h  |  6 ++
 sw/airborne/modules/drop/booz_drop.c          | 54 -----------------
 sw/airborne/modules/drop/booz_drop.h          | 38 ------------
 .../modules/servo_switch/servo_switch.c       |  9 +--
 .../modules/servo_switch/servo_switch.h       |  6 +-
 sw/airborne/subsystems/actuators.c            |  2 +-
 sw/airborne/subsystems/actuators.h            |  2 +-
 sw/tools/gen_airframe.ml                      |  2 +-
 15 files changed, 33 insertions(+), 277 deletions(-)
 delete mode 100644 conf/modules/booz_drop.xml
 delete mode 100644 conf/modules/booz_pwm.xml
 delete mode 100644 sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.c
 delete mode 100644 sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.h
 delete mode 100644 sw/airborne/modules/drop/booz_drop.c
 delete mode 100644 sw/airborne/modules/drop/booz_drop.h

diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index bfdaa7420a..379728374c 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -1,11 +1,8 @@
 <airframe name="BOOZ2_G1">
 
   <modules main_freq="512">
-    <!--load name="booz_pwm.xml">
-      <define name="USE_PWM1"/>
-    </load>
-    <load name="booz_drop.xml"/>
-    <load name="booz_cam.xml"/-->
+    <load name="servo_switch.xml"/>
+    <!--load name="booz_cam.xml"/-->
     <!--load name="sonar_maxbotix_booz.xml"/-->
     <!--load name="adc_generic_booz.xml"/-->
     <!--load name="sys_mon.xml"/-->
@@ -54,8 +51,8 @@
   </servos>
 
   <servos driver="Pwm">
-    <servo name="CAM" no="0" min="1000" neutral="1500" max="2000"/>
-    <servo name="DROP" no="5" min="1000" neutral="1500" max="2000"/>
+    <servo name="CAM" no="5" min="1000" neutral="1500" max="2000"/>
+    <servo name="SWITCH" no="0" min="2120" neutral="1500" max="1060"/>
   </servos>
 
   <commands>
@@ -63,13 +60,15 @@
     <axis name="ROLL"   failsafe_value="0"/>
     <axis name="YAW"    failsafe_value="0"/>
     <axis name="THRUST" failsafe_value="0"/>
+    <axis name="SWITCH" failsafe_value="0"/>
   </commands>
 
   <command_laws>
-    <set servo="PITCH" value="@PITCH"/>
-    <set servo="ROLL" value="@ROLL"/>
-    <set servo="YAW" value="@YAW"/>
+    <set servo="PITCH" value="@PITCH - TRIM_PITCH"/>
+    <set servo="ROLL" value="@ROLL - TRIM_ROLL"/>
+    <set servo="YAW" value="@YAW - TRIM_YAW"/>
     <set servo="THRUST" value="@THRUST"/>
+    <set servo="SWITCH" value="@SWITCH"/>
   </command_laws>
 
   <section name="IMU" prefix="IMU_">
@@ -230,12 +229,6 @@
     <define name="SetPwm(_v)" value="BoozSetPwm1Value(_v)"/>
   </section>
 
-  <section name="DROP">
-    <define name="DROP_SERVO_CLOSED" value="2120"/>
-    <define name="DROP_SERVO_OPEN" value="1060"/>
-    <!--define name="BoozDropPwm(_v)" value="BoozSetPwm0Value(_v)"/-->
-  </section>
-
   <section name="MISC">
     <define name="BOOZ_ANALOG_BARO_THRESHOLD" value="800"/>
     <define name="FACE_REINJ_1"  value="1024"/>
diff --git a/conf/messages.xml b/conf/messages.xml
index 3322b460b5..51db383031 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -737,7 +737,7 @@
   </message>
 
   <message name="ACTUATORS" id="105">
-    <field name="values" type="uint16[]" unit="none"/>
+    <field name="values" type="int16[]" unit="none"/>
   </message>
 
   <message name="BETH" id="106">
diff --git a/conf/modules/booz_drop.xml b/conf/modules/booz_drop.xml
deleted file mode 100644
index 2b3355bb72..0000000000
--- a/conf/modules/booz_drop.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-<!--
-     DROP_SERVO_CLOSED and DROP_SERVO_OPEN can be redefined in airframe file
-     units: PWM width in us
-     -->
-
-<module name="drop">
-  <doc>
-    <description>Drop control for BOOZ board</description>
-    <define name="DROP_SERVO_OPEN" value="pwm" description="PWM value when servo is in open position in us (optional)"/>
-    <define name="DROP_SERVO_CLOSED" value="pwm" description="PWM value when servo is in closed position in us (optional)"/>
-  </doc>
-  <!-- depend require="booz_pwm" -->
-  <header>
-    <file name="booz_drop.h"/>
-  </header>
-  <init fun="booz_drop_init()"/>
-  <periodic fun="booz_drop_periodic()" freq="10."/>
-  <makefile>
-    <define name="USE_DROP"/>
-    <file name="booz_drop.c"/>
-  </makefile>
-</module>
-
diff --git a/conf/modules/booz_pwm.xml b/conf/modules/booz_pwm.xml
deleted file mode 100644
index 422c7f0301..0000000000
--- a/conf/modules/booz_pwm.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-
-  <!--
-     Allow to use PWM output on booz boards
-     PWM0 is activated by default (PWM5 on lpc21 output)
-     PWM1 can be activated with USE_PWM1 flag (PWM2 on lpc21 output)
-   -->
-
-<module name="booz_pwm" dir="core">
-  <doc>
-    <description>Allow to use PWM output on booz boards, PWM0 is activated by default (PWM5 on lpc21 output)</description>
-    <define name="USE_PWM1" description="flag to activate with USE_PWM1 flag (PWM2 on lpc21 output)"/>
-  </doc>
-  <header>
-    <file name="booz_pwm_arch.h"/>
-  </header>
-  <init fun="booz_pwm_init_arch()"/>
-  <makefile target="ap">
-    <file_arch name="booz_pwm_arch.c"/>
-  </makefile>
-</module>
-
diff --git a/conf/modules/servo_switch.xml b/conf/modules/servo_switch.xml
index 175747143c..0c175a2e04 100644
--- a/conf/modules/servo_switch.xml
+++ b/conf/modules/servo_switch.xml
@@ -5,7 +5,7 @@
     <description>Servo switch</description>
     <define name="SERVO_SWITCH_ON_VALUE"  value="pwm" description="servo value in usec"/>
     <define name="SERVO_SWITCH_OFF_VALUE" value="pwm" description="servo value in usec"/>
-    <define name="SERVO_SWITCH_SERVO"     value="nb" description="Id of the servo to use"/>
+    <define name="SERVO_SWITCH_SERVO"     value="nb" description="Id of the switch command (default: COMMAND_SWITCH)"/>
   </doc>
   <header>
     <file name="servo_switch.h"/>
@@ -17,10 +17,14 @@
 <!-- these parameters should be set for that module in the airframe file unless you want the defaults
      Servo value in usec
   <load name="servo_switch.xml">
-    <define name="SERVO_SWITCH_ON_VALUE"  value="2000"/>
-    <define name="SERVO_SWITCH_OFF_VALUE" value="1000"/>
-    <define name="SERVO_SWITCH_SERVO"     value="SERVO_SWITCH"/>
+    <define name="SERVO_SWITCH_ON_VALUE"  value="MAX_PPRZ"/>
+    <define name="SERVO_SWITCH_OFF_VALUE" value="-MAX_PPRZ"/>
+    <define name="SERVO_SWITCH_SERVO"     value="COMMAND_SWITCH"/>
   </load>
+  
+  The SWITCH servo, the SWITCH command and the command_law (usually <set servo="SWITCH" value="@SWITCH"/>
+  must be defined correctly
+  The min and max positions of the servo are defined in the servo section
 -->
 
     <file name="servo_switch.c"/>
diff --git a/sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.c b/sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.c
deleted file mode 100644
index 45579d7992..0000000000
--- a/sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.c
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2011  ENAC
- *
- * 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.
- *
- */
-
-#include "modules/core/booz_pwm_arch.h"
-
-#define PWM_PERIOD CPU_TICKS_OF_USEC(20000)
-#define PWM_DUTY CPU_TICKS_OF_USEC(1500)
-
-void booz_pwm_init_arch( void ) {
-
-  /* start PWM5 */
-  /* select P0.21 as PWM5 */
-  PWM0_PINSEL |=  PWM0_PINSEL_VAL << PWM0_PINSEL_BIT;
-  /* select pwm period */
-  PWMMR0 = PWM_PERIOD;
-  /* select pwm value to 50% at init (1500 us) */
-  PWMMR5 = PWM_DUTY;
-  /* commit values */
-  PWMLER = PWMLER_LATCH0 | PWMLER_LATCH5;
-  /* prescle timer to match TIMER 0 (15MHz) */
-  PWMPR = T0_PCLK_DIV - 1;
-  /* enable counter and pwm mode */
-  PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
-  /* enable PWM5 */
-  PWMPCR = PWMPCR_ENA5;
-
-#ifdef USE_PWM1
-  /* start PWM2 */
-  PWM1_PINSEL |=  PWM1_PINSEL_VAL << PWM1_PINSEL_BIT;
-  /* select pwm value to 50% at init (1500 us) */
-  PWMMR2 = PWM_DUTY;
-  /* commit values */
-  PWMLER = PWMLER_LATCH2;
-  /* enable PWM2 */
-  PWMPCR |= PWMPCR_ENA2;
-#endif
-
-
-}
diff --git a/sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.h b/sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.h
deleted file mode 100644
index 567b5793fa..0000000000
--- a/sw/airborne/arch/lpc21/modules/core/booz_pwm_arch.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2011  ENAC
- *
- * 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.
- *
- */
-
-#ifndef BOOZ_PWM_ARCH_H
-#define BOOZ_PWM_ARCH_H
-
-#include "std.h"
-#include "mcu_periph/sys_time.h"
-
-extern void booz_pwm_init_arch(void);
-
-// Default PWM is PWM0
-#define BoozSetPwmValue(_v) BoozSetPwm0Value(_v)
-
-#define BoozSetPwm0Value(_v) { \
-  PWMMR5 = CPU_TICKS_OF_USEC(_v); \
-  PWMLER = PWMLER_LATCH5; \
-}
-
-#define BoozSetPwm1Value(_v) { \
-  PWMMR2 = CPU_TICKS_OF_USEC(_v); \
-  PWMLER = PWMLER_LATCH2; \
-}
-
-#endif /* BOOZ_PWM_ARCH_H */
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 12161a08fe..ba7d1f08a1 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -136,6 +136,12 @@
 #define PERIODIC_SEND_PPM(_trans, _dev) {}
 #endif
 
+#ifdef ACTUATORS
+#define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, ACTUATORS_NB, actuators)
+#else
+#define PERIODIC_SEND_ACTUATORS(_trans, _dev) {}
+#endif
+
 #define PERIODIC_SEND_IMU_GYRO_SCALED(_trans, _dev) {		\
     DOWNLINK_SEND_IMU_GYRO_SCALED(_trans, _dev,			\
                  &imu.gyro.p,		\
diff --git a/sw/airborne/modules/drop/booz_drop.c b/sw/airborne/modules/drop/booz_drop.c
deleted file mode 100644
index 206c8a09f3..0000000000
--- a/sw/airborne/modules/drop/booz_drop.c
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2009 Flixr
- *
- * 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.
- */
-
-#include "modules/drop/booz_drop.h"
-#include "modules/core/booz_pwm_arch.h"
-#include "generated/airframe.h"
-
-bool_t booz_drop_ball;
-int16_t booz_drop_servo;
-
-#ifndef DROP_SERVO_OPEN
-#define DROP_SERVO_OPEN 1700
-#endif
-#ifndef DROP_SERVO_CLOSED
-#define DROP_SERVO_CLOSED 900
-#endif
-
-// PWM definition
-#ifndef BoozDropPwm
-#define BoozDropPwm(_v) BoozSetPwmValue(_v)
-#endif
-
-void booz_drop_init(void) {
-  booz_drop_ball = FALSE;
-  booz_drop_periodic();
-}
-
-void booz_drop_periodic(void) {
-  if (booz_drop_ball == TRUE)
-    booz_drop_servo = DROP_SERVO_OPEN;
-  else
-    booz_drop_servo = DROP_SERVO_CLOSED;
-  BoozDropPwm(booz_drop_servo);
-}
diff --git a/sw/airborne/modules/drop/booz_drop.h b/sw/airborne/modules/drop/booz_drop.h
deleted file mode 100644
index c1b0182305..0000000000
--- a/sw/airborne/modules/drop/booz_drop.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2009 Flixr
- *
- * 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.
- */
-
-
-#ifndef BOOZ_DROP_H
-#define BOOZ_DROP_H
-
-#include "std.h"
-
-extern bool_t booz_drop_ball;
-
-extern void booz_drop_init(void);
-extern void booz_drop_periodic(void);
-
-#define NavDropNow() ({ booz_drop_ball = TRUE; FALSE; })
-#define NavDropClose() ({ booz_drop_ball = FALSE; FALSE; })
-
-#endif  /* BOOZ_DROP_H */
diff --git a/sw/airborne/modules/servo_switch/servo_switch.c b/sw/airborne/modules/servo_switch/servo_switch.c
index 2d90ae53e6..568dee2ecb 100644
--- a/sw/airborne/modules/servo_switch/servo_switch.c
+++ b/sw/airborne/modules/servo_switch/servo_switch.c
@@ -1,6 +1,4 @@
 /*
- * $Id: $
- *
  * Copyright (C) 2010 Flixr
  *
  * This file is part of paparazzi.
@@ -23,10 +21,9 @@
 
 #include "servo_switch/servo_switch.h"
 #include "generated/airframe.h"
-#include "actuators.h"
+#include "firmwares/rotorcraft/commands.h"
 
 bool_t servo_switch_on;
-int16_t servo_switch_value;
 
 void servo_switch_init(void) {
   servo_switch_on = FALSE;
@@ -35,7 +32,7 @@ void servo_switch_init(void) {
 
 void servo_switch_periodic(void) {
   if (servo_switch_on == TRUE)
-    SetServo(SERVO_SWITCH_SERVO, SERVO_SWITCH_ON_VALUE)
+    commands[SERVO_SWITCH_SERVO] = SERVO_SWITCH_ON_VALUE;
   else
-    SetServo(SERVO_SWITCH_SERVO, SERVO_SWITCH_OFF_VALUE)
+    commands[SERVO_SWITCH_SERVO] = SERVO_SWITCH_OFF_VALUE;
 }
diff --git a/sw/airborne/modules/servo_switch/servo_switch.h b/sw/airborne/modules/servo_switch/servo_switch.h
index 24a5a60da2..0edbf0bd72 100644
--- a/sw/airborne/modules/servo_switch/servo_switch.h
+++ b/sw/airborne/modules/servo_switch/servo_switch.h
@@ -31,13 +31,13 @@ extern bool_t servo_switch_on;
 extern int16_t servo_switch_value;
 
 #ifndef SERVO_SWITCH_ON_VALUE
-#define SERVO_SWITCH_ON_VALUE 2000
+#define SERVO_SWITCH_ON_VALUE MAX_PPRZ
 #endif
 #ifndef SERVO_SWITCH_OFF_VALUE
-#define SERVO_SWITCH_OFF_VALUE 1000
+#define SERVO_SWITCH_OFF_VALUE -MAX_PPRZ
 #endif
 #ifndef SERVO_SWITCH_SERVO
-#define SERVO_SWITCH_SERVO SERVO_SWITCH
+#define SERVO_SWITCH_SERVO COMMAND_SWITCH
 #endif
 
 
diff --git a/sw/airborne/subsystems/actuators.c b/sw/airborne/subsystems/actuators.c
index 907e24ad9d..a2789ca768 100644
--- a/sw/airborne/subsystems/actuators.c
+++ b/sw/airborne/subsystems/actuators.c
@@ -29,7 +29,7 @@
 
 #if ACTUATORS_NB
 
-uint16_t actuators[ACTUATORS_NB];
+int16_t actuators[ACTUATORS_NB];
 
 void actuators_init(void) {
 
diff --git a/sw/airborne/subsystems/actuators.h b/sw/airborne/subsystems/actuators.h
index 7a71c96bc6..f06950634c 100644
--- a/sw/airborne/subsystems/actuators.h
+++ b/sw/airborne/subsystems/actuators.h
@@ -40,7 +40,7 @@
 extern void actuators_init(void);
 
 /** Temporary storage (for debugging purpose, downlinked via telemetry) */
-extern uint16_t actuators[ACTUATORS_NB];
+extern int16_t actuators[ACTUATORS_NB];
 
 #endif /* ACTUATORS_NB */
 
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index 90fe069733..e1837a5459 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -258,7 +258,7 @@ let rec parse_section = fun s ->
       print_actuators_idx ();
 
       printf "#define SetActuatorsFromCommands(values) { \\\n";
-      printf "  uint32_t servo_value;\\\n";
+      printf "  int32_t servo_value;\\\n";
       printf "  float command_value;\\\n";
 
       List.iter parse_command_laws (Xml.children s);

From a7f888147291edbab4cef6b3da5fa2177253db23 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 16 Oct 2012 13:22:55 +0200
Subject: [PATCH 04/26] [actuators] servo_switch directly controls actuators
 again

---
 conf/airframes/ENAC/quadrotor/booz2_g1.xml    |  4 +--
 conf/modules/servo_switch.xml                 | 12 ++++----
 .../modules/servo_switch/servo_switch.c       | 16 ++++++----
 .../modules/servo_switch/servo_switch.h       |  6 ++--
 sw/airborne/subsystems/actuators.h            | 17 ++++++++---
 sw/tools/gen_airframe.ml                      | 29 +++++++++++--------
 6 files changed, 50 insertions(+), 34 deletions(-)

diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 379728374c..0298610a0a 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -52,7 +52,7 @@
 
   <servos driver="Pwm">
     <servo name="CAM" no="5" min="1000" neutral="1500" max="2000"/>
-    <servo name="SWITCH" no="0" min="2120" neutral="1500" max="1060"/>
+    <servo name="SWITCH" no="0" min="1060" neutral="1500" max="2120"/>
   </servos>
 
   <commands>
@@ -60,7 +60,6 @@
     <axis name="ROLL"   failsafe_value="0"/>
     <axis name="YAW"    failsafe_value="0"/>
     <axis name="THRUST" failsafe_value="0"/>
-    <axis name="SWITCH" failsafe_value="0"/>
   </commands>
 
   <command_laws>
@@ -68,7 +67,6 @@
     <set servo="ROLL" value="@ROLL - TRIM_ROLL"/>
     <set servo="YAW" value="@YAW - TRIM_YAW"/>
     <set servo="THRUST" value="@THRUST"/>
-    <set servo="SWITCH" value="@SWITCH"/>
   </command_laws>
 
   <section name="IMU" prefix="IMU_">
diff --git a/conf/modules/servo_switch.xml b/conf/modules/servo_switch.xml
index 0c175a2e04..3562dd93ad 100644
--- a/conf/modules/servo_switch.xml
+++ b/conf/modules/servo_switch.xml
@@ -5,7 +5,7 @@
     <description>Servo switch</description>
     <define name="SERVO_SWITCH_ON_VALUE"  value="pwm" description="servo value in usec"/>
     <define name="SERVO_SWITCH_OFF_VALUE" value="pwm" description="servo value in usec"/>
-    <define name="SERVO_SWITCH_SERVO"     value="nb" description="Id of the switch command (default: COMMAND_SWITCH)"/>
+    <define name="SERVO_SWITCH_SERVO"     value="nb" description="Id of the servo (default: SWITCH)"/>
   </doc>
   <header>
     <file name="servo_switch.h"/>
@@ -17,14 +17,12 @@
 <!-- these parameters should be set for that module in the airframe file unless you want the defaults
      Servo value in usec
   <load name="servo_switch.xml">
-    <define name="SERVO_SWITCH_ON_VALUE"  value="MAX_PPRZ"/>
-    <define name="SERVO_SWITCH_OFF_VALUE" value="-MAX_PPRZ"/>
-    <define name="SERVO_SWITCH_SERVO"     value="COMMAND_SWITCH"/>
+    <define name="SERVO_SWITCH_ON_VALUE"  value="2000"/>
+    <define name="SERVO_SWITCH_OFF_VALUE" value="1000"/>
+    <define name="SERVO_SWITCH_SERVO"     value="SWITCH"/>
   </load>
   
-  The SWITCH servo, the SWITCH command and the command_law (usually <set servo="SWITCH" value="@SWITCH"/>
-  must be defined correctly
-  The min and max positions of the servo are defined in the servo section
+  The servo will not exceed the min and max positions defined in the servo section
 -->
 
     <file name="servo_switch.c"/>
diff --git a/sw/airborne/modules/servo_switch/servo_switch.c b/sw/airborne/modules/servo_switch/servo_switch.c
index 568dee2ecb..d4a37adec7 100644
--- a/sw/airborne/modules/servo_switch/servo_switch.c
+++ b/sw/airborne/modules/servo_switch/servo_switch.c
@@ -21,18 +21,24 @@
 
 #include "servo_switch/servo_switch.h"
 #include "generated/airframe.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/actuators.h"
 
 bool_t servo_switch_on;
 
+// One level of macro stack to allow redefinition of the default servo
+#define _ServoSwitch(_n, _v) ActuatorSet(_n, _v)
+#define ServoSwitch(_n, _v) _ServoSwitch(_n, _v)
+
 void servo_switch_init(void) {
   servo_switch_on = FALSE;
   servo_switch_periodic();
 }
 
 void servo_switch_periodic(void) {
-  if (servo_switch_on == TRUE)
-    commands[SERVO_SWITCH_SERVO] = SERVO_SWITCH_ON_VALUE;
-  else
-    commands[SERVO_SWITCH_SERVO] = SERVO_SWITCH_OFF_VALUE;
+  if (servo_switch_on == TRUE) {
+    ServoSwitch(SERVO_SWITCH_SERVO, SERVO_SWITCH_ON_VALUE);
+  }
+  else {
+    ServoSwitch(SERVO_SWITCH_SERVO, SERVO_SWITCH_OFF_VALUE);
+  }
 }
diff --git a/sw/airborne/modules/servo_switch/servo_switch.h b/sw/airborne/modules/servo_switch/servo_switch.h
index 0edbf0bd72..c0a3ed7450 100644
--- a/sw/airborne/modules/servo_switch/servo_switch.h
+++ b/sw/airborne/modules/servo_switch/servo_switch.h
@@ -31,13 +31,13 @@ extern bool_t servo_switch_on;
 extern int16_t servo_switch_value;
 
 #ifndef SERVO_SWITCH_ON_VALUE
-#define SERVO_SWITCH_ON_VALUE MAX_PPRZ
+#define SERVO_SWITCH_ON_VALUE 2000
 #endif
 #ifndef SERVO_SWITCH_OFF_VALUE
-#define SERVO_SWITCH_OFF_VALUE -MAX_PPRZ
+#define SERVO_SWITCH_OFF_VALUE 1000
 #endif
 #ifndef SERVO_SWITCH_SERVO
-#define SERVO_SWITCH_SERVO COMMAND_SWITCH
+#define SERVO_SWITCH_SERVO SWITCH
 #endif
 
 
diff --git a/sw/airborne/subsystems/actuators.h b/sw/airborne/subsystems/actuators.h
index f06950634c..f742d02281 100644
--- a/sw/airborne/subsystems/actuators.h
+++ b/sw/airborne/subsystems/actuators.h
@@ -29,9 +29,10 @@
 
 #include "paparazzi.h"
 
-/** Defines SetActuatorsFromCommands() macro
- *  Defines ACTUATORS_NB to 0 if no servo
- *  Include servos drivers
+/*
+ * Defines SetActuatorsFromCommands() macro
+ * Defines ACTUATORS_NB to 0 if no servo
+ * Include servos drivers
  */
 #include "generated/airframe.h"
 
@@ -39,9 +40,17 @@
 
 extern void actuators_init(void);
 
-/** Temporary storage (for debugging purpose, downlinked via telemetry) */
+/** Actuators array.
+ * Temporary storage (for debugging purpose, downlinked via telemetry)
+ * */
 extern int16_t actuators[ACTUATORS_NB];
 
+/** Set actuators.
+ * @param _n actuators name as given in airframe file, servos section
+ * @param _v new actuator's value
+ */
+#define ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v)
+
 #endif /* ACTUATORS_NB */
 
 
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index e1837a5459..c3da4c988f 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -151,7 +151,15 @@ let preprocess_value = fun s v prefix ->
   Str.global_replace var_value "_var_\\1" s
 
 let print_actuators_idx = fun () ->
-  let nb = Hashtbl.fold (fun s _d i -> printf "#define SERVO_%s_IDX %d\n" s i; i+1) servos_drivers 0 in
+  let nb = Hashtbl.fold (fun s d i ->
+    printf "#define SERVO_%s_IDX %d\n" s i;
+    (* Set servo macro *)
+    printf "#define Set_%s_Servo(_v) { \\\n" s;
+    printf "  actuators[SERVO_%s_IDX] = Chop(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s;
+    printf "  Actuator%sSet(SERVO_%s, actuators[SERVO_%s_IDX]); \\\n" d s s;
+    printf "}\n\n";
+    i+1
+  ) servos_drivers 0 in
   define "ACTUATORS_NB" (string_of_int nb);
   nl ()
 
@@ -162,28 +170,25 @@ let parse_command_laws = fun command ->
        let servo = a "servo"
        and value = a "value" in
        let v = preprocess_value value "values" "COMMAND" in
-       printf "  command_value = %s;\\\n" v;
-       printf "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP : SERVO_%s_TRAVEL_DOWN;\\\n" servo servo;
-       printf "  servo_value = SERVO_%s_NEUTRAL + (int32_t)(command_value);\\\n" servo;
-       printf "  actuators[SERVO_%s_IDX] = Chop(servo_value, SERVO_%s_MIN, SERVO_%s_MAX);\\\n\\\n" servo servo servo;
-
-       let driver = get_servo_driver servo in
-       printf "  Actuator%sSet(SERVO_%s,actuators[SERVO_%s_IDX]);\\\n\\\n" driver servo servo
+       printf "  command_value = %s; \\\n" v;
+       printf "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP : SERVO_%s_TRAVEL_DOWN; \\\n" servo servo;
+       printf "  servo_value = SERVO_%s_NEUTRAL + (int32_t)(command_value); \\\n" servo;
+       printf "  Set_%s_Servo(servo_value); \\\n\\\n" servo
    | "let" ->
        let var = a "var"
        and value = a "value" in
        let v = preprocess_value value "values" "COMMAND" in
-       printf "  int16_t _var_%s = %s;\\\n" var v
+       printf "  int16_t _var_%s = %s; \\\n" var v
    | "call" ->
        let f = a "fun" in
-       printf "  %s;\\\n" f
+       printf "  %s; \\\n" f
    | "ratelimit" ->
        let var = a "var"
        and value = a "value"
        and rate_min = a "rate_min"
        and rate_max = a "rate_max" in
        let v = preprocess_value value "values" "COMMAND" in
-       printf "  static int16_t _var_%s = 0; _var_%s += Chop((%s) - (_var_%s), (%s), (%s));\\\n" var var v var rate_min rate_max
+       printf "  static int16_t _var_%s = 0; _var_%s += Chop((%s) - (_var_%s), (%s), (%s)); \\\n" var var v var rate_min rate_max
    | "define" ->
        parse_element "" command
    | _ -> xml_error "set|let"
@@ -259,7 +264,7 @@ let rec parse_section = fun s ->
 
       printf "#define SetActuatorsFromCommands(values) { \\\n";
       printf "  int32_t servo_value;\\\n";
-      printf "  float command_value;\\\n";
+      printf "  float command_value;\\\n\\\n";
 
       List.iter parse_command_laws (Xml.children s);
 

From 8679d6030e067ebe32af54a053b19020df6d75d7 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 16 Oct 2012 14:39:58 +0200
Subject: [PATCH 05/26] [actuators] dummy actuators driver (really needed ?)

---
 .../subsystems/rotorcraft/actuators_dummy.makefile |  5 -----
 .../{actuators_dummy.c => actuators_dummy.h}       | 14 +++++++-------
 2 files changed, 7 insertions(+), 12 deletions(-)
 delete mode 100644 conf/firmwares/subsystems/rotorcraft/actuators_dummy.makefile
 rename sw/airborne/subsystems/actuators/{actuators_dummy.c => actuators_dummy.h} (78%)

diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_dummy.makefile b/conf/firmwares/subsystems/rotorcraft/actuators_dummy.makefile
deleted file mode 100644
index 5c6b93839a..0000000000
--- a/conf/firmwares/subsystems/rotorcraft/actuators_dummy.makefile
+++ /dev/null
@@ -1,5 +0,0 @@
-#
-# empty dummy actuators for testing
-#
-
-ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_dummy.c
diff --git a/sw/airborne/subsystems/actuators/actuators_dummy.c b/sw/airborne/subsystems/actuators/actuators_dummy.h
similarity index 78%
rename from sw/airborne/subsystems/actuators/actuators_dummy.c
rename to sw/airborne/subsystems/actuators/actuators_dummy.h
index d8cbc1e706..5148c7b7b7 100644
--- a/sw/airborne/subsystems/actuators/actuators_dummy.c
+++ b/sw/airborne/subsystems/actuators/actuators_dummy.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 Gautier Hattenberger
  *
  * This file is part of paparazzi.
  *
@@ -19,11 +19,11 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#include "firmwares/rotorcraft/actuators.h"
+#ifndef ACTUATORS_DUMMY_H
+#define ACTUATORS_DUMMY_H
 
+#define ActuatorDummySet(_i, _v) {}
+#define ActuatorsDummyInit() {}
+#define ActuatorsDummyCommit() {}
 
-void actuators_init(void) {
-}
-
-void actuators_set(bool_t motors_on) {
-}
+#endif

From d7a80cce92bee278175dcacb96945b0f8d3ad138 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 16 Oct 2012 14:44:56 +0200
Subject: [PATCH 06/26] [actuators] reordering pwm_direct servos so it makes
 some sense

---
 .../subsystems/actuators/actuators_pwm_arch.h | 79 +++++++++++--------
 sw/airborne/boards/booz_1.0.h                 | 13 ++-
 sw/airborne/boards/navgo_1.0.h                | 12 ++-
 3 files changed, 63 insertions(+), 41 deletions(-)

diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
index 3a23cedbaf..c8ba69b539 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
@@ -4,18 +4,6 @@
 #include "LPC21xx.h"
 #include "mcu_periph/sys_time.h"
 
-#define SERVOS_TICS_OF_USEC(s) CPU_TICKS_OF_USEC(s)
-
-#define SERVO_REG_0 PWMMR5
-#define SERVO_REG_1 PWMMR3
-#define SERVO_REG_2 PWMMR1
-#define SERVO_REG_3 PWMMR6
-#define SERVO_REG_4 PWMMR4
-#define SERVO_REG_5 PWMMR2
-
-#define COMMAND_(i) SERVO_REG_ ## i
-#define ActuatorPwmSet(_i, _v) { COMMAND_(_i) = SERVOS_TICS_OF_USEC(_v); }
-
 
 /*
    wiring on classix PWM connector
@@ -29,35 +17,53 @@ PWM6        PWM2  SSEL0  EINT2   P0.7
 
 */
 
-#define PWM_PINSEL_MASK_VAL 3
+#define SERVOS_TICS_OF_USEC(s) CPU_TICKS_OF_USEC(s)
 
-#define PWM_SERVO_0_PINSEL PINSEL1
-#define PWM_SERVO_0_PINSEL_VAL 1
-#define PWM_SERVO_0_PINSEL_BIT 10
-#define PWM_SERVO_0_PINSEL_MASK ~(PWM_PINSEL_MASK_VAL << PWM_SERVO_0_PINSEL_BIT)
-#define PWM_SERVO_0_ENA PWMPCR_ENA5
-#define PWM_SERVO_0_LATCH PWMLER_LATCH5
+/* default definition of servo (from classix order) */
+#ifndef SERVO_REG_0
+#define SERVO_REG_0 PWMMR5
+#endif
+#ifndef SERVO_REG_1
+#define SERVO_REG_1 PWMMR3
+#endif
+#ifndef SERVO_REG_2
+#define SERVO_REG_2 PWMMR1
+#endif
+#ifndef SERVO_REG_3
+#define SERVO_REG_3 PWMMR6
+#endif
+#ifndef SERVO_REG_4
+#define SERVO_REG_4 PWMMR4
+#endif
+#ifndef SERVO_REG_5
+#define SERVO_REG_5 PWMMR2
+#endif
+
+#define COMMAND_(i) SERVO_REG_ ## i
+#define ActuatorPwmSet(_i, _v) { COMMAND_(_i) = SERVOS_TICS_OF_USEC(_v); }
+
+#define PWM_PINSEL_MASK_VAL 3
 
 #define PWM_SERVO_1_PINSEL PINSEL0
 #define PWM_SERVO_1_PINSEL_VAL 2
-#define PWM_SERVO_1_PINSEL_BIT 2
+#define PWM_SERVO_1_PINSEL_BIT 0
 #define PWM_SERVO_1_PINSEL_MASK ~(PWM_PINSEL_MASK_VAL << PWM_SERVO_1_PINSEL_BIT)
-#define PWM_SERVO_1_ENA PWMPCR_ENA3
-#define PWM_SERVO_1_LATCH PWMLER_LATCH3
+#define PWM_SERVO_1_ENA PWMPCR_ENA1
+#define PWM_SERVO_1_LATCH PWMLER_LATCH1
 
 #define PWM_SERVO_2_PINSEL PINSEL0
 #define PWM_SERVO_2_PINSEL_VAL 2
-#define PWM_SERVO_2_PINSEL_BIT 0
+#define PWM_SERVO_2_PINSEL_BIT 14
 #define PWM_SERVO_2_PINSEL_MASK ~(PWM_PINSEL_MASK_VAL << PWM_SERVO_2_PINSEL_BIT)
-#define PWM_SERVO_2_ENA PWMPCR_ENA1
-#define PWM_SERVO_2_LATCH PWMLER_LATCH1
+#define PWM_SERVO_2_ENA PWMPCR_ENA2
+#define PWM_SERVO_2_LATCH PWMLER_LATCH2
 
 #define PWM_SERVO_3_PINSEL PINSEL0
 #define PWM_SERVO_3_PINSEL_VAL 2
-#define PWM_SERVO_3_PINSEL_BIT 18
+#define PWM_SERVO_3_PINSEL_BIT 2
 #define PWM_SERVO_3_PINSEL_MASK ~(PWM_PINSEL_MASK_VAL << PWM_SERVO_3_PINSEL_BIT)
-#define PWM_SERVO_3_ENA PWMPCR_ENA6
-#define PWM_SERVO_3_LATCH PWMLER_LATCH6
+#define PWM_SERVO_3_ENA PWMPCR_ENA3
+#define PWM_SERVO_3_LATCH PWMLER_LATCH3
 
 #define PWM_SERVO_4_PINSEL PINSEL0
 #define PWM_SERVO_4_PINSEL_VAL 2
@@ -66,12 +72,19 @@ PWM6        PWM2  SSEL0  EINT2   P0.7
 #define PWM_SERVO_4_ENA PWMPCR_ENA4
 #define PWM_SERVO_4_LATCH PWMLER_LATCH4
 
-#define PWM_SERVO_5_PINSEL PINSEL0
-#define PWM_SERVO_5_PINSEL_VAL 2
-#define PWM_SERVO_5_PINSEL_BIT 14
+#define PWM_SERVO_5_PINSEL PINSEL1
+#define PWM_SERVO_5_PINSEL_VAL 1
+#define PWM_SERVO_5_PINSEL_BIT 10
 #define PWM_SERVO_5_PINSEL_MASK ~(PWM_PINSEL_MASK_VAL << PWM_SERVO_5_PINSEL_BIT)
-#define PWM_SERVO_5_ENA PWMPCR_ENA2
-#define PWM_SERVO_5_LATCH PWMLER_LATCH2
+#define PWM_SERVO_5_ENA PWMPCR_ENA5
+#define PWM_SERVO_5_LATCH PWMLER_LATCH5
+
+#define PWM_SERVO_6_PINSEL PINSEL0
+#define PWM_SERVO_6_PINSEL_VAL 2
+#define PWM_SERVO_6_PINSEL_BIT 18
+#define PWM_SERVO_6_PINSEL_MASK ~(PWM_PINSEL_MASK_VAL << PWM_SERVO_6_PINSEL_BIT)
+#define PWM_SERVO_6_ENA PWMPCR_ENA6
+#define PWM_SERVO_6_LATCH PWMLER_LATCH6
 
 extern const uint8_t pwm_latch_value;
 
diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h
index 3516c091d4..5fed1bd9d2 100644
--- a/sw/airborne/boards/booz_1.0.h
+++ b/sw/airborne/boards/booz_1.0.h
@@ -147,16 +147,21 @@
 #define MS2100_DRDY_EINT 3
 #define MS2100_DRDY_VIC_IT VIC_EINT3
 
-/* BOOZ PWM0 = PWM_SERVO_0 (driver) = PWM5 (lpc)
+
+/* Servo definition for actruators_pwm driver */
+
+/* BOOZ PWM0 = PWM_SERVO_5 (driver) = PWM5 (lpc)
  * on CAM connector */
 #if USE_PWM0
-#define PWM_SERVO_0 1
+#define PWM_SERVO_5 1
+#define SERVO_REG_0 PWMMR5
 #endif
 
-/* BOOZ PWM1 = PWM_SERVO_5 (driver) = PWM2 (lpc)
+/* BOOZ PWM1 = PWM_SERVO_2 (driver) = PWM2 (lpc)
  * on SPI connector */
 #if USE_PWM1
-#define PWM_SERVO_5 1
+#define PWM_SERVO_2 1
+#define SERVO_REG_1 PWMMR2
 #endif
 
 
diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h
index f409143362..ea3ff20eee 100644
--- a/sw/airborne/boards/navgo_1.0.h
+++ b/sw/airborne/boards/navgo_1.0.h
@@ -88,14 +88,18 @@
 #define SPI1_DRDY_EINT         0
 #define SPI1_DRDY_VIC_IT       VIC_EINT0
 
-/* NAVGO PWM0 = PWM_SERVO_5 (driver) = PWM2 (lpc) */
+/* Servo definition for actruators_pwm driver */
+
+/* NAVGO PWM0 = PWM_SERVO_2 (driver) = PWM2 (lpc) */
 #if USE_PWM0
-#define PWM_SERVO_5 1
+#define PWM_SERVO_2 1
+#define SERVO_REG_0 PWMMR2
 #endif
 
-/* NAVGO PWM1 = PWM_SERVO_0 (driver) = PWM5 (lpc) */
+/* NAVGO PWM1 = PWM_SERVO_5 (driver) = PWM5 (lpc) */
 #if USE_PWM1
-#define PWM_SERVO_0 1
+#define PWM_SERVO_5 1
+#define SERVO_REG_1 PWMMR5
 #endif
 
 #define BOARD_HAS_BARO 1

From 76f01ceae5ee0263f8d1238a4511a05d58989e14 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 16 Oct 2012 17:54:34 +0200
Subject: [PATCH 07/26] [actuators] converting more actuators drivers

- heli looks obsolete and should be replaced by one of the pwm driver
- pwm_supervion is merged into pwm (direct)
- maybe supervision should have its own subsystem
---
 .../rotorcraft/actuators_heli.makefile        |  3 -
 .../actuators_pwm_supervision.makefile        |  9 ---
 .../shared/actuators_asctec.makefile          |  1 +
 .../actuators_asctec_v2.makefile              | 11 ++--
 .../actuators_mkk.makefile                    | 11 ++--
 .../shared/actuators_pwm_supervision.makefile |  6 ++
 .../shared/actuators_skiron.makefile          |  1 +
 .../subsystems/actuators/actuators_pwm_arch.c |  2 +-
 .../subsystems/actuators/actuators_pwm_arch.h |  5 +-
 .../subsystems/actuators/actuators_pwm_arch.c |  2 +
 .../subsystems/actuators/actuators_pwm_arch.h | 14 +++--
 .../subsystems/actuators/actuators_asctec.c   | 16 ++---
 .../subsystems/actuators/actuators_heli.c     | 49 ---------------
 .../subsystems/actuators/actuators_mkk.c      | 12 ++--
 .../subsystems/actuators/actuators_mkk.h      |  9 ++-
 .../subsystems/actuators/actuators_pwm.c      | 11 +++-
 .../subsystems/actuators/actuators_pwm.h      | 10 ++-
 .../actuators/actuators_pwm_supervision.c     | 63 -------------------
 .../subsystems/actuators/actuators_skiron.h   |  1 -
 19 files changed, 66 insertions(+), 170 deletions(-)
 delete mode 100644 conf/firmwares/subsystems/rotorcraft/actuators_heli.makefile
 delete mode 100644 conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile
 rename conf/firmwares/subsystems/{rotorcraft => shared}/actuators_asctec_v2.makefile (75%)
 rename conf/firmwares/subsystems/{rotorcraft => shared}/actuators_mkk.makefile (84%)
 create mode 100644 conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile
 delete mode 100644 sw/airborne/subsystems/actuators/actuators_heli.c
 delete mode 100644 sw/airborne/subsystems/actuators/actuators_pwm_supervision.c

diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_heli.makefile b/conf/firmwares/subsystems/rotorcraft/actuators_heli.makefile
deleted file mode 100644
index e27dc150c5..0000000000
--- a/conf/firmwares/subsystems/rotorcraft/actuators_heli.makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-
-ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_heli.c
-ap.srcs += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile b/conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile
deleted file mode 100644
index 04cae36bbd..0000000000
--- a/conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile
+++ /dev/null
@@ -1,9 +0,0 @@
-
-ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c
-ap.srcs += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
-
-# Simulator
-nps.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-nps.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c
-nps.srcs += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
diff --git a/conf/firmwares/subsystems/shared/actuators_asctec.makefile b/conf/firmwares/subsystems/shared/actuators_asctec.makefile
index 38032aff04..189d90efb7 100644
--- a/conf/firmwares/subsystems/shared/actuators_asctec.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_asctec.makefile
@@ -1,4 +1,5 @@
 # asctec controllers
+$(TARGET).CFLAGS += -DACTUATORS
 ap.srcs += subsystems/actuators/actuators_asctec.c
 
 ifeq ($(ARCH), lpc21)
diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
similarity index 75%
rename from conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile
rename to conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
index eaa8e486c8..7c33e7794c 100644
--- a/conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
@@ -1,8 +1,6 @@
 # asctec controllers v2
 #
 #  <section name="SUPERVISION" prefix="SUPERVISION_">
-#    <define name="MIN_MOTOR" value="2"/>
-#    <define name="MAX_MOTOR" value="210"/>
 #    <define name="TRIM_A" value="2"/>
 #    <define name="TRIM_E" value="-1"/>
 #    <define name="TRIM_R" value="3"/>
@@ -16,9 +14,10 @@
 #
 #
 
-ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
+$(TARGET).CFLAGS += -DACTUATORS
+ap.srcs += subsystems/actuators/supervision.c
 ap.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL
-ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
+ap.srcs += subsystems/actuators/actuators_asctec.c
 
 ifeq ($(ARCH), lpc21)
 ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0
@@ -31,6 +30,6 @@ ap.CFLAGS += -DUSE_I2C1
 endif
 
 # Simulator
-nps.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-nps.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
+nps.srcs += subsystems/actuators/supervision.c
+nps.srcs += subsystems/actuators/actuators_asctec.c
 nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0
diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile b/conf/firmwares/subsystems/shared/actuators_mkk.makefile
similarity index 84%
rename from conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile
rename to conf/firmwares/subsystems/shared/actuators_mkk.makefile
index 440c947fd8..c62298ca03 100644
--- a/conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_mkk.makefile
@@ -18,8 +18,6 @@
 #  </section>
 #
 #  <section name="SUPERVISION" prefix="SUPERVISION_">
-#    <define name="MIN_MOTOR" value="2"/>
-#    <define name="MAX_MOTOR" value="210"/>
 #    <define name="TRIM_A" value="2"/>
 #    <define name="TRIM_E" value="-1"/>
 #    <define name="TRIM_R" value="3"/>
@@ -33,8 +31,9 @@
 #
 #
 
-ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
+$(TARGET).CFLAGS += -DACTUATORS
+ap.srcs += subsystems/actuators/supervision.c
+ap.srcs += subsystems/actuators/actuators_mkk.c
 
 ifeq ($(ARCH), lpc21)
 
@@ -52,6 +51,6 @@ ap.CFLAGS += -DUSE_I2C1
 endif
 
 # Simulator
-nps.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-nps.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
+nps.srcs += subsystems/actuators/supervision.c
+nps.srcs += subsystems/actuators/actuators_mkk.c
 nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_DEVICE=i2c0
diff --git a/conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile b/conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile
new file mode 100644
index 0000000000..3e5ed13d89
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile
@@ -0,0 +1,6 @@
+
+$(TARGET).CFLAGS += -DACTUATORS -DUSE_SUPERVISION
+$(TARGET).srcs   += subsystems/actuators/supervision.c
+$(TARGET).srcs   += subsystems/actuators/actuators_pwm.c
+$(TARGET).srcs   += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
+
diff --git a/conf/firmwares/subsystems/shared/actuators_skiron.makefile b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
index 254152a481..7361869cfe 100644
--- a/conf/firmwares/subsystems/shared/actuators_skiron.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
@@ -34,6 +34,7 @@ ifeq ($(SKIRON_I2C_SCL_TIME), )
 SKIRON_I2C_SCL_TIME=150
 endif
 
+$(TARGET).CFLAGS += -DACTUATORS
 ap.srcs += subsystems/actuators/supervision.c
 ap.srcs += subsystems/actuators/actuators_skiron.c
 
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
index 61eca3c1e0..27b9c2a361 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
@@ -49,7 +49,7 @@ const uint8_t pwm_latch_value = 0
 #endif
   ;
 
-void actuators_pwm_init ( void ) {
+void actuators_pwm_arch_init ( void ) {
 
   /* configure pins for PWM */
 #if defined PWM_SERVO_0
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
index c8ba69b539..51ef2ac11e 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
@@ -40,6 +40,7 @@ PWM6        PWM2  SSEL0  EINT2   P0.7
 #endif
 
 #define COMMAND_(i) SERVO_REG_ ## i
+/** Actuator set macro */
 #define ActuatorPwmSet(_i, _v) { COMMAND_(_i) = SERVOS_TICS_OF_USEC(_v); }
 
 #define PWM_PINSEL_MASK_VAL 3
@@ -88,11 +89,11 @@ PWM6        PWM2  SSEL0  EINT2   P0.7
 
 extern const uint8_t pwm_latch_value;
 
+/** Actuator commit macro */
 #define ActuatorsPwmCommit() { \
     PWMLER = pwm_latch_value;   \
   }
 
-extern void actuators_pwm_init(void);
-#define ActuatorsPwmInit() actuators_pwm_init()
+extern void actuators_pwm_arch_init(void);
 
 #endif /* ACTUATORS_PWM_ARCH_H */
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
index 0af66ccff6..59a5722288 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -30,6 +30,8 @@
 #include <libopencm3/stm32/f1/rcc.h>
 #include <libopencm3/stm32/timer.h>
 
+int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
+
 #define PCLK 72000000
 #define ONE_MHZ_CLK 1000000
 #ifndef SERVO_HZ
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
index c2f0e4227e..82d3164b4c 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
@@ -38,15 +38,19 @@
 #define ACTUATORS_PWM_NB 6
 #endif
 
-/* Needs to be included after ACTUATORS_PWM_NB is defined!
- * Otherwise ACTUATORS_PWM_NB is not declared in actuators_pwm.h
- */
-#include "subsystems/actuators/actuators_pwm.h"
+///* Needs to be included after ACTUATORS_PWM_NB is defined!
+// * Otherwise ACTUATORS_PWM_NB is not declared in actuators_pwm.h
+// */
+//#include "subsystems/actuators/actuators_pwm.h"
+
+extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
 
 extern void actuators_pwm_arch_init(void);
 extern void actuators_pwm_commit(void);
 
 #define SERVOS_TICS_OF_USEC(_v) (_v)
-#define ActuatorsCommit  actuators_pwm_commit
+
+#define ActuatorPwmSet(_i, _v) { actuators_pwm_values[_i] = _v; }
+#define ActuatorsPwmCommit  actuators_pwm_commit
 
 #endif /* ACTUATORS_PWM_ARCH_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec.c b/sw/airborne/subsystems/actuators/actuators_asctec.c
index 920779e13b..667757296d 100644
--- a/sw/airborne/subsystems/actuators/actuators_asctec.c
+++ b/sw/airborne/subsystems/actuators/actuators_asctec.c
@@ -26,7 +26,6 @@
 #include "subsystems/actuators.h"
 #include "subsystems/actuators/actuators_asctec.h"
 
-#include "firmwares/rotorcraft/commands.h"
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/sys_time.h"
 
@@ -98,10 +97,6 @@ void actuators_asctec_set(bool_t motors_on) {
   actuators_asctec.cmds[YAW]    = 0;
   actuators_asctec.cmds[THRUST] = 0;
 #else /* ! KILL_MOTORS */
-//  actuators_asctec.cmds[PITCH]  = ((commands[COMMAND_PITCH]  + SUPERVISION_TRIM_E) * ASCTEC_MAX_CMD) / MAX_PPRZ;
-//  actuators_asctec.cmds[ROLL]   = ((commands[COMMAND_ROLL]   + SUPERVISION_TRIM_A) * ASCTEC_MAX_CMD) / MAX_PPRZ;
-//  actuators_asctec.cmds[YAW]    = ((commands[COMMAND_YAW]    + SUPERVISION_TRIM_R) * ASCTEC_MAX_CMD) / MAX_PPRZ;
-//  actuators_asctec.cmds[THRUST] = (commands[COMMAND_THRUST] * ASCTEC_MAX_THROTTLE) / MAX_PPRZ;
   Bound(actuators_asctec.cmds[PITCH],ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
   Bound(actuators_asctec.cmds[ROLL], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
   Bound(actuators_asctec.cmds[YAW],  ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
@@ -147,7 +142,7 @@ void actuators_asctec_set(bool_t motors_on) {
 
 }
 #else /* ! ACTUATORS_ASCTEC_V2_PROTOCOL */
-void actuators_set(bool_t motors_on) {
+void actuators_asctec_set(bool_t motors_on) {
 #if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
     if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
@@ -174,7 +169,6 @@ void actuators_set(bool_t motors_on) {
       return;
   }
 
-  supervision_run(motors_on, FALSE, commands);
 #ifdef KILL_MOTORS
   actuators_asctec.i2c_trans.buf[0] = 0;
   actuators_asctec.i2c_trans.buf[1] = 0;
@@ -182,10 +176,10 @@ void actuators_set(bool_t motors_on) {
   actuators_asctec.i2c_trans.buf[3] = 0;
   actuators_asctec.i2c_trans.buf[4] = 0xAA;
 #else
-  actuators_asctec.i2c_trans.buf[0] = supervision.commands[SERVO_FRONT];
-  actuators_asctec.i2c_trans.buf[1] = supervision.commands[SERVO_BACK];
-  actuators_asctec.i2c_trans.buf[2] = supervision.commands[SERVO_LEFT];
-  actuators_asctec.i2c_trans.buf[3] = supervision.commands[SERVO_RIGHT];
+  actuators_asctec.i2c_trans.buf[0] = actuators_asctec.cmds[SERVO_FRONT];
+  actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cmds[SERVO_BACK];
+  actuators_asctec.i2c_trans.buf[2] = actuators_asctec.cmds[SERVO_LEFT];
+  actuators_asctec.i2c_trans.buf[3] = actuators_asctec.cmds[SERVO_RIGHT];
   actuators_asctec.i2c_trans.buf[4] = 0xAA + actuators_asctec.i2c_trans.buf[0] + actuators_asctec.i2c_trans.buf[1] +
                                              actuators_asctec.i2c_trans.buf[2] + actuators_asctec.i2c_trans.buf[3];
 #endif
diff --git a/sw/airborne/subsystems/actuators/actuators_heli.c b/sw/airborne/subsystems/actuators/actuators_heli.c
deleted file mode 100644
index 32a200b0d3..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_heli.c
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * Copyright (C) 2010 The Paparazzi Team
- *
- * 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.
- */
-
-#include "generated/airframe.h"
-
-#include "firmwares/rotorcraft/actuators.h"
-#include "firmwares/rotorcraft/commands.h"
-#include "subsystems/actuators/actuators_pwm.h"
-
-
-#define ESC_STOPPED SERVOS_TICS_OF_USEC(0)
-
-#ifndef KILL_MOTORS
-#define ESC_HOVER   SERVOS_TICS_OF_USEC(5500)
-#else
-#define ESC_HOVER   SERVOS_TICS_OF_USEC(0)
-#endif
-
-int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
-
-void actuators_init(void)
-{
-  actuators_pwm_arch_init();
-}
-
-
-void actuators_set(bool_t motors_on)
-{
-  SetActuatorsFromCommands(commands);
-}
-
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.c b/sw/airborne/subsystems/actuators/actuators_mkk.c
index 3a037eba0d..e968615882 100644
--- a/sw/airborne/subsystems/actuators/actuators_mkk.c
+++ b/sw/airborne/subsystems/actuators/actuators_mkk.c
@@ -23,10 +23,9 @@
  *  Actuators driver for Mikrokopter motor controllers.
  */
 
-#include "firmwares/rotorcraft/actuators.h"
-#include "firmwares/rotorcraft/actuators/actuators_mkk.h"
+#include "subsystems/actuators.h"
+#include "subsystems/actuators/actuators_mkk.h"
 
-#include "firmwares/rotorcraft/commands.h"
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/sys_time.h"
 
@@ -37,7 +36,7 @@ struct ActuatorsMkk actuators_mkk;
 uint32_t actuators_delay_time;
 bool_t   actuators_delay_done;
 
-void actuators_init(void) {
+void actuators_mkk_init(void) {
 
   supervision_init();
   const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
@@ -59,7 +58,7 @@ void actuators_init(void) {
 }
 
 
-void actuators_set(bool_t motors_on) {
+void actuators_mkk_set(void) {
 #if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
     if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
@@ -67,12 +66,9 @@ void actuators_set(bool_t motors_on) {
   }
 #endif
 
-  supervision_run(motors_on, FALSE, commands);
   for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
 #ifdef KILL_MOTORS
     actuators_mkk.trans[i].buf[0] = 0;
-#else
-    actuators_mkk.trans[i].buf[0] = supervision.commands[i];
 #endif
     i2c_submit(&ACTUATORS_MKK_DEVICE, &actuators_mkk.trans[i]);
   }
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.h b/sw/airborne/subsystems/actuators/actuators_mkk.h
index a4e5e41f76..0054ae34c2 100644
--- a/sw/airborne/subsystems/actuators/actuators_mkk.h
+++ b/sw/airborne/subsystems/actuators/actuators_mkk.h
@@ -30,6 +30,7 @@
 #include "mcu_periph/i2c.h"
 
 #include "generated/airframe.h"
+#include "subsystems/actuators/supervision.h"
 
 
 struct ActuatorsMkk {
@@ -38,9 +39,11 @@ struct ActuatorsMkk {
 
 extern struct ActuatorsMkk actuators_mkk;
 
+extern void actuators_mkk_init(void);
+extern void actuators_mkk_set(void);
 
-
-#include "firmwares/rotorcraft/actuators/supervision.h"
-
+#define ActuatorMkkSet(_i, _v) { actuators_mkk.trans[i].buf[0] = _v; }
+#define ActuatorsMkkInit() actuators_mkk_init()
+#define ActuatorsMkkCommit() actuators_mkk_set()
 
 #endif /* ACTUATORS_MKK_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.c b/sw/airborne/subsystems/actuators/actuators_pwm.c
index a5c432d3fe..0f3bfe636b 100644
--- a/sw/airborne/subsystems/actuators/actuators_pwm.c
+++ b/sw/airborne/subsystems/actuators/actuators_pwm.c
@@ -19,7 +19,14 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#include "actuators_pwm.h"
+#include "subsystems/actuators/actuators_pwm.h"
 
-int32_t actuators_pwm_values[SERVOS_PWM_NB];
+void actuators_pwm_init(void)
+{
+#if USE_SUPERVISION
+  supervision_init();
+#endif
+  // implemented in arch files
+  actuators_pwm_arch_init();
+}
 
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.h b/sw/airborne/subsystems/actuators/actuators_pwm.h
index cae834ffea..fbbd28570a 100644
--- a/sw/airborne/subsystems/actuators/actuators_pwm.h
+++ b/sw/airborne/subsystems/actuators/actuators_pwm.h
@@ -28,6 +28,14 @@
 #include BOARD_CONFIG
 #include "subsystems/actuators/actuators_pwm_arch.h"
 
-extern int32_t actuators_pwm_values[SERVOS_PWM_NB];
+#if USE_SUPERVISION
+#include "subsystems/actuators/supervision.h"
+#endif
+
+extern void actuators_pwm_init(void);
+
+#define ActuatorsPwmInit() actuators_pwm_init()
+/* ActuatorSet and ActuatorsPwmCommit are implemented in arch files
+ */
 
 #endif /* ACTUATORS_PWM_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm_supervision.c b/sw/airborne/subsystems/actuators/actuators_pwm_supervision.c
deleted file mode 100644
index 8350a36de3..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_pwm_supervision.c
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * Copyright (C) 2010-2012 The Paparazzi Team
- *
- * 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.
- */
-
-/** @file actuators_pwm_supervision.h
- *  PWM actuators with supervision.
- */
-
-#include "generated/airframe.h"
-
-/* warn if SUPERVISION_STOP_MOTOR is not defined in the airframe file */
-#ifndef SUPERVISION_STOP_MOTOR
-#warning "STOP_MOTOR is not defined in the SUPERVISION section, are you sure you want to use the default of 0?"
-#endif
-
-#include "firmwares/rotorcraft/actuators/supervision.h"
-#include "firmwares/rotorcraft/commands.h"
-#include "subsystems/radio_control.h"
-
-#include "firmwares/rotorcraft/actuators.h"
-#include "subsystems/actuators/actuators_pwm.h"
-
-
-/** actuator PWM values in usec. */
-int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
-
-void actuators_init(void)
-{
-  supervision_init();
-  actuators_pwm_arch_init();
-}
-
-void actuators_set(bool_t motors_on)
-{
-  /* set normal control surface actuators, i.e. servos */
-  SetActuatorsFromCommands(commands);
-
-  /* run supervision for actuators (motor controllers) that need mixing */
-  supervision_run(motors_on, FALSE, commands);
-
-  for (int i = 0; i < SUPERVISION_NB_MOTOR; i++) {
-    actuators_pwm_values[i] = supervision.commands[i];
-  }
-  ActuatorsCommit();
-}
-
diff --git a/sw/airborne/subsystems/actuators/actuators_skiron.h b/sw/airborne/subsystems/actuators/actuators_skiron.h
index b1a6b2906f..09ceb16fd7 100644
--- a/sw/airborne/subsystems/actuators/actuators_skiron.h
+++ b/sw/airborne/subsystems/actuators/actuators_skiron.h
@@ -38,7 +38,6 @@
 
 struct ActuatorsSkiron {
   struct i2c_transaction trans;
-  //uint8_t actuators_idx[ACTUATORS_SKIRON_NB];
 };
 
 extern struct ActuatorsSkiron actuators_skiron;

From 694ea6416ee37a4cda58c0374ca5d79bacbc3717 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 16 Oct 2012 18:12:00 +0200
Subject: [PATCH 08/26] [actuators] fix some Set macros

---
 .../arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h        | 2 +-
 sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h    | 2 +-
 sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h    | 2 +-
 sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h     | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
index 2e03368e5f..7d52cc22bb 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
@@ -42,7 +42,7 @@
 
 #define _4015_NB_CHANNELS 8
 extern uint16_t servos_values[_4015_NB_CHANNELS];
-#define Actuator4015(i) servos_values[i]
+#define Actuator4015Set(_i, _v) { servos_values[_i] = SERVOS_TICS_OF_USEC(_v); }
 
 extern void actuators_4015_init(void);
 #define Actuators4015Commit() {}
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h
index eab191954b..cf5574ae39 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_hw.h
@@ -35,7 +35,7 @@
 
 #define _4015_NB_CHANNELS 8
 extern uint16_t servos_values[_4015_NB_CHANNELS];
-#define Actuator4015(i) servos_values[i]
+#define Actuator4015Set(_i, _v) { servos_values[_i] = SERVOS_TICS_OF_USEC(_v); }
 
 extern void actuators_4015_init(void);
 #define Actuators4015Commit() {}
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
index 9a1c83e61b..8d9e2e68f3 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
@@ -19,7 +19,7 @@
 #endif
 
 extern uint16_t servos_values[_4017_NB_CHANNELS];
-#define Actuator4017Set(_i, _v) { servos_values[i] = SERVOS_TICS_OF_USEC(_v); }
+#define Actuator4017Set(_i, _v) { servos_values[_i] = SERVOS_TICS_OF_USEC(_v); }
 
 extern void actuators_4017_init(void);
 #define Actuators4017Commit() {}
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
index 13e8b5e382..2396622395 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
@@ -40,7 +40,7 @@
 
 #define _PPM_NB_CHANNELS 8
 extern uint16_t servos_values[_PPM_NB_CHANNELS];
-#define ActuatorPpm(i) servos_values[i]
+#define ActuatorPpmSet(_i, _v) { servos_values[_i] = SERVOS_TICS_OF_USEC(_v); }
 
 extern void actuators_ppm_init(void);
 #define ActuatorsPpmCommit() {}

From 28543d4e30c8bea1e10a445ec2b7b01821a9703e Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 18 Oct 2012 17:15:06 +0200
Subject: [PATCH 09/26] [actuators] update, rename and clean rotorcraft cam
 control

---
 conf/airframes/ENAC/quadrotor/blender.xml     |  53 +++----
 conf/airframes/ENAC/quadrotor/booz2_g1.xml    |  37 ++---
 ...x_booz_cam.xml => xbox_rotorcraft_cam.xml} |   2 +-
 conf/messages.xml                             |   6 +-
 conf/modules/booz_cam.xml                     |  19 ---
 conf/modules/rotorcraft_cam.xml               |  33 ++++
 .../{booz_cam.xml => rotorcraft_cam.xml}      |   8 +-
 conf/telemetry/default_rotorcraft.xml         |   2 +-
 sw/airborne/firmwares/rotorcraft/telemetry.h  |   4 +-
 sw/airborne/modules/cam_control/booz_cam.c    | 149 ------------------
 sw/airborne/modules/cam_control/booz_cam.h    |  73 ---------
 .../modules/cam_control/rotorcraft_cam.c      | 112 +++++++++++++
 .../modules/cam_control/rotorcraft_cam.h      | 139 ++++++++++++++++
 sw/airborne/subsystems/actuators.h            |   3 +-
 14 files changed, 335 insertions(+), 305 deletions(-)
 rename conf/joystick/{xbox_booz_cam.xml => xbox_rotorcraft_cam.xml} (80%)
 delete mode 100644 conf/modules/booz_cam.xml
 create mode 100644 conf/modules/rotorcraft_cam.xml
 rename conf/settings/modules/{booz_cam.xml => rotorcraft_cam.xml} (54%)
 delete mode 100644 sw/airborne/modules/cam_control/booz_cam.c
 delete mode 100644 sw/airborne/modules/cam_control/booz_cam.h
 create mode 100644 sw/airborne/modules/cam_control/rotorcraft_cam.c
 create mode 100644 sw/airborne/modules/cam_control/rotorcraft_cam.h

diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index ed812e9dac..11ccf464c0 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -3,12 +3,9 @@
 <airframe name="Blender">
 
   <modules main_freq="512">
-    <!--load name="booz_pwm.xml">
-      <define name="USE_PWM0"/>
-    </load>
-    <load name="booz_drop.xml"/>
-    <load name="booz_cam.xml"/-->
-    <load name="sonar_maxbotix_booz.xml"/>
+    <load name="servo_switch.xml"/>
+    <load name="rotorcraft_cam.xml"/>
+    <!--load name="sonar_maxbotix_booz.xml"/-->
     <!--load name="adc_generic.xml">
       <configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
     </load-->
@@ -50,21 +47,21 @@
     <define name="TRIM_R" value="-100"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
-    <define name="ROLL_COEF"  value="{    0,    0, -256,  256}"/>
-    <define name="PITCH_COEF" value="{  256, -256,    0,    0}"/>
-    <define name="YAW_COEF"   value="{ -256, -256,  256,  256}"/>
+    <define name="ROLL_COEF"  value="{    0, -256,    0,  256}"/>
+    <define name="PITCH_COEF" value="{  256,    0, -256,    0}"/>
+    <define name="YAW_COEF"   value="{ -256,  256, -256,  256}"/>
     <define name="THRUST_COEF" value="{ 256,  256,  256,  256}"/>
   </section>
 
   <servos driver="Skiron">
-    <servo name="FRONT"  no="0"    min="20" neutral="20" max="255"/>
-    <servo name="BACK"   no="2"    min="20" neutral="20" max="255"/>
-    <servo name="RIGHT"  no="1"    min="20" neutral="20" max="255"/>
-    <servo name="LEFT"   no="3"    min="20" neutral="20" max="255"/>
+    <servo name="FRONT"  no="0"    min="0" neutral="20" max="255"/>
+    <servo name="RIGHT"  no="1"    min="0" neutral="20" max="255"/>
+    <servo name="BACK"   no="2"    min="0" neutral="20" max="255"/>
+    <servo name="LEFT"   no="3"    min="0" neutral="20" max="255"/>
   </servos>
 
   <servos driver="Pwm">
-    <servo name="DROP" no="5" min="1000" neutral="1500" max="2000"/>
+    <servo name="DROP" no="0" min="1000" neutral="1500" max="2000"/>
   </servos>
 
   <commands>
@@ -74,11 +71,6 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
-  <!--section name="ACTUATORS_SKIRON" prefix="ACTUATORS_SKIRON_">
-    <define name="NB" value="4"/>
-    <define name="IDX" value="{ SERVO_FRONT, SERVO_BACK, SERVO_RIGHT, SERVO_LEFT }"/>
-  </section-->
-
   <command_laws>
     <call fun="supervision_run(autopilot_motors_on,FALSE,values)"/>
     <set servo="FRONT" value="supervision.commands[SERVO_FRONT]"/>
@@ -264,25 +256,18 @@
    <define name="BOOZ_FMS_TIMEOUT" value="0"/>
  </section>
 
- <section name="CAM" prefix="BOOZ_CAM_">
-    <!--define name="TILT_NEUTRAL" value="1500"/>
-    <define name="TILT_MIN" value="1060"/>
-    <define name="TILT_MAX" value="2250"/>
-    <define name="TILT_ANGLE_MIN" value="-90." unit="deg"/>
-    <define name="TILT_ANGLE_MAX" value="  0." unit="deg"/-->
-    <define name="PAN_NEUTRAL" value="0"/>
-    <define name="PAN_MIN" value="0"/>
-    <define name="PAN_MAX" value="25736"/> <!-- 360 deg (2^12) -->
-    <define name="DEFAULT_MODE" value="BOOZ_CAM_MODE_MANUAL"/>
+ <section name="CAM" prefix="ROTORCRAFT_CAM_">
+    <define name="DEFAULT_MODE" value="ROTORCRAFT_CAM_MODE_MANUAL"/>
     <define name="ON" value="{}"/>
     <define name="OFF" value="{}"/>
-    <!--define name="SetPwm(_v)" value="Booz2SetPwm1Value(_v)"/-->
  </section>
 
-  <section name="DROP">
-    <define name="DROP_SERVO_CLOSED" value="2000"/>
-    <define name="DROP_SERVO_OPEN" value="1000"/>
-    <define name="BoozDropPwm(_v)" value="BoozSetPwm0Value(_v)"/>
+  <section name="SERVO_SWITCH">
+    <define name="SERVO_SWITCH_SERVO" value="DROP"/>
+    <define name="SERVO_SWITCH_ON_VALUE" value="SERVO_DROP_MIN"/>
+    <define name="SERVO_SWITCH_OFF_VALUE" value="SERVO_DROP_MAX"/>
+    <define name="DropOpen()" value="ServoSwitchOn()"/>
+    <define name="DropClose()" value="ServoSwitchOff()"/>
   </section>
 
  <section name="MISC">
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 0298610a0a..03413695ea 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -2,7 +2,7 @@
 
   <modules main_freq="512">
     <load name="servo_switch.xml"/>
-    <!--load name="booz_cam.xml"/-->
+    <load name="rotorcraft_cam.xml"/>
     <!--load name="sonar_maxbotix_booz.xml"/-->
     <!--load name="adc_generic_booz.xml"/-->
     <!--load name="sys_mon.xml"/-->
@@ -37,12 +37,6 @@
     <subsystem name="ins"           type="hff"/>
   </firmware>
 
-  <section name="TRIM">
-    <define name="TRIM_ROLL" value="0"/>
-    <define name="TRIM_PITCH" value="6"/>
-    <define name="TRIM_YAW" value="0"/>
-  </section>
-
   <servos driver="Asctec">
     <servo name="PITCH"  no="0" min="-100" neutral="0" max="100"/>
     <servo name="ROLL"   no="1" min="-100" neutral="0" max="100"/>
@@ -51,8 +45,8 @@
   </servos>
 
   <servos driver="Pwm">
-    <servo name="CAM" no="5" min="1000" neutral="1500" max="2000"/>
     <servo name="SWITCH" no="0" min="1060" neutral="1500" max="2120"/>
+    <servo name="CAM" no="1" min="1000" neutral="1500" max="2300"/>
   </servos>
 
   <commands>
@@ -62,6 +56,12 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
+  <section name="TRIM">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="6"/>
+    <define name="TRIM_YAW" value="0"/>
+  </section>
+
   <command_laws>
     <set servo="PITCH" value="@PITCH - TRIM_PITCH"/>
     <set servo="ROLL" value="@ROLL - TRIM_ROLL"/>
@@ -213,18 +213,19 @@
     <define name="BOOZ_FMS_TIMEOUT" value="0"/>
   </section>
 
-  <section name="CAM" prefix="BOOZ_CAM_">
+  <section name="CAM" prefix="ROTORCRAFT_CAM_">
     <define name="ON" value="LED_ON(CAM_SWITCH_LED)"/>
     <define name="OFF" value="LED_OFF(CAM_SWITCH_LED)"/>
-    <define name="TILT_NEUTRAL" value="1500"/>
-    <define name="TILT_MAX" value="1000"/>
-    <define name="TILT_MIN" value="2300"/>
-    <define name="TILT_ANGLE_MIN" value="-90." unit="deg"/>
-    <define name="TILT_ANGLE_MAX" value=" 10." unit="deg"/>
-    <define name="PAN_NEUTRAL" value="0"/>
-    <define name="PAN_MIN" value="0"/>
-    <define name="PAN_MAX" value="25736"/> <!-- 360 deg (2^12) -->
-    <define name="SetPwm(_v)" value="BoozSetPwm1Value(_v)"/>
+    <define name="TILT_SERVO" value="CAM"/>
+    <define name="TILT_ANGLE_MAX" value="-90." unit="deg"/>
+    <define name="TILT_ANGLE_MIN" value=" 10." unit="deg"/>
+  </section>
+
+  <section name="SERVO_SWITCH">
+    <define name="SERVO_SWITCH_ON_VALUE" value="SERVO_SWITCH_MIN"/>
+    <define name="SERVO_SWITCH_OFF_VALUE" value="SERVO_SWITCH_MAX"/>
+    <define name="DropOpen()" value="ServoSwitchOn()"/>
+    <define name="DropClose()" value="ServoSwitchOff()"/>
   </section>
 
   <section name="MISC">
diff --git a/conf/joystick/xbox_booz_cam.xml b/conf/joystick/xbox_rotorcraft_cam.xml
similarity index 80%
rename from conf/joystick/xbox_booz_cam.xml
rename to conf/joystick/xbox_rotorcraft_cam.xml
index 5eecdc472e..5e14acba4d 100644
--- a/conf/joystick/xbox_booz_cam.xml
+++ b/conf/joystick/xbox_rotorcraft_cam.xml
@@ -4,7 +4,7 @@
     <axis index="7" name="tilt"/>
   </input>
   <messages period="0.1">
-    <message class="datalink" name="BOOZ_CAM_STICK">
+    <message class="datalink" name="ROTORCRAFT_CAM_STICK">
       <field name="tilt" value="-tilt"/>
       <field name="pan" value="pan"/>
     </message>
diff --git a/conf/messages.xml b/conf/messages.xml
index 51db383031..b2752570b5 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1394,8 +1394,8 @@
     <field name="var" type="float"/>
   </message>
 
-  <message name="BOOZ2_CAM" id="168">
-    <field name="tilt" type="int16" unit="us"/>
+  <message name="ROTORCRAFT_CAM" id="168">
+    <field name="tilt" type="int16" unit="2^12rad" alt_unit="deg" alt_unit_coef="0.0139882"/>
     <field name="pan" type="int16" unit="2^12rad" alt_unit="deg" alt_unit_coef="0.0139882"/>
   </message>
 
@@ -2177,7 +2177,7 @@
    <field name="yd"     type="float" unit="m"/>
  </message>
 
- <message name="BOOZ_CAM_STICK" id="152" link="forwarded">
+ <message name="ROTORCRAFT_CAM_STICK" id="152" link="forwarded">
    <field name="ac_id" type="uint8"/>
    <field name="tilt" type="int8"/>
    <field name="pan" type="int8"/>
diff --git a/conf/modules/booz_cam.xml b/conf/modules/booz_cam.xml
deleted file mode 100644
index 48d9baaa98..0000000000
--- a/conf/modules/booz_cam.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-
-<module name="booz_cam" dir="cam_control">
-  <doc>
-    <description>Camera control for BOOZ board</description>
-  </doc>
-  <!-- depend require="booz_pwm|led" -->
-  <header>
-    <file name="booz_cam.h"/>
-  </header>
-  <init fun="booz_cam_init()"/>
-  <periodic fun="booz_cam_periodic()" freq="10."/>
-  <datalink message="BOOZ_CAM_STICK" fun="BOOZ_CAM_STICK_PARSE(dl_buffer)"/>
-  <makefile>
-    <define name="USE_CAM"/>
-    <file name="booz_cam.c"/>
-  </makefile>
-</module>
-
diff --git a/conf/modules/rotorcraft_cam.xml b/conf/modules/rotorcraft_cam.xml
new file mode 100644
index 0000000000..05142e146d
--- /dev/null
+++ b/conf/modules/rotorcraft_cam.xml
@@ -0,0 +1,33 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="rotorcraft_cam" dir="cam_control">
+  <doc>
+    <description>
+      Camera control for rotorcraft.
+      The camera is controled by the heading of the vehicle for pan and can be controlled by a servo for tilt if defined.
+    </description>
+    <section name="CAM" prefix="ROTORCRAFT_CAM_">
+      <define name="DEFAULT_MODE" value="MODE_NONE|MODE_MANUAL|MODE_HEADING|MODE_WP" description="Select default mode (default: MODE_NONE)"/>
+      <define name="CAM_ON" value="handler" description="Handler to power on the camera ({} to disable)"/>
+      <define name="CAM_OFF" value="handler" description="Handler to power off the camera ({} to disable)"/>
+      <define name="TILT_SERVO" value="name" description="Servo name for tilt control (no tilt control if not defined)"/>
+      <define name="TILT_ANGLE_MIN" value="angle" description="Tilt angle corresponding to the servo min position"/>
+      <define name="TILT_ANGLE_MAX" value="angle" description="Tilt angle corresponding to the servo max position"/>
+      <define name="USE_PAN" value="TRUE|FALSE" description="Enable pan control with heading (default: TRUE)"/>
+      <define name="TRACK_WP" value="WP_XXX" description="Enable waypoint tracking mode on the selected WP (default: enable if waypoint CAM is defined)"/>
+      <define name="STICK_TILT_INC" value="angle/stick_input" description="Sensitivity of the datalink stick tilt control (default: 10 deg)"/>
+      <define name="STICK_PAN_INC" value="angle/stick_input" description="Sensitivity of the datalink stick pan control (default: 20 deg)"/>
+    </section>
+  </doc>
+  <header>
+    <file name="rotorcraft_cam.h"/>
+  </header>
+  <init fun="rotorcraft_cam_init()"/>
+  <periodic fun="rotorcraft_cam_periodic()" freq="10."/>
+  <datalink message="ROTORCRAFT_CAM_STICK" fun="ROTORCRAFT_CAM_STICK_PARSE(dl_buffer)"/>
+  <makefile>
+    <define name="USE_CAM"/>
+    <file name="rotorcraft_cam.c"/>
+  </makefile>
+</module>
+
diff --git a/conf/settings/modules/booz_cam.xml b/conf/settings/modules/rotorcraft_cam.xml
similarity index 54%
rename from conf/settings/modules/booz_cam.xml
rename to conf/settings/modules/rotorcraft_cam.xml
index 4b7c08e05d..17ae9b50b4 100644
--- a/conf/settings/modules/booz_cam.xml
+++ b/conf/settings/modules/rotorcraft_cam.xml
@@ -2,7 +2,7 @@
   <dl_settings>
 
     <dl_settings NAME="CAM">
-      <dl_setting var="booz_cam_mode" MIN="0" STEP="1" MAX="3" module="cam_control/booz_cam" shortname="mode" values="NONE|MAN|HEADING|WP" handler="SetCamMode">
+      <dl_setting var="rotorcraft_cam_mode" MIN="0" STEP="1" MAX="3" module="cam_control/rotorcraft_cam" shortname="mode" values="NONE|MAN|HEADING|WP" handler="SetCamMode">
         <strip_button name="CN" value="0" group="cam_mode1"/>
         <strip_button name="CM" value="1" group="cam_mode1"/>
         <strip_button name="CH" value="2" group="cam_mode2"/>
@@ -10,12 +10,12 @@
         <key_press key="F1" value="0"/>
         <key_press key="F2" value="1"/>
       </dl_setting>
-      <dl_setting var="booz_cam_tilt_pwm" min="1000" step="1" max="2500" shortname="tilt_pwm"/>
-      <dl_setting var="booz_cam_tilt" min="-90" step="1" max="0" shortname="tilt" alt_unit="deg" alt_unit_coef="0.0139882">
+      <dl_setting var="rotorcraft_cam_tilt_pwm" min="1000" step="1" max="2500" shortname="tilt_pwm"/>
+      <dl_setting var="rotorcraft_cam_tilt" min="-90" step="1" max="0" shortname="tilt" alt_unit="deg" alt_unit_coef="0.0139882">
         <strip_button name="Look Foreward" icon="lookfore.png" value="0" group="cam_look"/>
         <strip_button name="Look Down" icon="lookdown.png" value="-6434" group="cam_look"/>
       </dl_setting>
-      <dl_setting var="booz_cam_pan" MIN="0" STEP="1" MAX="360" shortname="pan" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
+      <dl_setting var="rotorcraft_cam_pan" MIN="0" STEP="1" MAX="360" shortname="pan" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
     </dl_settings>
 
   </dl_settings>
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index c55ee17611..2376ac8749 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -13,7 +13,7 @@
       <message name="INS_REF"                period="5.1"/>
       <message name="ROTORCRAFT_NAV_STATUS"  period="1.6"/>
       <message name="WP_MOVED"               period="1.3"/>
-      <message name="BOOZ2_CAM"              period="1."/>
+      <message name="ROTORCRAFT_CAM"         period="1."/>
       <message name="GPS_INT"                period=".25"/>
       <message name="INS"                    period=".25"/>
       <message name="I2C_ERRORS"             period="6."/>
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index ba7d1f08a1..d54fcea817 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -737,9 +737,9 @@
   }
 
 #if USE_CAM
-#define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) DOWNLINK_SEND_BOOZ2_CAM(_trans, _dev,&booz_cam_tilt,&booz_cam_pan);
+#define PERIODIC_SEND_ROTORCRAFT_CAM(_trans, _dev) DOWNLINK_SEND_ROTORCRAFT_CAM(_trans, _dev,&rotorcraft_cam_tilt,&rotorcraft_cam_pan);
 #else
-#define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) {}
+#define PERIODIC_SEND_ROTORCRAFT_CAM(_trans, _dev) {}
 #endif
 
 #define PERIODIC_SEND_ROTORCRAFT_TUNE_HOVER(_trans, _dev) {             \
diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c
deleted file mode 100644
index ddd5dd3451..0000000000
--- a/sw/airborne/modules/cam_control/booz_cam.c
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2009 Gautier Hattenberger <gautier.hattenberger@laas.fr>,
- *                    Antoine Drouin <poinix@gmail.com>
- *
- * 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.
- */
-
-#include "cam_control/booz_cam.h"
-#include "modules/core/booz_pwm_arch.h"
-#include "state.h"
-#include "firmwares/rotorcraft/navigation.h"
-#include "generated/flight_plan.h"
-#include "std.h"
-
-uint8_t booz_cam_mode;
-
-// Tilt definition
-int16_t booz_cam_tilt;
-int16_t booz_cam_tilt_pwm;
-#ifdef BOOZ_CAM_TILT_NEUTRAL
-#ifndef BOOZ_CAM_TILT_MIN
-#define BOOZ_CAM_TILT_MIN BOOZ_CAM_TILT_NEUTRAL
-#endif
-#ifndef BOOZ_CAM_TILT_MAX
-#define BOOZ_CAM_TILT_MAX BOOZ_CAM_TILT_NEUTRAL
-#endif
-#define BOOZ_CAM_USE_TILT 1
-#endif
-
-// Pan definition
-int16_t booz_cam_pan;
-#ifdef BOOZ_CAM_PAN_NEUTRAL
-#ifndef BOOZ_CAM_PAN_MIN
-#define BOOZ_CAM_PAN_MIN BOOZ_CAM_PAN_NEUTRAL
-#endif
-#ifndef BOOZ_CAM_PAN_MAX
-#define BOOZ_CAM_PAN_MAX BOOZ_CAM_PAN_NEUTRAL
-#endif
-#define BOOZ_CAM_USE_PAN 1
-#endif
-
-#if defined BOOZ_CAM_TILT_ANGLE_MIN && defined BOOZ_CAM_TILT_ANGLE_MAX && defined BOOZ_CAM_USE_TILT
-#define CAM_TA_MIN ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MIN)
-#define CAM_TA_MAX ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MAX)
-#define BOOZ_CAM_USE_TILT_ANGLES 1
-#endif
-
-// PWM definition
-#ifndef BOOZ_CAM_SetPwm
-#define BOOZ_CAM_SetPwm(_v) BoozSetPwmValue(_v)
-#endif
-
-#ifndef BOOZ_CAM_DEFAULT_MODE
-#define BOOZ_CAM_DEFAULT_MODE BOOZ_CAM_MODE_NONE
-#endif
-
-void booz_cam_init(void) {
-  booz_cam_SetCamMode(BOOZ_CAM_DEFAULT_MODE);
-#ifdef BOOZ_CAM_USE_TILT
-  booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
-  BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
-  booz_cam_tilt = 0;
-#else
-  booz_cam_tilt_pwm = 1500;
-  booz_cam_tilt = 0;
-#endif
-#ifdef BOOZ_CAM_USE_PAN
-  booz_cam_pan = BOOZ_CAM_PAN_NEUTRAL;
-#else
-  booz_cam_pan = 0;
-#endif
-}
-
-#define D_TILT (BOOZ_CAM_TILT_MAX - BOOZ_CAM_TILT_MIN)
-#define CT_MIN Min(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX)
-#define CT_MAX Max(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX)
-#define CP_MIN Min(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX)
-#define CP_MAX Max(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX)
-
-void booz_cam_periodic(void) {
-
-  switch (booz_cam_mode) {
-    case BOOZ_CAM_MODE_NONE:
-#ifdef BOOZ_CAM_USE_TILT
-      booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
-#endif
-#ifdef BOOZ_CAM_USE_PAN
-      booz_cam_pan = stateGetNedToBodyEulers_i()->psi;
-#endif
-      break;
-    case BOOZ_CAM_MODE_MANUAL:
-#ifdef BOOZ_CAM_USE_TILT
-      Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
-#endif
-      break;
-    case BOOZ_CAM_MODE_HEADING:
-#ifdef BOOZ_CAM_USE_TILT_ANGLES
-      Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX);
-      booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
-      Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
-#endif
-#ifdef BOOZ_CAM_USE_PAN
-      Bound(booz_cam_pan, CP_MIN, CP_MAX);
-      nav_heading = booz_cam_pan;
-#endif
-      break;
-    case BOOZ_CAM_MODE_WP:
-#ifdef WP_CAM
-      {
-        struct Int32Vect2 diff;
-        VECT2_DIFF(diff, waypoints[WP_CAM], *stateGetPositionEnu_i());
-        INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
-        INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
-        nav_heading = booz_cam_pan;
-#ifdef BOOZ_CAM_USE_TILT_ANGLES
-        int32_t dist, height;
-        INT32_VECT2_NORM(dist, diff);
-        height = (waypoints[WP_CAM].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
-        INT32_ATAN2(booz_cam_tilt, height, dist);
-        Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
-        booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
-        Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
-#endif
-      }
-#endif
-      break;
-  }
-#ifdef BOOZ_CAM_USE_TILT
-  BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
-#endif
-}
-
diff --git a/sw/airborne/modules/cam_control/booz_cam.h b/sw/airborne/modules/cam_control/booz_cam.h
deleted file mode 100644
index 2a1fbfb0d8..0000000000
--- a/sw/airborne/modules/cam_control/booz_cam.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2009 Gautier Hattenberger <gautier.hattenberger@laas.fr>,
- *                    Antoine Drouin <poinix@gmail.com>
- *
- * 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.
- */
-
-#ifndef BOOZ_CAM_H
-#define BOOZ_CAM_H
-
-#include "generated/airframe.h"
-#include "math/pprz_algebra_int.h"
-#include "std.h"
-#include "led.h"
-
-#define BOOZ_CAM_MODE_NONE     0
-#define BOOZ_CAM_MODE_MANUAL   1
-#define BOOZ_CAM_MODE_HEADING  2
-#define BOOZ_CAM_MODE_WP       3
-
-// Warning:
-// LED_ON set GPIO low
-// LED_OFF set GPIO high
-#ifndef BOOZ_CAM_ON
-#define BOOZ_CAM_ON LED_OFF(CAM_SWITCH_LED)
-#endif
-#ifndef BOOZ_CAM_OFF
-#define BOOZ_CAM_OFF LED_ON(CAM_SWITCH_LED)
-#endif
-
-extern uint8_t booz_cam_mode;
-
-extern int16_t booz_cam_tilt;
-extern int16_t booz_cam_pan;
-extern int16_t booz_cam_tilt_pwm;
-
-extern void booz_cam_init(void);
-extern void booz_cam_periodic(void);
-
-#define booz_cam_SetCamMode(_v) { \
-  booz_cam_mode = _v; \
-  if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { BOOZ_CAM_OFF; } \
-  else { BOOZ_CAM_ON; } \
-}
-
-#define BOOZ_CAM_STICK_TILT_INC (ANGLE_BFP_OF_REAL(RadOfDeg(10.))/127.)
-#define BOOZ_CAM_STICK_PAN_INC (ANGLE_BFP_OF_REAL(RadOfDeg(20.))/127.)
-
-#define BOOZ_CAM_STICK_PARSE(_dl_buffer) { \
-  booz_cam_tilt += (int16_t)(BOOZ_CAM_STICK_TILT_INC*(float)DL_BOOZ_CAM_STICK_tilt(_dl_buffer)); \
-  booz_cam_pan += (int16_t)(BOOZ_CAM_STICK_PAN_INC*(float)DL_BOOZ_CAM_STICK_pan(dl_buffer)); \
-  INT32_COURSE_NORMALIZE(booz_cam_pan); \
-}
-
-#endif /* BOOZ2_CAM_H */
-
diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c
new file mode 100644
index 0000000000..0df7e0ba3f
--- /dev/null
+++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (C) 2009-2012 Gautier Hattenberger <gautier.hattenberger@enac.fr>,
+ *                    Antoine Drouin <poinix@gmail.com>
+ *
+ * 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.
+ */
+
+#include "modules/cam_control/rotorcraft_cam.h"
+
+#include "subsystems/actuators.h"
+#include "state.h"
+#include "firmwares/rotorcraft/navigation.h"
+#include "std.h"
+
+uint8_t rotorcraft_cam_mode;
+
+#define _SERVO_PARAM(_s,_p) SERVO_ ## _s ## _ ## _p
+#define SERVO_PARAM(_s,_p) _SERVO_PARAM(_s,_p)
+
+// Tilt definition
+int16_t rotorcraft_cam_tilt;
+int16_t rotorcraft_cam_tilt_pwm;
+#if ROTORCRAFT_CAM_USE_TILT
+#define ROTORCRAFT_CAM_TILT_NEUTRAL SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, NEUTRAL)
+#define ROTORCRAFT_CAM_TILT_MIN SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MIN)
+#define ROTORCRAFT_CAM_TILT_MAX SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MAX)
+#define D_TILT (ROTORCRAFT_CAM_TILT_MAX - ROTORCRAFT_CAM_TILT_MIN)
+#define CT_MIN Min(CAM_TA_MIN, CAM_TA_MAX)
+#define CT_MAX Max(CAM_TA_MIN, CAM_TA_MAX)
+#endif
+
+// Pan definition
+int16_t rotorcraft_cam_pan;
+#define ROTORCRAFT_CAM_PAN_MIN 0
+#define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI
+
+void rotorcraft_cam_init(void) {
+  rotorcraft_cam_SetCamMode(ROTORCRAFT_CAM_DEFAULT_MODE);
+#if ROTORCRAFT_CAM_USE_TILT
+  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
+  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
+#else
+  rotorcraft_cam_tilt_pwm = 1500;
+#endif
+  rotorcraft_cam_tilt = 0;
+  rotorcraft_cam_pan = 0;
+}
+
+void rotorcraft_cam_periodic(void) {
+
+  switch (rotorcraft_cam_mode) {
+    case ROTORCRAFT_CAM_MODE_NONE:
+#if ROTORCRAFT_CAM_USE_TILT
+      rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
+#endif
+#if ROTORCRAFT_CAM_USE_PAN
+      rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
+#endif
+      break;
+    case ROTORCRAFT_CAM_MODE_MANUAL:
+      // nothing to do here, just apply tilt pwm at the end
+      break;
+    case ROTORCRAFT_CAM_MODE_HEADING:
+#if ROTORCRAFT_CAM_USE_TILT_ANGLES
+      Bound(rotorcraft_cam_tilt,CT_MIN,CT_MAX);
+      rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
+#endif
+#if ROTORCRAFT_CAM_USE_PAN
+      INT32_COURSE_NORMALIZE(rotorcraft_cam_pan);
+      nav_heading = rotorcraft_cam_pan;
+#endif
+      break;
+    case ROTORCRAFT_CAM_MODE_WP:
+#ifdef ROTORCRAFT_CAM_TRACK_WP
+      {
+        struct Int32Vect2 diff;
+        VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
+        INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
+        INT32_ATAN2(rotorcraft_cam_pan,diff.x,diff.y);
+        nav_heading = rotorcraft_cam_pan;
+#if ROTORCRAFT_CAM_USE_TILT_ANGLES
+        int32_t dist, height;
+        INT32_VECT2_NORM(dist, diff);
+        height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
+        INT32_ATAN2(rotorcraft_cam_tilt, height, dist);
+        Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
+        rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
+#endif
+      }
+#endif
+      break;
+  }
+#if ROTORCRAFT_CAM_USE_TILT
+  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
+#endif
+}
+
diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.h b/sw/airborne/modules/cam_control/rotorcraft_cam.h
new file mode 100644
index 0000000000..1b2babfe80
--- /dev/null
+++ b/sw/airborne/modules/cam_control/rotorcraft_cam.h
@@ -0,0 +1,139 @@
+/*
+ * Copyright (C) 2009-2012 Gautier Hattenberger <gautier.hattenberger@laas.fr>,
+ *                    Antoine Drouin <poinix@gmail.com>
+ *
+ * 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.
+ */
+
+/** Camera control module for rotorcraft.
+ *
+ * The camera is controled by the heading of the vehicle for pan
+ * and can be controlled by a servo for tilt if defined.
+ *
+ * Four modes:
+ *  - NONE: no control
+ *  - MANUAL: the servo position is set with PWM
+ *  - HEADING: the servo position and the heading of the rotorcraft are set with angles
+ *  - WP: the camera is tracking a waypoint (Default: CAM)
+ *
+ * The CAM_SWITCH can be used to power the camera in normal modes
+ * and disable it when in NONE mode
+ */
+
+#ifndef ROTORCRAFT_CAM_H
+#define ROTORCRAFT_CAM_H
+
+#include "generated/airframe.h"
+#include "generated/flight_plan.h"
+#include "math/pprz_algebra_int.h"
+#include "std.h"
+#include "led.h"
+
+#define ROTORCRAFT_CAM_MODE_NONE     0
+#define ROTORCRAFT_CAM_MODE_MANUAL   1
+#define ROTORCRAFT_CAM_MODE_HEADING  2
+#define ROTORCRAFT_CAM_MODE_WP       3
+
+/** Default mode is NONE. */
+#ifndef ROTORCRAFT_CAM_DEFAULT_MODE
+#define ROTORCRAFT_CAM_DEFAULT_MODE ROTORCRAFT_CAM_MODE_NONE
+#endif
+
+/** Cam power control.
+ * By default CAM_SWITCH is used
+ * Warning:
+ *  LED_ON set GPIO low on some boards (lpc)
+ *  LED_OFF set GPIO high on some boards (lpc)
+ */
+#ifndef ROTORCRAFT_CAM_ON
+#define ROTORCRAFT_CAM_ON LED_OFF(CAM_SWITCH_LED)
+#endif
+#ifndef ROTORCRAFT_CAM_OFF
+#define ROTORCRAFT_CAM_OFF LED_ON(CAM_SWITCH_LED)
+#endif
+
+/** Cam tilt control.
+ * By default use tilt control if a servo is assigned
+ */
+#ifdef ROTORCRAFT_CAM_TILT_SERVO
+#define ROTORCRAFT_CAM_USE_TILT 1
+#else
+#define ROTORCRAFT_CAM_USE_TILT 0
+#endif
+
+/** Use angles for tilt in HEADING and WP modes.
+ */
+#if defined ROTORCRAFT_CAM_TILT_ANGLE_MIN && defined ROTORCRAFT_CAM_TILT_ANGLE_MAX && defined ROTORCRAFT_CAM_USE_TILT
+#define CAM_TA_MIN ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_TILT_ANGLE_MIN)
+#define CAM_TA_MAX ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_TILT_ANGLE_MAX)
+#define ROTORCRAFT_CAM_USE_TILT_ANGLES 1
+#endif
+
+/** Cam pan control.
+ * By default use pan control (heading)
+ */
+#ifndef ROTORCRAFT_CAM_USE_PAN
+#define ROTORCRAFT_CAM_USE_PAN 1
+#endif
+
+/** WP control.
+ * By default use WP_CAM waypoint if defined
+ */
+#ifndef ROTORCRAFT_CAM_TRACK_WP
+#ifdef WP_CAM
+#define ROTORCRAFT_CAM_TRACK_WP WP_CAM
+#endif
+#endif
+
+extern uint8_t rotorcraft_cam_mode;
+
+extern int16_t rotorcraft_cam_tilt;
+extern int16_t rotorcraft_cam_pan;
+extern int16_t rotorcraft_cam_tilt_pwm;
+
+extern void rotorcraft_cam_init(void);
+extern void rotorcraft_cam_periodic(void);
+
+/** Set camera mode.
+ * Camera is powered down in NONE mode if CAM_{ON|OFF} are defined
+ */
+#define rotorcraft_cam_SetCamMode(_v) { \
+  rotorcraft_cam_mode = _v; \
+  if (rotorcraft_cam_mode == ROTORCRAFT_CAM_MODE_NONE) { ROTORCRAFT_CAM_OFF; } \
+  else { ROTORCRAFT_CAM_ON; } \
+}
+
+/** Cam control from datalink message.
+ * camera tilt and pan are incremented by STICK_TILT_INC and STICK_PAN_INC
+ * when maximum command is received from the stick
+ */
+#ifndef ROTORCRAFT_CAM_STICK_TILT_INC
+#define ROTORCRAFT_CAM_STICK_TILT_INC RadOfDeg(10.)
+#endif
+#ifndef ROTORCRAFT_CAM_STICK_PAN_INC
+#define ROTORCRAFT_CAM_STICK_PAN_INC RadOfDeg(20.)
+#endif
+
+#define ROTORCRAFT_CAM_STICK_PARSE(_dl_buffer) { \
+  rotorcraft_cam_tilt += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_TILT_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_tilt(_dl_buffer)); \
+  rotorcraft_cam_pan += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_PAN_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_pan(dl_buffer)); \
+  INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); \
+}
+
+#endif /* ROTORCRAFT_CAM_H */
+
diff --git a/sw/airborne/subsystems/actuators.h b/sw/airborne/subsystems/actuators.h
index f742d02281..04d90811b6 100644
--- a/sw/airborne/subsystems/actuators.h
+++ b/sw/airborne/subsystems/actuators.h
@@ -49,7 +49,8 @@ extern int16_t actuators[ACTUATORS_NB];
  * @param _n actuators name as given in airframe file, servos section
  * @param _v new actuator's value
  */
-#define ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v)
+#define _ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v)
+#define ActuatorSet(_n, _v) _ActuatorSet(_n, _v)
 
 #endif /* ACTUATORS_NB */
 

From 6d06db2b7b63024674830106c48705879a562b80 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 18 Oct 2012 17:16:05 +0200
Subject: [PATCH 10/26] [actuators] stop motor is min pprz

this allows to define the stop value with "min", idle with "neutral" and
full throttle with "max" in servo xml node
---
 sw/airborne/subsystems/actuators/supervision.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/subsystems/actuators/supervision.c b/sw/airborne/subsystems/actuators/supervision.c
index dde7e1f7b7..00b701e005 100644
--- a/sw/airborne/subsystems/actuators/supervision.c
+++ b/sw/airborne/subsystems/actuators/supervision.c
@@ -41,7 +41,7 @@
 #define SUPERVISION_MAX_MOTOR MAX_PPRZ
 
 #ifndef SUPERVISION_STOP_MOTOR
-#define SUPERVISION_STOP_MOTOR 0
+#define SUPERVISION_STOP_MOTOR -MAX_PPRZ
 #endif
 
 #ifndef SUPERVISION_MIN_MOTOR_STARTUP

From 278ef4e065b2e1b13cbd44a2ac143994c0e853af Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 18 Oct 2012 17:17:53 +0200
Subject: [PATCH 11/26] [servo_switch] group gcs buttons

---
 conf/settings/modules/servo_switch.xml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/conf/settings/modules/servo_switch.xml b/conf/settings/modules/servo_switch.xml
index fe29a120c9..687f0c7bb5 100644
--- a/conf/settings/modules/servo_switch.xml
+++ b/conf/settings/modules/servo_switch.xml
@@ -4,8 +4,8 @@
   <dl_settings NAME="Servo switch control">
     <dl_settings NAME="SWITCH">
       <dl_setting var="servo_switch_on" min="0" step="1" max="1" module="servo_switch/servo_switch" values="Off|On">
-        <strip_button name="ON"  value="1"/>
-        <strip_button name="OFF" value="0"/>
+        <strip_button name="ON"  value="1" group="servo_switch"/>
+        <strip_button name="OFF" value="0" group="servo_switch"/>
       </dl_setting>
     </dl_settings>
   </dl_settings>

From bb15ef1516f97334debc2a4e0075ba5eb44aa925 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 18 Oct 2012 18:28:58 +0200
Subject: [PATCH 12/26] [servo_switch] include airframe to allow redefinition

---
 sw/airborne/modules/servo_switch/servo_switch.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/sw/airborne/modules/servo_switch/servo_switch.h b/sw/airborne/modules/servo_switch/servo_switch.h
index c0a3ed7450..b2fbfc2476 100644
--- a/sw/airborne/modules/servo_switch/servo_switch.h
+++ b/sw/airborne/modules/servo_switch/servo_switch.h
@@ -26,6 +26,7 @@
 
 #include "std.h"
 #include "paparazzi.h"
+#include "generated/airframe.h"
 
 extern bool_t servo_switch_on;
 extern int16_t servo_switch_value;

From cdbdf91bfc487df9f82c097840dc3febb48876bc Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 18 Oct 2012 18:30:23 +0200
Subject: [PATCH 13/26] [motor_mixing] old supervision

- rename supervision to motor_mixing
- split into a separated subsystem
- pwm_supervision is now pwm direct + motor mixing
---
 conf/airframes/ENAC/quadrotor/blender.xml     |  19 +-
 .../shared/actuators_asctec_v2.makefile       |  16 +-
 .../subsystems/shared/actuators_mkk.makefile  |  16 +-
 .../shared/actuators_pwm_supervision.makefile |   6 -
 .../shared/actuators_skiron.makefile          |  16 +-
 .../subsystems/shared/motor_mixing.makefile   |  19 ++
 .../rotorcraft/guidance/guidance_v.h          |  11 --
 sw/airborne/firmwares/rotorcraft/main.c       |   7 +
 .../subsystems/actuators/actuators_asctec.c   |   4 -
 .../subsystems/actuators/actuators_asctec.h   |   3 -
 .../subsystems/actuators/actuators_mkk.c      |   1 -
 .../subsystems/actuators/actuators_mkk.h      |   1 -
 .../subsystems/actuators/actuators_pwm.c      |   3 -
 .../subsystems/actuators/actuators_pwm.h      |   4 -
 .../subsystems/actuators/actuators_skiron.c   |   1 -
 .../subsystems/actuators/actuators_skiron.h   |   2 -
 .../subsystems/actuators/motor_mixing.c       | 186 ++++++++++++++++++
 .../{supervision.h => motor_mixing.h}         |  28 +--
 .../subsystems/actuators/supervision.c        | 186 ------------------
 19 files changed, 242 insertions(+), 287 deletions(-)
 delete mode 100644 conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile
 create mode 100644 conf/firmwares/subsystems/shared/motor_mixing.makefile
 create mode 100644 sw/airborne/subsystems/actuators/motor_mixing.c
 rename sw/airborne/subsystems/actuators/{supervision.h => motor_mixing.h} (63%)
 delete mode 100644 sw/airborne/subsystems/actuators/supervision.c

diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index 11ccf464c0..aab8d9aeae 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -29,6 +29,7 @@
     <subsystem name="actuators"     type="skiron">
       <define name="SKIRON_I2C_SCL_TIME" value="25"/>
     </subsystem>
+    <subsystem name="motor_mixing"/>
     <subsystem name="actuators"     type="direct">
       <define name="USE_PWM0"/>
     </subsystem>
@@ -41,10 +42,10 @@
     <subsystem name="ins"           type="hff"/>
   </firmware>
 
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="TRIM_A" value="-50"/>
-    <define name="TRIM_E" value="-150"/>
-    <define name="TRIM_R" value="-100"/>
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="-50"/>
+    <define name="TRIM_PITCH" value="-150"/>
+    <define name="TRIM_YAW" value="-100"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"  value="{    0, -256,    0,  256}"/>
@@ -72,11 +73,11 @@
   </commands>
 
   <command_laws>
-    <call fun="supervision_run(autopilot_motors_on,FALSE,values)"/>
-    <set servo="FRONT" value="supervision.commands[SERVO_FRONT]"/>
-    <set servo="BACK" value="supervision.commands[SERVO_BACK]"/>
-    <set servo="RIGHT" value="supervision.commands[SERVO_RIGHT]"/>
-    <set servo="LEFT" value="supervision.commands[SERVO_LEFT]"/>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+    <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+    <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+    <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
   </command_laws>
 
   <section name="IMU" prefix="IMU_">
diff --git a/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
index 7c33e7794c..1133841086 100644
--- a/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
@@ -1,21 +1,12 @@
 # asctec controllers v2
 #
-#  <section name="SUPERVISION" prefix="SUPERVISION_">
-#    <define name="TRIM_A" value="2"/>
-#    <define name="TRIM_E" value="-1"/>
-#    <define name="TRIM_R" value="3"/>
-#    <define name="NB_MOTOR" value="4"/>
-#    <define name="SCALE" value="256"/>
-#    <define name="ROLL_COEF"   value="{    0,    0, -256,  256}"/>
-#    <define name="PITCH_COEF"  value="{ 256,  -256,    0,    0}"/>
-#    <define name="YAW_COEF"    value="{-256,  -256,  256,  256}"/>
-#    <define name="THRUST_COEF" value="{ 256,   256,  256,  256}"/>
-#  </section>
+# required xml configuration:
 #
+#  servo section with driver="Asctec"
+#  command_laws section to map motor_mixing commands to servos
 #
 
 $(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/supervision.c
 ap.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL
 ap.srcs += subsystems/actuators/actuators_asctec.c
 
@@ -30,6 +21,5 @@ ap.CFLAGS += -DUSE_I2C1
 endif
 
 # Simulator
-nps.srcs += subsystems/actuators/supervision.c
 nps.srcs += subsystems/actuators/actuators_asctec.c
 nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0
diff --git a/conf/firmwares/subsystems/shared/actuators_mkk.makefile b/conf/firmwares/subsystems/shared/actuators_mkk.makefile
index c62298ca03..a9c290a0d2 100644
--- a/conf/firmwares/subsystems/shared/actuators_mkk.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_mkk.makefile
@@ -17,22 +17,11 @@
 #    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
 #  </section>
 #
-#  <section name="SUPERVISION" prefix="SUPERVISION_">
-#    <define name="TRIM_A" value="2"/>
-#    <define name="TRIM_E" value="-1"/>
-#    <define name="TRIM_R" value="3"/>
-#    <define name="NB_MOTOR" value="4"/>
-#    <define name="SCALE" value="256"/>
-#    <define name="ROLL_COEF"  value="{    0,    0, -256,  256}"/>
-#    <define name="PITCH_COEF" value="{  256, -256,    0,    0}"/>
-#    <define name="YAW_COEF"   value="{ -256, -256,  256,  256}"/>
-#    <define name="THRUST_COEF" value="{ 256,  256,  256,  256}"/>
-#  </section>
-#
+#  servo section with driver="Mkk"
+#  command_laws section to map motor_mixing commands to servos
 #
 
 $(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/supervision.c
 ap.srcs += subsystems/actuators/actuators_mkk.c
 
 ifeq ($(ARCH), lpc21)
@@ -51,6 +40,5 @@ ap.CFLAGS += -DUSE_I2C1
 endif
 
 # Simulator
-nps.srcs += subsystems/actuators/supervision.c
 nps.srcs += subsystems/actuators/actuators_mkk.c
 nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_DEVICE=i2c0
diff --git a/conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile b/conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile
deleted file mode 100644
index 3e5ed13d89..0000000000
--- a/conf/firmwares/subsystems/shared/actuators_pwm_supervision.makefile
+++ /dev/null
@@ -1,6 +0,0 @@
-
-$(TARGET).CFLAGS += -DACTUATORS -DUSE_SUPERVISION
-$(TARGET).srcs   += subsystems/actuators/supervision.c
-$(TARGET).srcs   += subsystems/actuators/actuators_pwm.c
-$(TARGET).srcs   += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
-
diff --git a/conf/firmwares/subsystems/shared/actuators_skiron.makefile b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
index 7361869cfe..86375c977d 100644
--- a/conf/firmwares/subsystems/shared/actuators_skiron.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
@@ -13,20 +13,8 @@
 #
 # required xml configuration:
 #
-#  <section name="SUPERVISION" prefix="SUPERVISION_">
-#    <define name="TRIM_A" value="0"/>
-#    <define name="TRIM_E" value="0"/>
-#    <define name="TRIM_R" value="0"/>
-#    <define name="NB_MOTOR" value="4"/>
-#    <define name="SCALE" value="256"/>
-#    <define name="ROLL_COEF"  value="{    0,    0, -256,  256}"/>
-#    <define name="PITCH_COEF" value="{  256, -256,    0,    0}"/>
-#    <define name="YAW_COEF"   value="{ -256, -256,  256,  256}"/>
-#    <define name="THRUST_COEF" value="{ 256,  256,  256,  256}"/>
-#  </section>
-#
 #  servo section with driver="Skiron"
-#  command_laws section to map supervision commands to servos
+#  command_laws section to map motor_mixing commands to servos
 #
 
 # set default i2c timing if not already configured
@@ -35,7 +23,6 @@ SKIRON_I2C_SCL_TIME=150
 endif
 
 $(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/supervision.c
 ap.srcs += subsystems/actuators/actuators_skiron.c
 
 ifeq ($(ARCH), lpc21)
@@ -44,7 +31,6 @@ ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_
 endif
 
 # Simulator
-nps.srcs += subsystems/actuators/supervision.c
 nps.srcs += subsystems/actuators/actuators_skiron.c
 nps.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME) -DI2C0_VIC_SLOT=10 -DACTUATORS_SKIRON_DEVICE=i2c0
 
diff --git a/conf/firmwares/subsystems/shared/motor_mixing.makefile b/conf/firmwares/subsystems/shared/motor_mixing.makefile
new file mode 100644
index 0000000000..c621595cb8
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/motor_mixing.makefile
@@ -0,0 +1,19 @@
+#  Motor Mixing
+#
+#  <section name="MIXING" prefix="MOTOR_MIXING_">
+#    <define name="TRIM_ROLL" value="2"/>
+#    <define name="TRIM_PITCH" value="-1"/>
+#    <define name="TRIM_YAW" value="3"/>
+#    <define name="NB_MOTOR" value="4"/>
+#    <define name="SCALE" value="256"/>
+#    <define name="ROLL_COEF"   value="{    0,    0, -256,  256}"/>
+#    <define name="PITCH_COEF"  value="{ 256,  -256,    0,    0}"/>
+#    <define name="YAW_COEF"    value="{-256,  -256,  256,  256}"/>
+#    <define name="THRUST_COEF" value="{ 256,   256,  256,  256}"/>
+#  </section>
+#
+#
+
+$(TARGET).CFLAGS += -DUSE_MOTOR_MIXING
+$(TARGET).srcs   += subsystems/actuators/motor_mixing.c
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
index 28b887eb9b..56c2437215 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
@@ -32,17 +32,6 @@
 #include "generated/airframe.h"
 #include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
 
-/** Supervision default bounds
- *  In case Asctec controllers are used without supervision
- *  Used in control and adaptation filter
- * */
-#ifndef SUPERVISION_MIN_MOTOR
-#define SUPERVISION_MIN_MOTOR 1
-#endif
-#ifndef SUPERVISION_MAX_MOTOR
-#define SUPERVISION_MAX_MOTOR 200
-#endif
-
 #include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
 
 #define GUIDANCE_V_MODE_KILL      0
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index ec47388565..e927c539a8 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -37,6 +37,9 @@
 
 #include "firmwares/rotorcraft/commands.h"
 #include "subsystems/actuators.h"
+#if USE_MOTOR_MIXING
+#include "subsystems/actuators/motor_mixing.h"
+#endif
 
 #include "subsystems/imu.h"
 #include "subsystems/gps.h"
@@ -100,6 +103,10 @@ STATIC_INLINE void main_init( void ) {
   stateInit();
 
   actuators_init();
+#if USE_MOTOR_MIXING
+  motor_mixing_init();
+#endif
+
   radio_control_init();
 
 #if DATALINK == XBEE
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec.c b/sw/airborne/subsystems/actuators/actuators_asctec.c
index 667757296d..d8ed2ce1c5 100644
--- a/sw/airborne/subsystems/actuators/actuators_asctec.c
+++ b/sw/airborne/subsystems/actuators/actuators_asctec.c
@@ -62,10 +62,6 @@ void actuators_asctec_init(void) {
   actuators_delay_time = 0;
 #endif
 
-#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
-  supervision_init();
-#endif
-
 }
 
 #ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec.h b/sw/airborne/subsystems/actuators/actuators_asctec.h
index fc6aae40e0..1c4fa3dffd 100644
--- a/sw/airborne/subsystems/actuators/actuators_asctec.h
+++ b/sw/airborne/subsystems/actuators/actuators_asctec.h
@@ -29,9 +29,6 @@
 #include "mcu_periph/i2c.h"
 
 #include "generated/airframe.h"
-#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
-#include "subsystems/actuators/supervision.h"
-#endif
 
 enum actuators_asctec_cmd { NONE,
                            TEST,
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.c b/sw/airborne/subsystems/actuators/actuators_mkk.c
index e968615882..23f8c0c0e8 100644
--- a/sw/airborne/subsystems/actuators/actuators_mkk.c
+++ b/sw/airborne/subsystems/actuators/actuators_mkk.c
@@ -38,7 +38,6 @@ bool_t   actuators_delay_done;
 
 void actuators_mkk_init(void) {
 
-  supervision_init();
   const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
   for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
     actuators_mkk.trans[i].type = I2CTransTx;
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.h b/sw/airborne/subsystems/actuators/actuators_mkk.h
index 0054ae34c2..f1436e4373 100644
--- a/sw/airborne/subsystems/actuators/actuators_mkk.h
+++ b/sw/airborne/subsystems/actuators/actuators_mkk.h
@@ -30,7 +30,6 @@
 #include "mcu_periph/i2c.h"
 
 #include "generated/airframe.h"
-#include "subsystems/actuators/supervision.h"
 
 
 struct ActuatorsMkk {
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.c b/sw/airborne/subsystems/actuators/actuators_pwm.c
index 0f3bfe636b..0606133b38 100644
--- a/sw/airborne/subsystems/actuators/actuators_pwm.c
+++ b/sw/airborne/subsystems/actuators/actuators_pwm.c
@@ -23,9 +23,6 @@
 
 void actuators_pwm_init(void)
 {
-#if USE_SUPERVISION
-  supervision_init();
-#endif
   // implemented in arch files
   actuators_pwm_arch_init();
 }
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.h b/sw/airborne/subsystems/actuators/actuators_pwm.h
index fbbd28570a..80953089fa 100644
--- a/sw/airborne/subsystems/actuators/actuators_pwm.h
+++ b/sw/airborne/subsystems/actuators/actuators_pwm.h
@@ -28,10 +28,6 @@
 #include BOARD_CONFIG
 #include "subsystems/actuators/actuators_pwm_arch.h"
 
-#if USE_SUPERVISION
-#include "subsystems/actuators/supervision.h"
-#endif
-
 extern void actuators_pwm_init(void);
 
 #define ActuatorsPwmInit() actuators_pwm_init()
diff --git a/sw/airborne/subsystems/actuators/actuators_skiron.c b/sw/airborne/subsystems/actuators/actuators_skiron.c
index 79182faa6e..8bce7b3370 100644
--- a/sw/airborne/subsystems/actuators/actuators_skiron.c
+++ b/sw/airborne/subsystems/actuators/actuators_skiron.c
@@ -37,7 +37,6 @@ bool_t   actuators_delay_done;
 
 void actuators_skiron_init(void) {
 
-  supervision_init();
   actuators_skiron.trans.type = I2CTransTx;
   actuators_skiron.trans.len_w = SERVOS_SKIRON_NB;
   actuators_skiron.trans.slave_addr = ACTUATORS_SKIRON_I2C_ADDR;
diff --git a/sw/airborne/subsystems/actuators/actuators_skiron.h b/sw/airborne/subsystems/actuators/actuators_skiron.h
index 09ceb16fd7..7c8aebdcaf 100644
--- a/sw/airborne/subsystems/actuators/actuators_skiron.h
+++ b/sw/airborne/subsystems/actuators/actuators_skiron.h
@@ -30,8 +30,6 @@
 #include "mcu_periph/i2c.h"
 
 #include "generated/airframe.h"
-#include "subsystems/actuators/supervision.h"
-
 
 // Use I2C broadcast adderss
 #define ACTUATORS_SKIRON_I2C_ADDR 0x00
diff --git a/sw/airborne/subsystems/actuators/motor_mixing.c b/sw/airborne/subsystems/actuators/motor_mixing.c
new file mode 100644
index 0000000000..5975c4ca8f
--- /dev/null
+++ b/sw/airborne/subsystems/actuators/motor_mixing.c
@@ -0,0 +1,186 @@
+/*
+ * Copyright (C) 2008-2012 The Paparazzi Team
+ *
+ * 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.
+ */
+
+/** @file motor_mixing.c
+ *  Motor Mixing.
+ *  Handles the mapping of roll/pitch/yaw commands
+ *  to actual motor commands.
+ */
+
+#include "subsystems/actuators/motor_mixing.h"
+#include "paparazzi.h"
+
+//#include <stdint.h>
+#ifndef INT32_MIN
+#define INT32_MIN (-2147483647-1)
+#endif
+
+#ifndef INT32_MAX
+#define INT32_MAX (2147483647)
+#endif
+
+#define MOTOR_MIXING_MIN_MOTOR 0
+#define MOTOR_MIXING_MAX_MOTOR MAX_PPRZ
+
+#ifndef MOTOR_MIXING_STOP_MOTOR
+#define MOTOR_MIXING_STOP_MOTOR -MAX_PPRZ
+#endif
+
+#ifndef MOTOR_MIXING_MIN_MOTOR_STARTUP
+#define MOTOR_MIXING_MIN_MOTOR_STARTUP MOTOR_MIXING_MIN_MOTOR
+#endif
+
+#if defined (MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP) || defined (MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP)
+#define MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
+
+#ifndef MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP
+#define MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
+#endif
+/*
+#ifndef MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP
+#define MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP INT32_MAX
+#endif
+*/
+#endif
+
+static const int32_t roll_coef[MOTOR_MIXING_NB_MOTOR]   = MOTOR_MIXING_ROLL_COEF;
+static const int32_t pitch_coef[MOTOR_MIXING_NB_MOTOR]  = MOTOR_MIXING_PITCH_COEF;
+static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR]    = MOTOR_MIXING_YAW_COEF;
+static const int32_t thrust_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_THRUST_COEF;
+
+struct MotorMixing motor_mixing;
+
+void motor_mixing_init(void) {
+  uint8_t i;
+  for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
+    motor_mixing.commands[i] = 0;
+    motor_mixing.trim[i] =
+      roll_coef[i]  * MOTOR_MIXING_TRIM_ROLL +
+      pitch_coef[i] * MOTOR_MIXING_TRIM_PITCH +
+      yaw_coef[i]   * MOTOR_MIXING_TRIM_YAW;
+    motor_mixing.override_enabled[i] = FALSE;
+    motor_mixing.override_value[i] = MOTOR_MIXING_STOP_MOTOR;
+  }
+  motor_mixing.nb_failure = 0;
+}
+
+__attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
+  uint8_t j;
+  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
+    motor_mixing.commands[j] += (offset);
+}
+
+__attribute__ ((always_inline)) static inline void bound_commands(void) {
+  uint8_t j;
+  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
+    Bound(motor_mixing.commands[j],
+          MOTOR_MIXING_MIN_MOTOR, MOTOR_MIXING_MAX_MOTOR);
+}
+
+#ifdef MOTOR_MIXING_USE_MAX_MOTOR_STEP_BINDING
+__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
+  uint8_t j;
+  static int32_t prev_commands[MOTOR_MIXING_NB_MOTOR];
+  static uint8_t initialized = 0;
+
+  if (initialized == 1) {
+    for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++) {
+      int32_t new_command_diff = motor_mixing.commands[j] - prev_commands[j];
+      Bound(new_command_diff,
+            MOTOR_MIXING_MAX_NEGATIVE_MOTOR_STEP, MOTOR_MIXING_MAX_POSITIVE_MOTOR_STEP);
+      motor_mixing.commands[j] = prev_commands[j] + new_command_diff;
+    }
+  }else{
+    initialized = 1;
+  }
+
+  for (j=0; j<MOTOR_MIXING_NB_MOTOR; j++)
+    prev_commands[j] = motor_mixing.commands[j];
+}
+#else
+__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
+}
+#endif
+
+void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
+{
+  int i;
+  for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
+#ifdef MOTOR_MIXING_STARTUP_DELAY
+    if (counter > i * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
+      if (counter > MOTOR_MIXING_NB_MOTOR * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) {
+        motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP + (MOTOR_MIXING_MIN_MOTOR - MOTOR_MIXING_MIN_MOTOR_STARTUP) * counter / max_counter;
+      } else {
+        motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP;
+      }
+    } else {
+      motor_mixing.commands[i] = 0;
+    }
+#else
+    if (counter < i * max_counter / MOTOR_MIXING_NB_MOTOR) {
+      motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP;
+    }
+#endif
+  }
+}
+
+void motor_mixing_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
+  uint8_t i;
+  if (motors_on) {
+    int32_t min_cmd = INT32_MAX;
+    int32_t max_cmd = INT32_MIN;
+    /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
+    for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
+      motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR +
+        (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
+         roll_coef[i]   * in_cmd[COMMAND_ROLL]   +
+         pitch_coef[i]  * in_cmd[COMMAND_PITCH]  +
+         yaw_coef[i]    * in_cmd[COMMAND_YAW]    +
+         motor_mixing.trim[i]) / MOTOR_MIXING_SCALE *
+        (MOTOR_MIXING_MAX_MOTOR - MOTOR_MIXING_MIN_MOTOR) / MAX_PPRZ;
+      if (motor_mixing.commands[i] < min_cmd)
+        min_cmd = motor_mixing.commands[i];
+      if (motor_mixing.commands[i] > max_cmd)
+        max_cmd = motor_mixing.commands[i];
+    }
+    if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR)
+      motor_mixing.nb_failure++;
+    if (min_cmd < MOTOR_MIXING_MIN_MOTOR)
+      offset_commands(-(min_cmd - MOTOR_MIXING_MIN_MOTOR));
+    if (max_cmd > MOTOR_MIXING_MAX_MOTOR)
+      offset_commands(-(max_cmd - MOTOR_MIXING_MAX_MOTOR));
+
+    /* For testing motor failure */
+    if (motors_on && override_on) {
+      for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
+        if (motor_mixing.override_enabled[i])
+          motor_mixing.commands[i] = motor_mixing.override_value[i];
+      }
+    }
+    bound_commands();
+    bound_commands_step();
+  }
+  else {
+    for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
+      motor_mixing.commands[i] = MOTOR_MIXING_STOP_MOTOR;
+    }
+  }
+}
diff --git a/sw/airborne/subsystems/actuators/supervision.h b/sw/airborne/subsystems/actuators/motor_mixing.h
similarity index 63%
rename from sw/airborne/subsystems/actuators/supervision.h
rename to sw/airborne/subsystems/actuators/motor_mixing.h
index 66621af8eb..ad69d15441 100644
--- a/sw/airborne/subsystems/actuators/supervision.h
+++ b/sw/airborne/subsystems/actuators/motor_mixing.h
@@ -19,30 +19,30 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file supervision.h
- *  Supervision.
+/** @file motor_mixing.h
+ *  Motor Mixing.
  *  Handles the mapping of roll/pitch/yaw commands
  *  to actual motor commands.
  */
 
-#ifndef SUPERVISION_H
-#define SUPERVISION_H
+#ifndef MOTOR_MIXING_H
+#define MOTOR_MIXING_H
 
 #include "std.h"
 #include "generated/airframe.h"
 
-struct Supervision {
-  int32_t commands[SUPERVISION_NB_MOTOR];
-  int32_t trim[SUPERVISION_NB_MOTOR];
-  bool_t override_enabled[SUPERVISION_NB_MOTOR];
-  int32_t override_value[SUPERVISION_NB_MOTOR];
+struct MotorMixing {
+  int32_t commands[MOTOR_MIXING_NB_MOTOR];
+  int32_t trim[MOTOR_MIXING_NB_MOTOR];
+  bool_t override_enabled[MOTOR_MIXING_NB_MOTOR];
+  int32_t override_value[MOTOR_MIXING_NB_MOTOR];
   uint32_t nb_failure;
 };
 
-extern struct Supervision supervision;
+extern struct MotorMixing motor_mixing;
 
-extern void supervision_init(void);
-extern void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[]);
-extern void supervision_run_spinup(uint32_t counter, uint32_t max_counter);
+extern void motor_mixing_init(void);
+extern void motor_mixing_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[]);
+extern void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter);
 
-#endif /* SUPERVISION_H */
+#endif /* MOTOR_MIXING_H */
diff --git a/sw/airborne/subsystems/actuators/supervision.c b/sw/airborne/subsystems/actuators/supervision.c
deleted file mode 100644
index 00b701e005..0000000000
--- a/sw/airborne/subsystems/actuators/supervision.c
+++ /dev/null
@@ -1,186 +0,0 @@
-/*
- * Copyright (C) 2008-2012 The Paparazzi Team
- *
- * 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.
- */
-
-/** @file supervision.c
- *  Supervision.
- *  Handles the mapping of roll/pitch/yaw commands
- *  to actual motor commands.
- */
-
-#include "subsystems/actuators/supervision.h"
-#include "paparazzi.h"
-
-//#include <stdint.h>
-#ifndef INT32_MIN
-#define INT32_MIN (-2147483647-1)
-#endif
-
-#ifndef INT32_MAX
-#define INT32_MAX (2147483647)
-#endif
-
-#define SUPERVISION_MIN_MOTOR 0
-#define SUPERVISION_MAX_MOTOR MAX_PPRZ
-
-#ifndef SUPERVISION_STOP_MOTOR
-#define SUPERVISION_STOP_MOTOR -MAX_PPRZ
-#endif
-
-#ifndef SUPERVISION_MIN_MOTOR_STARTUP
-#define SUPERVISION_MIN_MOTOR_STARTUP SUPERVISION_MIN_MOTOR
-#endif
-
-#if defined (SUPERVISION_MAX_NEGATIVE_MOTOR_STEP) || defined (SUPERVISION_MAX_POSITIVE_MOTOR_STEP)
-#define SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
-
-#ifndef SUPERVISION_MAX_NEGATIVE_MOTOR_STEP
-#define SUPERVISION_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
-#endif
-/*
-#ifndef SUPERVISION_MAX_POSITIVE_MOTOR_STEP
-#define SUPERVISION_MAX_POSITIVE_MOTOR_STEP INT32_MAX
-#endif
-*/
-#endif
-
-static const int32_t roll_coef[SUPERVISION_NB_MOTOR]   = SUPERVISION_ROLL_COEF;
-static const int32_t pitch_coef[SUPERVISION_NB_MOTOR]  = SUPERVISION_PITCH_COEF;
-static const int32_t yaw_coef[SUPERVISION_NB_MOTOR]    = SUPERVISION_YAW_COEF;
-static const int32_t thrust_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_THRUST_COEF;
-
-struct Supervision supervision;
-
-void supervision_init(void) {
-  uint8_t i;
-  for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
-    supervision.commands[i] = 0;
-    supervision.trim[i] =
-      roll_coef[i]  * SUPERVISION_TRIM_A +
-      pitch_coef[i] * SUPERVISION_TRIM_E +
-      yaw_coef[i]   * SUPERVISION_TRIM_R;
-    supervision.override_enabled[i] = FALSE;
-    supervision.override_value[i] = SUPERVISION_STOP_MOTOR;
-  }
-  supervision.nb_failure = 0;
-}
-
-__attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) {
-  uint8_t j;
-  for (j=0; j<SUPERVISION_NB_MOTOR; j++)
-    supervision.commands[j] += (offset);
-}
-
-__attribute__ ((always_inline)) static inline void bound_commands(void) {
-  uint8_t j;
-  for (j=0; j<SUPERVISION_NB_MOTOR; j++)
-    Bound(supervision.commands[j],
-          SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
-}
-
-#ifdef SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
-__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
-  uint8_t j;
-  static int32_t prev_commands[SUPERVISION_NB_MOTOR];
-  static uint8_t initialized = 0;
-
-  if (initialized == 1) {
-    for (j=0; j<SUPERVISION_NB_MOTOR; j++) {
-      int32_t new_command_diff = supervision.commands[j] - prev_commands[j];
-      Bound(new_command_diff,
-            SUPERVISION_MAX_NEGATIVE_MOTOR_STEP, SUPERVISION_MAX_POSITIVE_MOTOR_STEP);
-      supervision.commands[j] = prev_commands[j] + new_command_diff;
-    }
-  }else{
-    initialized = 1;
-  }
-
-  for (j=0; j<SUPERVISION_NB_MOTOR; j++)
-    prev_commands[j] = supervision.commands[j];
-}
-#else
-__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
-}
-#endif
-
-void supervision_run_spinup(uint32_t counter, uint32_t max_counter)
-{
-  int i;
-  for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
-#ifdef SUPERVISION_STARTUP_DELAY
-    if (counter > i * max_counter / (SUPERVISION_NB_MOTOR + SUPERVISION_STARTUP_DELAY)) {
-      if (counter > SUPERVISION_NB_MOTOR * max_counter / (SUPERVISION_NB_MOTOR + SUPERVISION_STARTUP_DELAY)) {
-        supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP + (SUPERVISION_MIN_MOTOR - SUPERVISION_MIN_MOTOR_STARTUP) * counter / max_counter;
-      } else {
-        supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP;
-      }
-    } else {
-      supervision.commands[i] = 0;
-    }
-#else
-    if (counter < i * max_counter / SUPERVISION_NB_MOTOR) {
-      supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP;
-    }
-#endif
-  }
-}
-
-void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
-  uint8_t i;
-  if (motors_on) {
-    int32_t min_cmd = INT32_MAX;
-    int32_t max_cmd = INT32_MIN;
-    /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
-    for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
-      supervision.commands[i] = SUPERVISION_MIN_MOTOR +
-        (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
-         roll_coef[i]   * in_cmd[COMMAND_ROLL]   +
-         pitch_coef[i]  * in_cmd[COMMAND_PITCH]  +
-         yaw_coef[i]    * in_cmd[COMMAND_YAW]    +
-         supervision.trim[i]) / SUPERVISION_SCALE *
-        (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ;
-      if (supervision.commands[i] < min_cmd)
-        min_cmd = supervision.commands[i];
-      if (supervision.commands[i] > max_cmd)
-        max_cmd = supervision.commands[i];
-    }
-    if (min_cmd < SUPERVISION_MIN_MOTOR && max_cmd > SUPERVISION_MAX_MOTOR)
-      supervision.nb_failure++;
-    if (min_cmd < SUPERVISION_MIN_MOTOR)
-      offset_commands(-(min_cmd - SUPERVISION_MIN_MOTOR));
-    if (max_cmd > SUPERVISION_MAX_MOTOR)
-      offset_commands(-(max_cmd - SUPERVISION_MAX_MOTOR));
-
-    /* For testing motor failure */
-    if (motors_on && override_on) {
-      for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
-        if (supervision.override_enabled[i])
-          supervision.commands[i] = supervision.override_value[i];
-      }
-    }
-    bound_commands();
-    bound_commands_step();
-  }
-  else {
-    for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
-      supervision.commands[i] = SUPERVISION_STOP_MOTOR;
-    }
-  }
-}

From ee6b776090d7c051a7cda2f60b1219e5a7b210ec Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 18 Oct 2012 19:03:04 +0200
Subject: [PATCH 14/26] [actuators] update examples airframes

---
 conf/airframes/examples/booz2.xml             | 31 ++++++++++------
 conf/airframes/examples/h_hex.xml             | 37 ++++++++++++-------
 conf/airframes/examples/lisa_asctec.xml       | 33 +++++++++++------
 .../quadrotor_lisa_m_2_pwm_spektrum.xml       | 37 ++++++++++---------
 ...uadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml | 32 +++++++++-------
 .../examples/quadrotor_lisa_m_mkk.xml         | 26 ++++++++++---
 6 files changed, 123 insertions(+), 73 deletions(-)

diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml
index 553fc34a9d..0786585482 100644
--- a/conf/airframes/examples/booz2.xml
+++ b/conf/airframes/examples/booz2.xml
@@ -1,10 +1,12 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
 <airframe name="BOOZ2_a1">
 
-  <servos>
-    <servo name="FRONT" no="0" min="0" neutral="0" max="255"/>
-    <servo name="BACK"  no="1" min="0" neutral="0" max="255"/>
-    <servo name="RIGHT" no="2" min="0" neutral="0" max="255"/>
-    <servo name="LEFT"  no="3" min="0" neutral="0" max="255"/>
+  <servos driver="Mkk">
+    <servo name="FRONT" no="0" min="0" neutral="2" max="210"/>
+    <servo name="BACK"  no="1" min="0" neutral="2" max="210"/>
+    <servo name="RIGHT" no="2" min="0" neutral="2" max="210"/>
+    <servo name="LEFT"  no="3" min="0" neutral="2" max="210"/>
   </servos>
 
   <commands>
@@ -19,12 +21,10 @@
     <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
   </section>
 
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="MIN_MOTOR" value="2"/>
-    <define name="MAX_MOTOR" value="210"/>
-    <define name="TRIM_A" value="0"/>
-    <define name="TRIM_E" value="0"/>
-    <define name="TRIM_R" value="0"/>
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"  value="{    0,    0, -256,  256}"/>
@@ -33,6 +33,14 @@
     <define name="THRUST_COEF" value="{ 256,  256,  256,  256}"/>
   </section>
 
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+    <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+    <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+    <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
+  </command_laws>
+
   <section name="IMU" prefix="IMU_">
 
     <define name="GYRO_P_NEUTRAL" value="33924"/>
@@ -199,6 +207,7 @@
     </target>
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="telemetry"     type="transparent"/>
+    <subsystem name="motor_mixing"/>
     <subsystem name="actuators"     type="mkk"/>
     <subsystem name="imu"           type="b2_v1.0"/>
     <subsystem name="gps"           type="ublox"/>
diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml
index 04fcaf4edd..13a8ded1e1 100644
--- a/conf/airframes/examples/h_hex.xml
+++ b/conf/airframes/examples/h_hex.xml
@@ -1,13 +1,15 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
 <!-- this is a custom hexa Lisa/L#3 and mikrokopter controllers -->
 <airframe name="booz2_a6">
 
-  <servos>
-    <servo name="BACK_RIGHT"   no="0" min="0" neutral="0" max="255"/>
-    <servo name="BACK_LEFT"    no="1" min="0" neutral="0" max="255"/>
-    <servo name="CENTER_RIGHT" no="2" min="0" neutral="0" max="255"/>
-    <servo name="CENTER_LEFT"  no="3" min="0" neutral="0" max="255"/>
-    <servo name="FRONT_RIGHT"  no="4" min="0" neutral="0" max="255"/>
-    <servo name="FRONT_LEFT"   no="5" min="0" neutral="0" max="255"/>
+  <servos driver="Mkk">
+    <servo name="BACK_RIGHT"   no="0" min="0" neutral="2" max="210"/>
+    <servo name="BACK_LEFT"    no="1" min="0" neutral="2" max="210"/>
+    <servo name="CENTER_RIGHT" no="2" min="0" neutral="2" max="210"/>
+    <servo name="CENTER_LEFT"  no="3" min="0" neutral="2" max="210"/>
+    <servo name="FRONT_RIGHT"  no="4" min="0" neutral="2" max="210"/>
+    <servo name="FRONT_LEFT"   no="5" min="0" neutral="2" max="210"/>
   </servos>
 
   <commands>
@@ -22,12 +24,10 @@
     <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58, 0x5A, 0X5C }"/>
   </section>
 
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="MIN_MOTOR" value="2"/>
-    <define name="MAX_MOTOR" value="210"/>
-    <define name="TRIM_A" value="0"/>
-    <define name="TRIM_E" value="0"/>
-    <define name="TRIM_R" value="0"/>
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
     <define name="NB_MOTOR" value="6"/>
     <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"   value="{  -69,   69, -256,  256, -186,  186 }"/>
@@ -36,6 +36,16 @@
     <define name="THRUST_COEF" value="{  256,  256,  256,  256,  256,  256 }"/>
   </section>
 
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="BACK_RIGHT"   value="motor_mixing.commands[SERVO_BACK_RIGHT]"/>
+    <set servo="BACK_LEFT"    value="motor_mixing.commands[SERVO_BACK_LEFT]"/>
+    <set servo="CENTER_RIGHT" value="motor_mixing.commands[SERVO_CENTER_RIGHT]"/>
+    <set servo="CENTER_LEFT"  value="motor_mixing.commands[SERVO_CENTER_LEFT]"/>
+    <set servo="FRONT_RIGHT"  value="motor_mixing.commands[SERVO_FRONT_RIGHT]"/>
+    <set servo="FRONT_LEFT"   value="motor_mixing.commands[SERVO_FRONT_LEFT]"/>
+  </command_laws>
+
   <section name="IMU" prefix="IMU_">
 
     <define name="GYRO_P_NEUTRAL" value="31948"/>
@@ -172,6 +182,7 @@
     <target name="ap" board="lisa_l_1.0">
 <!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="motor_mixing"/>
       <subsystem name="actuators"     type="mkk"/>
     </target>
     <subsystem name="telemetry"     type="transparent"/>
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml
index fce3a2180d..19d65a5dcd 100644
--- a/conf/airframes/examples/lisa_asctec.xml
+++ b/conf/airframes/examples/lisa_asctec.xml
@@ -1,12 +1,14 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
 <!-- this is an asctec frame equiped with  Lisa/L#3 and asctec V2 controllers -->
 
 <airframe name="lisa_asctec">
 
-  <servos>
-    <servo name="FRONT"   no="0" min="0" neutral="0" max="255"/>
-    <servo name="BACK"    no="1" min="0" neutral="0" max="255"/>
-    <servo name="LEFT"    no="2" min="0" neutral="0" max="255"/>
-    <servo name="RIGHT"   no="3" min="0" neutral="0" max="255"/>
+  <servos driver="Asctec">
+    <servo name="FRONT"   no="0" min="0" neutral="3" max="200"/>
+    <servo name="BACK"    no="1" min="0" neutral="3" max="200"/>
+    <servo name="LEFT"    no="2" min="0" neutral="3" max="200"/>
+    <servo name="RIGHT"   no="3" min="0" neutral="3" max="200"/>
   </servos>
 
   <commands>
@@ -22,13 +24,10 @@
     <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
   </section>
 
-
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="MIN_MOTOR" value="3"/>
-    <define name="MAX_MOTOR" value="200"/>
-    <define name="TRIM_A" value="0"/>
-    <define name="TRIM_E" value="0"/>
-    <define name="TRIM_R" value="0"/>
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"   value="{  0  ,    0,  256, -256 }"/>
@@ -37,6 +36,14 @@
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+    <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+    <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+    <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
+  </command_laws>
+
   <section name="IMU" prefix="IMU_">
     <define name="GYRO_P_NEUTRAL" value="32276"/>
     <define name="GYRO_Q_NEUTRAL" value="33987"/>
@@ -183,6 +190,7 @@
     <target name="ap" board="lisa_l_1.1">
 <!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="motor_mixing"/>
       <subsystem name="actuators"     type="asctec_v2"/>
       <subsystem name="telemetry"     type="transparent"/>
       <define name="RADIO_MODE" value="RADIO_AUX2"/>
@@ -192,6 +200,7 @@
     <target name="sim" board="pc">
       <subsystem name="fdm"           type="jsbsim"/>
       <subsystem name="radio_control" type="ppm"/>
+      <subsystem name="motor_mixing"/>
       <subsystem name="actuators"     type="mkk"/>
     </target>
 
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
index 64fa05cb6e..d397aa9a57 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
@@ -25,7 +25,8 @@
       <subsystem name="radio_control" type="ppm"/>
     </target>
 
-    <subsystem name="actuators"     type="pwm_supervision">
+    <subsystem name="motor_mixing"/>
+    <subsystem name="actuators"     type="direct">
       <define name="SERVO_HZ" value="400"/>
       <define name="USE_SERVOS_7AND8"/>
     </subsystem>
@@ -38,11 +39,11 @@
     <subsystem name="ins"/>
   </firmware>
 
-  <servos>
-    <servo name="FRONT"   no="0" min="1000" neutral="1000" max="2000"/>
-    <servo name="BACK"    no="1" min="1000" neutral="1000" max="2000"/>
-    <servo name="RIGHT"   no="2" min="1000" neutral="1000" max="2000"/>
-    <servo name="LEFT"    no="3" min="1000" neutral="1000" max="2000"/>
+  <servos driver="Pwm">
+    <servo name="FRONT"   no="0" min="1000" neutral="1100" max="1900"/>
+    <servo name="BACK"    no="1" min="1000" neutral="1100" max="1900"/>
+    <servo name="RIGHT"   no="2" min="1000" neutral="1100" max="1900"/>
+    <servo name="LEFT"    no="3" min="1000" neutral="1100" max="1900"/>
   </servos>
 
   <commands>
@@ -52,18 +53,10 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
-  <command_laws>
-    <!-- command_laws is needed for pwm_supervision -->
-    <!-- but can be empty if no additional servos are used -->
-  </command_laws>
-
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="STOP_MOTOR" value="1000"/>
-    <define name="MIN_MOTOR"  value="1100"/>
-    <define name="MAX_MOTOR"  value="1900"/>
-    <define name="TRIM_A" value="0"/>
-    <define name="TRIM_E" value="0"/>
-    <define name="TRIM_R" value="0"/>
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
     <!-- front/back turning CW, right/left CCW -->
@@ -73,6 +66,14 @@
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+    <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+    <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+    <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
+  </command_laws>
+
   <section name="IMU" prefix="IMU_">
     <define name="ACCEL_X_NEUTRAL" value="11"/>
     <define name="ACCEL_Y_NEUTRAL" value="11"/>
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
index c89a281729..af2c0368b9 100755
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
@@ -25,7 +25,8 @@
       <subsystem name="radio_control" type="ppm"/>
     </target>
 
-    <subsystem name="actuators"     type="pwm_supervision">
+    <subsystem name="motor_mixing"/>
+    <subsystem name="actuators"     type="direct">
       <define name="SERVO_HZ" value="400"/>
       <define name="USE_SERVOS_7AND8"/>
     </subsystem>
@@ -43,11 +44,11 @@
     <load name="geo_mag.xml"/>
   </modules>
 
-  <servos>
-    <servo name="FRONT"   no="0" min="1000" neutral="1000" max="2000"/>
-    <servo name="BACK"    no="1" min="1000" neutral="1000" max="2000"/>
-    <servo name="RIGHT"   no="2" min="1000" neutral="1000" max="2000"/>
-    <servo name="LEFT"    no="3" min="1000" neutral="1000" max="2000"/>
+  <servos driver="Pwm">
+    <servo name="FRONT"   no="0" min="1000" neutral="1100" max="1900"/>
+    <servo name="BACK"    no="1" min="1000" neutral="1100" max="1900"/>
+    <servo name="RIGHT"   no="2" min="1000" neutral="1100" max="1900"/>
+    <servo name="LEFT"    no="3" min="1000" neutral="1100" max="1900"/>
   </servos>
 
   <commands>
@@ -62,13 +63,10 @@
     <!-- but can be empty if no additional servos are used -->
   </command_laws>
 
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="STOP_MOTOR" value="1000"/>
-    <define name="MIN_MOTOR"  value="1100"/>
-    <define name="MAX_MOTOR"  value="1900"/>
-    <define name="TRIM_A" value="0"/>
-    <define name="TRIM_E" value="0"/>
-    <define name="TRIM_R" value="0"/>
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
     <!-- front/back turning CW, right/left CCW -->
@@ -78,6 +76,14 @@
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+    <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+    <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+    <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
+  </command_laws>
+
   <section name="IMU" prefix="IMU_">
     <define name="ACCEL_X_NEUTRAL" value="11"/>
     <define name="ACCEL_Y_NEUTRAL" value="11"/>
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
index 9a01431aa4..a178aa5bdf 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
@@ -20,6 +20,7 @@
 
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="telemetry"     type="transparent"/>
+    <subsystem name="motor_mixing"/>
     <subsystem name="actuators"     type="mkk"/>
     <subsystem name="imu"           type="aspirin_v1.5"/>
     <subsystem name="gps"           type="ublox"/>
@@ -28,6 +29,13 @@
     <subsystem name="ins"/>
   </firmware>
 
+  <servos driver="Mkk">
+    <servo name="FRONT"  no="0"    min="0" neutral="2" max="200"/>
+    <servo name="BACK"   no="1"    min="0" neutral="2" max="200"/>
+    <servo name="RIGHT"  no="2"    min="0" neutral="2" max="200"/>
+    <servo name="LEFT"   no="3"    min="0" neutral="2" max="200"/>
+  </servos>
+
   <commands>
     <axis name="ROLL"   failsafe_value="0"/>
     <axis name="PITCH"  failsafe_value="0"/>
@@ -41,12 +49,10 @@
     <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
   </section>
 
-  <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="MIN_MOTOR" value="2"/>
-    <define name="MAX_MOTOR" value="200"/>
-    <define name="TRIM_A" value="0"/>
-    <define name="TRIM_E" value="0"/>
-    <define name="TRIM_R" value="0"/>
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
     <!-- front/back turning CW, left/right CCW -->
@@ -56,6 +62,14 @@
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+    <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+    <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+    <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
+  </command_laws>
+
   <section name="IMU" prefix="IMU_">
     <define name="ACCEL_X_NEUTRAL" value="11"/>
     <define name="ACCEL_Y_NEUTRAL" value="11"/>

From 4c3fb47be486cbe44ba0aef7a27a87dddce7843d Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 13:28:35 +0200
Subject: [PATCH 15/26] [actuators] default actuators for lisa and classix

- actuators_direct is now actuators_pwm (consistency with file names)
---
 conf/airframes/ENAC/quadrotor/blender.xml     |  2 +-
 conf/airframes/ENAC/quadrotor/booz2_g1.xml    |  2 +-
 .../LisaL_v1.1_aspirin_v1.5_rc.xml            | 21 +++++++++----------
 .../TestHardware/LisaL_v1.1_b2_v1.2_rc.xml    | 21 +++++++++----------
 .../quadrotor_lisa_m_2_pwm_spektrum.xml       |  2 +-
 ...uadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml |  2 +-
 conf/boards/classix.makefile                  |  8 +++++++
 conf/boards/lisa_l_1.0.makefile               | 10 +++++++++
 conf/boards/lisa_l_1.1.makefile               | 10 +++++++++
 conf/boards/lisa_m_1.0.makefile               | 10 +++++++++
 conf/boards/lisa_m_2.0.makefile               | 10 +++++++++
 conf/firmwares/fixedwing.makefile             | 11 +---------
 conf/firmwares/setup.makefile                 |  6 +++---
 ...direct.makefile => actuators_pwm.makefile} |  0
 sw/airborne/boards/classix_1.0.h              |  8 ++++++-
 sw/airborne/boards/lisa_l_1.0.h               |  6 ++++++
 sw/airborne/boards/lisa_m_1.0.h               |  6 ++++++
 sw/airborne/boards/lisa_m_2.0.h               |  6 ++++++
 18 files changed, 101 insertions(+), 40 deletions(-)
 rename conf/firmwares/subsystems/shared/{actuators_direct.makefile => actuators_pwm.makefile} (100%)

diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index aab8d9aeae..55cbf4c8a5 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -30,7 +30,7 @@
       <define name="SKIRON_I2C_SCL_TIME" value="25"/>
     </subsystem>
     <subsystem name="motor_mixing"/>
-    <subsystem name="actuators"     type="direct">
+    <subsystem name="actuators"     type="pwm">
       <define name="USE_PWM0"/>
     </subsystem>
     <subsystem name="imu"           type="navgo"/>
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 03413695ea..90a9907561 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -24,7 +24,7 @@
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="telemetry"     type="transparent"/>
     <subsystem name="actuators"     type="asctec"/>
-    <subsystem name="actuators"     type="direct">
+    <subsystem name="actuators"     type="pwm">
       <define name="USE_PWM0"/>
       <define name="USE_PWM1"/>
     </subsystem>
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
index 3aed8ead28..c3196161cb 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
@@ -24,6 +24,7 @@
         <subsystem name="telemetry"     type="xbee_api">
             <configure name="MODEM_BAUD" value="B38400"/>
         </subsystem>
+        <subsystem name="motor_mixing"/>
         <subsystem name="actuators"     type="mkk"/>
         <subsystem name="imu"           type="aspirin_v1.5"/>
         <subsystem name="gps"           type="ublox">
@@ -70,11 +71,11 @@
      </firmware>
      -->
 
-    <servos>
-        <servo name="FRONT"   no="0" min="0" neutral="0" max="255"/>
-        <servo name="BACK"    no="1" min="0" neutral="0" max="255"/>
-        <servo name="RIGHT"   no="2" min="0" neutral="0" max="255"/>
-        <servo name="LEFT"    no="3" min="0" neutral="0" max="255"/>
+    <servos driver="Mkk">
+        <servo name="FRONT"   no="0" min="0" neutral="2" max="210"/>
+        <servo name="BACK"    no="1" min="0" neutral="2" max="210"/>
+        <servo name="RIGHT"   no="2" min="0" neutral="2" max="210"/>
+        <servo name="LEFT"    no="3" min="0" neutral="2" max="210"/>
     </servos>
 
     <commands>
@@ -89,12 +90,10 @@
         <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
     </section>
 
-    <section name="SUPERVISION" prefix="SUPERVISION_">
-        <define name="MIN_MOTOR" value="2"/>
-        <define name="MAX_MOTOR" value="210"/>
-        <define name="TRIM_A" value="0"/>
-        <define name="TRIM_E" value="0"/>
-        <define name="TRIM_R" value="0"/>
+    <section name="MIXING" prefix="MOTOR_MIXING_">
+        <define name="TRIM_ROLL" value="0"/>
+        <define name="TRIM_PITCH" value="0"/>
+        <define name="TRIM_YAW" value="0"/>
         <define name="NB_MOTOR" value="4"/>
         <define name="SCALE" value="256"/>
         <define name="ROLL_COEF"   value="{    0,    0, -256,  256 }"/>
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
index 38bee2483a..912c5b2563 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
@@ -23,6 +23,7 @@
             <subsystem name="radio_control" type="ppm"/>
         </target>
 
+        <subsystem name="motor_mixing"/>
         <subsystem name="actuators"     type="mkk"/>
         <subsystem name="telemetry"     type="transparent"/>
         <subsystem name="imu"           type="b2_v1.2"/>
@@ -70,11 +71,11 @@
     </firmware>
 -->
 
-    <servos>
-        <servo name="FRONT"   no="0" min="0" neutral="0" max="255"/>
-        <servo name="BACK"    no="1" min="0" neutral="0" max="255"/>
-        <servo name="RIGHT"   no="2" min="0" neutral="0" max="255"/>
-        <servo name="LEFT"    no="3" min="0" neutral="0" max="255"/>
+    <servos driver="Mkk">
+        <servo name="FRONT"   no="0" min="0" neutral="2" max="210"/>
+        <servo name="BACK"    no="1" min="0" neutral="2" max="210"/>
+        <servo name="RIGHT"   no="2" min="0" neutral="2" max="210"/>
+        <servo name="LEFT"    no="3" min="0" neutral="2" max="210"/>
     </servos>
 
     <commands>
@@ -91,12 +92,10 @@
     </section>
 
 
-    <section name="SUPERVISION" prefix="SUPERVISION_">
-        <define name="MIN_MOTOR" value="2"/>
-        <define name="MAX_MOTOR" value="210"/>
-        <define name="TRIM_A" value="0"/>
-        <define name="TRIM_E" value="0"/>
-        <define name="TRIM_R" value="0"/>
+    <section name="MIXING" prefix="MOTOR_MIXING_">
+        <define name="TRIM_ROLL" value="0"/>
+        <define name="TRIM_PITCH" value="0"/>
+        <define name="TRIM_YAW" value="0"/>
         <define name="NB_MOTOR" value="4"/>
         <define name="SCALE" value="256"/>
         <define name="ROLL_COEF"   value="{    0,    0, -256,  256 }"/>
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
index d397aa9a57..4942450dab 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
@@ -26,7 +26,7 @@
     </target>
 
     <subsystem name="motor_mixing"/>
-    <subsystem name="actuators"     type="direct">
+    <subsystem name="actuators"     type="pwm">
       <define name="SERVO_HZ" value="400"/>
       <define name="USE_SERVOS_7AND8"/>
     </subsystem>
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
index af2c0368b9..dcc9a9c309 100755
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
@@ -26,7 +26,7 @@
     </target>
 
     <subsystem name="motor_mixing"/>
-    <subsystem name="actuators"     type="direct">
+    <subsystem name="actuators"     type="pwm">
       <define name="SERVO_HZ" value="400"/>
       <define name="USE_SERVOS_7AND8"/>
     </subsystem>
diff --git a/conf/boards/classix.makefile b/conf/boards/classix.makefile
index efdfbdc3bc..c4dd62e1dc 100644
--- a/conf/boards/classix.makefile
+++ b/conf/boards/classix.makefile
@@ -55,5 +55,13 @@ ifndef SYS_TIME_LED
 SYS_TIME_LED = none
 endif
 
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
+#
+ifndef ACTUATORS
+ACTUATORS = actuators_pwm
+endif
+
 
 
diff --git a/conf/boards/lisa_l_1.0.makefile b/conf/boards/lisa_l_1.0.makefile
index 1194c8684f..dcfbac5f47 100644
--- a/conf/boards/lisa_l_1.0.makefile
+++ b/conf/boards/lisa_l_1.0.makefile
@@ -86,6 +86,16 @@ ifndef GPS_BAUD
 GPS_BAUD=B38400
 endif
 
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
+#
+ifndef ACTUATORS
+ACTUATORS = actuators_pwm
+endif
+
 #
 # this is the DRDY pin of a max1168 on a booz IMU
 #
diff --git a/conf/boards/lisa_l_1.1.makefile b/conf/boards/lisa_l_1.1.makefile
index 9a6521e594..1553611865 100644
--- a/conf/boards/lisa_l_1.1.makefile
+++ b/conf/boards/lisa_l_1.1.makefile
@@ -89,6 +89,16 @@ ifndef GPS_BAUD
 GPS_BAUD=B38400
 endif
 
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
+#
+ifndef ACTUATORS
+ACTUATORS = actuators_pwm
+endif
+
 #
 # this is the DRDY pin of a max1168 on a booz IMU
 #
diff --git a/conf/boards/lisa_m_1.0.makefile b/conf/boards/lisa_m_1.0.makefile
index cc4d6f0c2b..120542da52 100644
--- a/conf/boards/lisa_m_1.0.makefile
+++ b/conf/boards/lisa_m_1.0.makefile
@@ -83,6 +83,16 @@ GPS_BAUD=B38400
 endif
 
 
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
+#
+ifndef ACTUATORS
+ACTUATORS = actuators_pwm
+endif
+
 
 ifndef ADC_IR1
 ADC_IR1      = 1
diff --git a/conf/boards/lisa_m_2.0.makefile b/conf/boards/lisa_m_2.0.makefile
index 5af995b5b4..a97cb1cb5a 100644
--- a/conf/boards/lisa_m_2.0.makefile
+++ b/conf/boards/lisa_m_2.0.makefile
@@ -86,6 +86,16 @@ GPS_BAUD=B38400
 endif
 
 
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
+#
+ifndef ACTUATORS
+ACTUATORS = actuators_pwm
+endif
+
 
 ifndef ADC_IR1
 ADC_IR1      = 1
diff --git a/conf/firmwares/fixedwing.makefile b/conf/firmwares/fixedwing.makefile
index 337a2def80..aaa3ba7644 100644
--- a/conf/firmwares/fixedwing.makefile
+++ b/conf/firmwares/fixedwing.makefile
@@ -34,16 +34,7 @@ endif
 
 ifeq ($(TARGET),$(ACTUATOR_TARGET))
 
-  ifeq ($(ACTUATORS),)
-
-    ifeq ($(BOARD),lisa_l)
-      include $(CFG_SHARED)/actuators_direct.makefile
-    endif
-    ifeq ($(BOARD),lisa_m)
-      include $(CFG_SHARED)/actuators_direct.makefile
-    endif
-
-  else
+  ifneq ($(ACTUATORS),)
     include $(CFG_SHARED)/$(ACTUATORS).makefile
   endif
 
diff --git a/conf/firmwares/setup.makefile b/conf/firmwares/setup.makefile
index cb68bd9572..12c0d77a18 100644
--- a/conf/firmwares/setup.makefile
+++ b/conf/firmwares/setup.makefile
@@ -84,10 +84,10 @@ ifeq ($(TARGET), setup_actuators)
     endif
 
     ifeq ($(BOARD),lisa_l)
-      include $(CFG_SHARED)/actuators_direct.makefile
+      include $(CFG_SHARED)/actuators_pwm.makefile
     endif
-	ifeq ($(BOARD),lisa_m)
-      include $(CFG_SHARED)/actuators_direct.makefile
+		ifeq ($(BOARD),lisa_m)
+      include $(CFG_SHARED)/actuators_pwm.makefile
     endif
 
   else
diff --git a/conf/firmwares/subsystems/shared/actuators_direct.makefile b/conf/firmwares/subsystems/shared/actuators_pwm.makefile
similarity index 100%
rename from conf/firmwares/subsystems/shared/actuators_direct.makefile
rename to conf/firmwares/subsystems/shared/actuators_pwm.makefile
diff --git a/sw/airborne/boards/classix_1.0.h b/sw/airborne/boards/classix_1.0.h
index fd21440500..4d96d3d355 100644
--- a/sw/airborne/boards/classix_1.0.h
+++ b/sw/airborne/boards/classix_1.0.h
@@ -46,7 +46,13 @@
 #define PPM_CCR_CRI TCCR_CR2_I
 #define PPM_CR T0CR2
 
-/* SERVOS : 4017 on FBW_RC0 connector */
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+/* SERVOS : 4017 on FBW_RC0 connector (ppm driver) */
 
 /* MAT0.1 on P0.5 */
 #define SERVO_CLOCK_PIN 5
diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h
index 0bfed3a280..d703b0bf28 100644
--- a/sw/airborne/boards/lisa_l_1.0.h
+++ b/sw/airborne/boards/lisa_l_1.0.h
@@ -21,6 +21,12 @@
 #define IMU_ACC_DRDY_GPIO             GPIOD
 #define IMU_ACC_DRDY_GPIO_PORTSOURCE  GPIO_PortSourceGPIOD
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
 
 /* PA0 - ADC0 */
 /* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h
index add01752b1..22192ce9d3 100644
--- a/sw/airborne/boards/lisa_m_1.0.h
+++ b/sw/airborne/boards/lisa_m_1.0.h
@@ -59,6 +59,12 @@
 #define IMU_ACC_DRDY_GPIO_PORTSOURCE  GPIO_PortSourceGPIOB
 
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
 
 #define DefaultVoltageOfAdc(adc) (0.00485*adc)
 
diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h
index 2d411960cd..358b3fe089 100644
--- a/sw/airborne/boards/lisa_m_2.0.h
+++ b/sw/airborne/boards/lisa_m_2.0.h
@@ -95,6 +95,12 @@
 #define IMU_ACC_DRDY_GPIO_PORTSOURCE  GPIO_PortSourceGPIOB
 
 
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
 
 #define DefaultVoltageOfAdc(adc) (0.0045*adc)
 

From c3ae77b5f7db8714ff8bb4b29cb5c786e10b3b36 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 14:51:58 +0200
Subject: [PATCH 16/26] [airframe] section MKK has to be before servos section

---
 .../TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml      | 10 +++++-----
 .../airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml | 11 +++++------
 conf/airframes/examples/booz2.xml                    | 10 +++++-----
 conf/airframes/examples/h_hex.xml                    | 10 +++++-----
 conf/airframes/examples/quadrotor_lisa_m_mkk.xml     | 12 ++++++------
 5 files changed, 26 insertions(+), 27 deletions(-)

diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
index c3196161cb..6fbca0e2f7 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
@@ -71,6 +71,11 @@
      </firmware>
      -->
 
+    <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+        <define name="NB" value="4"/>
+        <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+    </section>
+
     <servos driver="Mkk">
         <servo name="FRONT"   no="0" min="0" neutral="2" max="210"/>
         <servo name="BACK"    no="1" min="0" neutral="2" max="210"/>
@@ -85,11 +90,6 @@
         <axis name="THRUST" failsafe_value="0"/>
     </commands>
 
-    <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
-        <define name="NB" value="4"/>
-        <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
-    </section>
-
     <section name="MIXING" prefix="MOTOR_MIXING_">
         <define name="TRIM_ROLL" value="0"/>
         <define name="TRIM_PITCH" value="0"/>
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
index 912c5b2563..7752cd645a 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
@@ -71,6 +71,11 @@
     </firmware>
 -->
 
+    <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+        <define name="NB" value="4"/>
+        <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+    </section>
+
     <servos driver="Mkk">
         <servo name="FRONT"   no="0" min="0" neutral="2" max="210"/>
         <servo name="BACK"    no="1" min="0" neutral="2" max="210"/>
@@ -85,12 +90,6 @@
         <axis name="THRUST" failsafe_value="0"/>
     </commands>
 
-    <!-- for the sim -->
-    <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
-        <define name="NB" value="4"/>
-        <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
-    </section>
-
 
     <section name="MIXING" prefix="MOTOR_MIXING_">
         <define name="TRIM_ROLL" value="0"/>
diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml
index 0786585482..f9986df8b8 100644
--- a/conf/airframes/examples/booz2.xml
+++ b/conf/airframes/examples/booz2.xml
@@ -2,6 +2,11 @@
 
 <airframe name="BOOZ2_a1">
 
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="4"/>
+    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+  </section>
+
   <servos driver="Mkk">
     <servo name="FRONT" no="0" min="0" neutral="2" max="210"/>
     <servo name="BACK"  no="1" min="0" neutral="2" max="210"/>
@@ -16,11 +21,6 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
-  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
-    <define name="NB" value="4"/>
-    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
-  </section>
-
   <section name="MIXING" prefix="MOTOR_MIXING_">
     <define name="TRIM_ROLL" value="0"/>
     <define name="TRIM_PITCH" value="0"/>
diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml
index 13a8ded1e1..a064eca868 100644
--- a/conf/airframes/examples/h_hex.xml
+++ b/conf/airframes/examples/h_hex.xml
@@ -3,6 +3,11 @@
 <!-- this is a custom hexa Lisa/L#3 and mikrokopter controllers -->
 <airframe name="booz2_a6">
 
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="6"/>
+    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58, 0x5A, 0X5C }"/>
+  </section>
+
   <servos driver="Mkk">
     <servo name="BACK_RIGHT"   no="0" min="0" neutral="2" max="210"/>
     <servo name="BACK_LEFT"    no="1" min="0" neutral="2" max="210"/>
@@ -19,11 +24,6 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
-  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
-    <define name="NB" value="6"/>
-    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58, 0x5A, 0X5C }"/>
-  </section>
-
   <section name="MIXING" prefix="MOTOR_MIXING_">
     <define name="TRIM_ROLL" value="0"/>
     <define name="TRIM_PITCH" value="0"/>
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
index a178aa5bdf..42fcb6b0d0 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
@@ -29,6 +29,12 @@
     <subsystem name="ins"/>
   </firmware>
 
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="4"/>
+    <!-- FRONT, BACK, RIGHT, LEFT -->
+    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+  </section>
+
   <servos driver="Mkk">
     <servo name="FRONT"  no="0"    min="0" neutral="2" max="200"/>
     <servo name="BACK"   no="1"    min="0" neutral="2" max="200"/>
@@ -43,12 +49,6 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
-  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
-    <define name="NB" value="4"/>
-    <!-- FRONT, BACK, RIGHT, LEFT -->
-    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
-  </section>
-
   <section name="MIXING" prefix="MOTOR_MIXING_">
     <define name="TRIM_ROLL" value="0"/>
     <define name="TRIM_PITCH" value="0"/>

From 6c5974af6fd681771f697e83551ae27569af0bc0 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 14:54:10 +0200
Subject: [PATCH 17/26] [actuators][sim] sim drivers

---
 .../subsystems/actuators/actuators_pwm_arch.h | 21 ++++++++++++
 .../subsystems/actuators/servos_4015_MAT_hw.h |  2 --
 .../subsystems/actuators/servos_4017_hw.h     | 22 ++++++++++++
 .../subsystems/actuators/actuators_pwm_arch.c |  5 +--
 .../subsystems/actuators/actuators_pwm_arch.h | 13 +++----
 .../subsystems/actuators/servos_4015_MAT_hw.h | 33 ++++++++++++++++++
 .../sim/subsystems/actuators/servos_4017_hw.h | 34 +++++++++++++++++++
 .../sim/subsystems/actuators/servos_ppm_hw.h  | 34 +++++++++++++++++++
 8 files changed, 149 insertions(+), 15 deletions(-)
 create mode 100644 sw/airborne/arch/sim/subsystems/actuators/servos_4015_MAT_hw.h
 create mode 100644 sw/airborne/arch/sim/subsystems/actuators/servos_4017_hw.h
 create mode 100644 sw/airborne/arch/sim/subsystems/actuators/servos_ppm_hw.h

diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
index 51ef2ac11e..071b4c288b 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
@@ -1,3 +1,24 @@
+/*
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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.
+ */
+
 #ifndef ACTUATORS_PWM_ARCH_H
 #define ACTUATORS_PWM_ARCH_H
 
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
index 7d52cc22bb..9e69bb07d5 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4015_MAT_hw.h
@@ -1,6 +1,4 @@
 /*
- * $Id$
- *
  * Copyright (C) 2006  Antoine Drouin
  *
  * This file is part of paparazzi.
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
index 8d9e2e68f3..e131ad18e5 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
@@ -1,3 +1,25 @@
+/*
+ * Copyright (C) 2012 ENAC
+ *
+ * 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.
+ *
+ */
+
 #ifndef SERVOS_4017_HW_H
 #define SERVOS_4017_HW_H
 
diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c
index cb0f4faf31..ad6b754f91 100644
--- a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c
@@ -23,12 +23,9 @@
  *  dummy servos handling for sim
  */
 
-#include "subsystems/actuators/actuators_pwm.h"
+#include "subsystems/actuators/actuators_pwm_arch.h"
 
 void actuators_pwm_arch_init(void) {
 
 }
 
-void actuators_pwm_commit(void) {
-
-}
diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h
index 1c2f9ea167..45566777af 100644
--- a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h
@@ -26,17 +26,12 @@
 #ifndef ACTUATORS_PWM_ARCH_H
 #define ACTUATORS_PWM_ARCH_H
 
-#define ACTUATORS_PWM_NB 8
+#define SERVOS_TICS_OF_USEC(_v) (_v)
 
-/* Needs to be included after ACTUATORS_PWM_NB is defined!
- * Otherwise ACTUATORS_PWM_NB is not declared in actuators_pwm.h
- */
-#include "subsystems/actuators/actuators_pwm.h"
+#define ActuatorPwmSet(_i, _v) {}
+#define ActuatorsPwmInit() {}
+#define ActuatorsPwmCommit() {}
 
 extern void actuators_pwm_arch_init(void);
-extern void actuators_pwm_commit(void);
-
-#define SERVOS_TICS_OF_USEC(_v) (_v)
-#define ActuatorsCommit() {}
 
 #endif /* ACTUATORS_PWM_ARCH_H */
diff --git a/sw/airborne/arch/sim/subsystems/actuators/servos_4015_MAT_hw.h b/sw/airborne/arch/sim/subsystems/actuators/servos_4015_MAT_hw.h
new file mode 100644
index 0000000000..f3e00e1822
--- /dev/null
+++ b/sw/airborne/arch/sim/subsystems/actuators/servos_4015_MAT_hw.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+#ifndef SERVOS_4015_HW_H
+#define SERVOS_4015_HW_H
+
+#define SERVOS_TICS_OF_USEC(_v) (_v)
+
+#define Actuator4015Set(_i, _v) {}
+#define Actuators4015Init() {}
+#define Actuators4015Commit() {}
+
+#endif
+
diff --git a/sw/airborne/arch/sim/subsystems/actuators/servos_4017_hw.h b/sw/airborne/arch/sim/subsystems/actuators/servos_4017_hw.h
new file mode 100644
index 0000000000..fad3ed1c7f
--- /dev/null
+++ b/sw/airborne/arch/sim/subsystems/actuators/servos_4017_hw.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+#ifndef SERVOS_4017_HW_H
+#define SERVOS_4017_HW_H
+
+#define SERVOS_TICS_OF_USEC(_v) (_v)
+
+#define Actuator4017Set(_i, _v) {}
+#define Actuators4017Init() {}
+#define Actuators4017Commit() {}
+
+#endif
+
+
diff --git a/sw/airborne/arch/sim/subsystems/actuators/servos_ppm_hw.h b/sw/airborne/arch/sim/subsystems/actuators/servos_ppm_hw.h
new file mode 100644
index 0000000000..63f8d5eeda
--- /dev/null
+++ b/sw/airborne/arch/sim/subsystems/actuators/servos_ppm_hw.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+#ifndef SERVOS_PPM_HW_H
+#define SERVOS_PPM_HW_H
+
+#define SERVOS_TICS_OF_USEC(_v) (_v)
+
+#define ActuatorPpmSet(_i, _v) {}
+#define ActuatorsPpmInit() {}
+#define ActuatorsPpmCommit() {}
+
+#endif
+
+

From 4fb8468eb137002a7c957f71ed0403d7c0d5393e Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 14:54:38 +0200
Subject: [PATCH 18/26] [actuators] use integer in actuators macro

---
 sw/tools/gen_airframe.ml | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index c3da4c988f..17fc569504 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -130,7 +130,9 @@ let parse_servo = fun driver c ->
 
   define (name^"_NEUTRAL") (sof neutral);
   define (name^"_TRAVEL_UP") (sof travel_up);
+  define_integer (name^"_TRAVEL_UP") travel_up 16;
   define (name^"_TRAVEL_DOWN") (sof travel_down);
+  define_integer (name^"_TRAVEL_DOWN") travel_down 16;
 
   let min = Pervasives.min min max
   and max = Pervasives.max min max in
@@ -171,8 +173,9 @@ let parse_command_laws = fun command ->
        and value = a "value" in
        let v = preprocess_value value "values" "COMMAND" in
        printf "  command_value = %s; \\\n" v;
-       printf "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP : SERVO_%s_TRAVEL_DOWN; \\\n" servo servo;
-       printf "  servo_value = SERVO_%s_NEUTRAL + (int32_t)(command_value); \\\n" servo;
+       printf "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM; \\\n" servo servo;
+       printf "  command_value /= command_value>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" servo servo;
+       printf "  servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" servo;
        printf "  Set_%s_Servo(servo_value); \\\n\\\n" servo
    | "let" ->
        let var = a "var"
@@ -181,7 +184,7 @@ let parse_command_laws = fun command ->
        printf "  int16_t _var_%s = %s; \\\n" var v
    | "call" ->
        let f = a "fun" in
-       printf "  %s; \\\n" f
+       printf "  %s; \\\n\\\n" f
    | "ratelimit" ->
        let var = a "var"
        and value = a "value"
@@ -264,7 +267,7 @@ let rec parse_section = fun s ->
 
       printf "#define SetActuatorsFromCommands(values) { \\\n";
       printf "  int32_t servo_value;\\\n";
-      printf "  float command_value;\\\n\\\n";
+      printf "  int32_t command_value;\\\n\\\n";
 
       List.iter parse_command_laws (Xml.children s);
 

From 81c683bf10e6ad80c8761a98cceb6cc6cc25348b Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 14:58:34 +0200
Subject: [PATCH 19/26] [actuators] fix include in stm32 pwm driver

---
 .../arch/stm32/subsystems/actuators/actuators_pwm_arch.c     | 2 +-
 .../arch/stm32/subsystems/actuators/actuators_pwm_arch.h     | 5 -----
 2 files changed, 1 insertion(+), 6 deletions(-)

diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
index 59a5722288..193fb1cb9d 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -23,7 +23,7 @@
  *  STM32 PWM servos handling.
  */
 
-#include "actuators.h"
+#include "subsystems/actuators.h"
 #include "subsystems/actuators/actuators_pwm_arch.h"
 
 #include <libopencm3/stm32/f1/gpio.h>
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
index 82d3164b4c..e5d0d547b7 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
@@ -38,11 +38,6 @@
 #define ACTUATORS_PWM_NB 6
 #endif
 
-///* Needs to be included after ACTUATORS_PWM_NB is defined!
-// * Otherwise ACTUATORS_PWM_NB is not declared in actuators_pwm.h
-// */
-//#include "subsystems/actuators/actuators_pwm.h"
-
 extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
 
 extern void actuators_pwm_arch_init(void);

From 59c4d89ee51c0c689708f943176802fe0fc2e01a Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 15:13:30 +0200
Subject: [PATCH 20/26] [actuators] fix macros for default actuators

---
 sw/airborne/boards/classix_1.0.h     | 2 +-
 sw/airborne/boards/lisa_l_1.0.h      | 2 +-
 sw/airborne/boards/lisa_m_1.0.h      | 2 +-
 sw/airborne/boards/lisa_m_2.0.h      | 2 +-
 sw/airborne/boards/tiny_0.99.h       | 2 +-
 sw/airborne/boards/tiny_1.1.h        | 2 +-
 sw/airborne/boards/tiny_2.0.h        | 2 +-
 sw/airborne/boards/tiny_sim.h        | 2 +-
 sw/airborne/boards/umarim_1.0.h      | 2 +-
 sw/airborne/boards/umarim_lite_2.0.h | 2 +-
 10 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/sw/airborne/boards/classix_1.0.h b/sw/airborne/boards/classix_1.0.h
index 4d96d3d355..ee7d102c04 100644
--- a/sw/airborne/boards/classix_1.0.h
+++ b/sw/airborne/boards/classix_1.0.h
@@ -48,7 +48,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
-#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
 #define ActuatorsDefaultInit() ActuatorsPwmInit()
 #define ActuatorsDefaultCommit() ActuatorsPwmCommit()
 
diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h
index d703b0bf28..b324f421f8 100644
--- a/sw/airborne/boards/lisa_l_1.0.h
+++ b/sw/airborne/boards/lisa_l_1.0.h
@@ -23,7 +23,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
-#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
 #define ActuatorsDefaultInit() ActuatorsPwmInit()
 #define ActuatorsDefaultCommit() ActuatorsPwmCommit()
 
diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h
index 22192ce9d3..b48dc45b0a 100644
--- a/sw/airborne/boards/lisa_m_1.0.h
+++ b/sw/airborne/boards/lisa_m_1.0.h
@@ -61,7 +61,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
-#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
 #define ActuatorsDefaultInit() ActuatorsPwmInit()
 #define ActuatorsDefaultCommit() ActuatorsPwmCommit()
 
diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h
index 358b3fe089..a43720622d 100644
--- a/sw/airborne/boards/lisa_m_2.0.h
+++ b/sw/airborne/boards/lisa_m_2.0.h
@@ -97,7 +97,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
-#define ActuatorDefault(_x) ActuatorPwm(_x)
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
 #define ActuatorsDefaultInit() ActuatorsPwmInit()
 #define ActuatorsDefaultCommit() ActuatorsPwmCommit()
 
diff --git a/sw/airborne/boards/tiny_0.99.h b/sw/airborne/boards/tiny_0.99.h
index f7055626c0..6aed88d605 100644
--- a/sw/airborne/boards/tiny_0.99.h
+++ b/sw/airborne/boards/tiny_0.99.h
@@ -37,7 +37,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4015.h"
-#define ActuatorDefault(_x) Actuator4015(_x)
+#define ActuatorDefaultSet(_x,_y) Actuator4015Set(_x,_y)
 #define ActuatorsDefaultInit() Actuators4015Init()
 #define ActuatorsDefaultCommit() Actuators4015Commit()
 
diff --git a/sw/airborne/boards/tiny_1.1.h b/sw/airborne/boards/tiny_1.1.h
index 21f806732f..a589957443 100644
--- a/sw/airborne/boards/tiny_1.1.h
+++ b/sw/airborne/boards/tiny_1.1.h
@@ -48,7 +48,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4015.h"
-#define ActuatorDefault(_x) Actuator4015(_x)
+#define ActuatorDefaultSet(_x,_y) Actuator4015Set(_x,_y)
 #define ActuatorsDefaultInit() Actuators4015Init()
 #define ActuatorsDefaultCommit() Actuators4015Commit()
 
diff --git a/sw/airborne/boards/tiny_2.0.h b/sw/airborne/boards/tiny_2.0.h
index c48148f6bd..f828e60a83 100644
--- a/sw/airborne/boards/tiny_2.0.h
+++ b/sw/airborne/boards/tiny_2.0.h
@@ -51,7 +51,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
-#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorDefaultSet(_x,_y) Actuator4017Set(_x,_y)
 #define ActuatorsDefaultInit() Actuators4017Init()
 #define ActuatorsDefaultCommit() Actuators4017Commit()
 
diff --git a/sw/airborne/boards/tiny_sim.h b/sw/airborne/boards/tiny_sim.h
index c020bada14..229028bfe3 100644
--- a/sw/airborne/boards/tiny_sim.h
+++ b/sw/airborne/boards/tiny_sim.h
@@ -31,7 +31,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
-#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorDefaultSet(_x,_y) Actuator4017Set(_x,_y)
 #define ActuatorsDefaultInit() Actuators4017Init()
 #define ActuatorsDefaultCommit() Actuators4017Commit()
 
diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h
index 52d2c68688..c69e27da97 100644
--- a/sw/airborne/boards/umarim_1.0.h
+++ b/sw/airborne/boards/umarim_1.0.h
@@ -32,7 +32,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
-#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorDefaultSet(_x,_y) Actuator4017Set(_x,_y)
 #define ActuatorsDefaultInit() Actuators4017Init()
 #define ActuatorsDefaultCommit() Actuators4017Commit()
 
diff --git a/sw/airborne/boards/umarim_lite_2.0.h b/sw/airborne/boards/umarim_lite_2.0.h
index e3d8527cc8..6f023e5548 100644
--- a/sw/airborne/boards/umarim_lite_2.0.h
+++ b/sw/airborne/boards/umarim_lite_2.0.h
@@ -32,7 +32,7 @@
 
 /* Default actuators driver */
 #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h"
-#define ActuatorDefault(_x) Actuator4017(_x)
+#define ActuatorDefaultSet(_x,_y) Actuator4017Set(_x,_y)
 #define ActuatorsDefaultInit() Actuators4017Init()
 #define ActuatorsDefaultCommit() Actuators4017Commit()
 

From 13565a0e0676f1d7ef4e7db454b1a708d16bb6c9 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 17:07:37 +0200
Subject: [PATCH 21/26] [actuators] some bug fix

- actuators test programms are disabled for now, needs more work
---
 .../TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml          | 8 ++++++++
 conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml    | 9 ++++++++-
 conf/airframes/examples/h_hex.xml                        | 4 ++--
 conf/airframes/examples/lisa_asctec.xml                  | 4 ++--
 conf/airframes/examples/setup_lisam2.xml                 | 2 +-
 sw/airborne/firmwares/setup/setup_actuators.c            | 9 ++++-----
 sw/airborne/lisa/lisa_stm_passthrough_main.c             | 4 ++--
 sw/airborne/lisa/test/lisa_test_actuators_mkk.c          | 4 ++--
 sw/airborne/subsystems/actuators/actuators_mkk.h         | 2 +-
 sw/airborne/test/test_actuators.c                        | 4 ++--
 sw/simulator/nps/nps_autopilot_rotorcraft.c              | 6 +++---
 11 files changed, 35 insertions(+), 21 deletions(-)

diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
index 6fbca0e2f7..557d03acb9 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
@@ -102,6 +102,14 @@
         <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
     </section>
 
+    <command_laws>
+      <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+      <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+      <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+      <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+      <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
+    </command_laws>
+
     <section name="IMU" prefix="IMU_">
         <define name="GYRO_P_NEUTRAL" value="32581"/>
         <define name="GYRO_Q_NEUTRAL" value="32008"/>
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
index 7752cd645a..f72a7f9295 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
@@ -90,7 +90,6 @@
         <axis name="THRUST" failsafe_value="0"/>
     </commands>
 
-
     <section name="MIXING" prefix="MOTOR_MIXING_">
         <define name="TRIM_ROLL" value="0"/>
         <define name="TRIM_PITCH" value="0"/>
@@ -103,6 +102,14 @@
         <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
     </section>
 
+    <command_laws>
+      <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+      <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+      <set servo="BACK"   value="motor_mixing.commands[SERVO_BACK]"/>
+      <set servo="RIGHT"  value="motor_mixing.commands[SERVO_RIGHT]"/>
+      <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
+    </command_laws>
+
     <section name="IMU" prefix="IMU_">
         <define name="GYRO_P_NEUTRAL" value="32581"/>
         <define name="GYRO_Q_NEUTRAL" value="32008"/>
diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml
index a064eca868..0afc6f9d0a 100644
--- a/conf/airframes/examples/h_hex.xml
+++ b/conf/airframes/examples/h_hex.xml
@@ -211,8 +211,8 @@
     <target name="test_adxl345"             board="lisa_l_1.0"/>
     <target name="test_esc_mkk_simple"      board="lisa_l_1.0"/>
     <target name="test_esc_asctecv1_simple" board="lisa_l_1.0"/>
-    <target name="test_actuators_mkk"       board="lisa_l_1.0"/>
-    <target name="test_actuators_asctecv1"  board="lisa_l_1.0"/>
+    <!--target name="test_actuators_mkk"       board="lisa_l_1.0"/>
+    <target name="test_actuators_asctecv1"  board="lisa_l_1.0"/-->
   </firmware>
 
 </airframe>
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml
index 19d65a5dcd..0d38e840c1 100644
--- a/conf/airframes/examples/lisa_asctec.xml
+++ b/conf/airframes/examples/lisa_asctec.xml
@@ -228,8 +228,8 @@
     <target name="test_adxl345"             board="lisa_l_1.1"/>
     <target name="test_esc_mkk_simple"      board="lisa_l_1.1"/>
     <target name="test_esc_asctecv1_simple" board="lisa_l_1.1"/>
-    <target name="test_actuators_mkk"       board="lisa_l_1.1"/>
-    <target name="test_actuators_asctecv1"  board="lisa_l_1.1"/>
+    <!--target name="test_actuators_mkk"       board="lisa_l_1.1"/>
+    <target name="test_actuators_asctecv1"  board="lisa_l_1.1"/-->
   </firmware>
 
 
diff --git a/conf/airframes/examples/setup_lisam2.xml b/conf/airframes/examples/setup_lisam2.xml
index 640c0e72e5..0d228f0fd9 100644
--- a/conf/airframes/examples/setup_lisam2.xml
+++ b/conf/airframes/examples/setup_lisam2.xml
@@ -4,7 +4,7 @@
 
   <firmware name="setup">
     <target name="tunnel"           board="lisa_m_2.0"/>
-    <target name="setup_actuators"  board="lisa_m_2.0"/>
+    <!--target name="setup_actuators"  board="lisa_m_2.0"/-->
   </firmware>
 
   <firmware name="lisa_test_progs">
diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c
index 1b72f44107..d55e6815ce 100644
--- a/sw/airborne/firmwares/setup/setup_actuators.c
+++ b/sw/airborne/firmwares/setup/setup_actuators.c
@@ -4,8 +4,7 @@
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
 #include "led.h"
-#include "firmwares/fixedwing/actuators.h"
-//#include "actuators.h"
+#include "subsystems/actuators.h"
 
 #include "firmwares/fixedwing/main_fbw.h"
 
@@ -31,7 +30,7 @@ void dl_parse_msg( void ) {
     uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
     LED_TOGGLE(2);
     if (servo_no < ACTUATORS_NB)
-      SetServo(servo_no, servo_value);
+      //SetServo(servo_no, servo_value);
   }
 #ifdef DlSetting
   else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
@@ -40,7 +39,7 @@ void dl_parse_msg( void ) {
     DlSetting(i, val);
     LED_TOGGLE(2);
     for (int j=0 ; j<8 ; j++) {
-      SetServo(j,actuators[j]);
+      //SetServo(j,actuators[j]);
     }
     DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
   } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
@@ -59,7 +58,7 @@ void init_fbw( void ) {
 
   uint8_t i;
   for(i = 0; i < ACTUATORS_NB; i++) {
-    SetServo(i, 1500);
+    //SetServo(i, 1500);
   }
 
   //  SetServo(SERVO_GAZ, SERVO_GAZ_MIN);
diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c
index 0d95029419..aeda89aa54 100644
--- a/sw/airborne/lisa/lisa_stm_passthrough_main.c
+++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c
@@ -27,8 +27,8 @@
 #include "mcu_periph/sys_time.h"
 #include "subsystems/datalink/downlink.h"
 #include "firmwares/rotorcraft/commands.h"
-#include "actuators.h"
-#include "actuators/actuators_pwm.h"
+#include "subsystems/actuators.h"
+#include "subsystems/actuators/actuators_pwm.h"
 #include "subsystems/imu.h"
 #include "subsystems/radio_control.h"
 #include "autopilot.h"
diff --git a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
index 04a5679034..cd86efe09b 100644
--- a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
+++ b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
@@ -25,7 +25,7 @@
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
 #include "firmwares/rotorcraft/commands.h"
-#include "actuators.h"
+#include "subsystems/actuators.h"
 #include "subsystems/datalink/downlink.h"
 #include "led.h"
 
@@ -95,7 +95,7 @@ static inline void main_periodic_task( void ) {
     commands[COMMAND_YAW] = 20;
     commands[COMMAND_THRUST] = 0;
     // actuators_set(TRUE);
-    actuators_set(FALSE);
+    //actuators_set(FALSE);
   }
   LED_PERIODIC();
 
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.h b/sw/airborne/subsystems/actuators/actuators_mkk.h
index f1436e4373..e90a2ecae8 100644
--- a/sw/airborne/subsystems/actuators/actuators_mkk.h
+++ b/sw/airborne/subsystems/actuators/actuators_mkk.h
@@ -41,7 +41,7 @@ extern struct ActuatorsMkk actuators_mkk;
 extern void actuators_mkk_init(void);
 extern void actuators_mkk_set(void);
 
-#define ActuatorMkkSet(_i, _v) { actuators_mkk.trans[i].buf[0] = _v; }
+#define ActuatorMkkSet(_i, _v) { actuators_mkk.trans[_i].buf[0] = _v; }
 #define ActuatorsMkkInit() actuators_mkk_init()
 #define ActuatorsMkkCommit() actuators_mkk_set()
 
diff --git a/sw/airborne/test/test_actuators.c b/sw/airborne/test/test_actuators.c
index c7380336d0..dc2d4eda10 100644
--- a/sw/airborne/test/test_actuators.c
+++ b/sw/airborne/test/test_actuators.c
@@ -28,7 +28,7 @@
 
 #include "mcu_periph/i2c.h"
 #include "firmwares/rotorcraft/commands.h"
-#include "firmwares/rotorcraft/actuators.h"
+#include "subsystems/actuators.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -61,7 +61,7 @@ static inline void main_periodic_task( void ) {
   commands[COMMAND_YAW]=0;
   commands[COMMAND_THRUST]=1;
 
-  actuators_set(TRUE);
+  //actuators_set(TRUE);
 
   LED_PERIODIC();
 
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index 08d5d19dd2..81fab9ac3f 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -32,7 +32,7 @@
 #include "mcu_periph/sys_time.h"
 #include "state.h"
 
-#include "actuators/supervision.h"
+#include "actuators/motor_mixing.h"
 
 
 struct NpsAutopilot autopilot;
@@ -100,8 +100,8 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
 
   /* scale final motor commands to 0-1 for feeding the fdm */
   /* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */
-  for (uint8_t i=0; i<SUPERVISION_NB_MOTOR; i++)
-    autopilot.commands[i] = (double)(supervision.commands[i] - SUPERVISION_MIN_MOTOR) / SUPERVISION_MAX_MOTOR;
+  for (uint8_t i=0; i<MOTOR_MIXING_NB_MOTOR; i++)
+    autopilot.commands[i] = (double)(motor_mixing.commands[i]/MAX_PPRZ);
 
 }
 

From f30b8d6469db02287454c72411c301cf09f0e24a Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 21:32:09 +0200
Subject: [PATCH 22/26] [actuators] fix some bugs

---
 conf/airframes/examples/lisa_asctec.xml                        | 2 +-
 conf/airframes/examples/setup_lisam2.xml                       | 2 +-
 sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h | 1 -
 sw/simulator/nps/nps_autopilot_rotorcraft.c                    | 2 +-
 4 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml
index 0d38e840c1..c65616a7a6 100644
--- a/conf/airframes/examples/lisa_asctec.xml
+++ b/conf/airframes/examples/lisa_asctec.xml
@@ -214,7 +214,7 @@
   <firmware name="lisa_test_progs">
     <target name="test_led"   board="lisa_l_1.1"/>
     <target name="test_uart"   board="lisa_l_1.1"/>
-    <target name="test_servos"   board="lisa_l_1.1"/>
+    <!--target name="test_servos"   board="lisa_l_1.1"/-->
     <target name="test_telemetry"   board="lisa_l_1.1"/>
     <target name="test_baro"        board="lisa_l_1.1"/>
     <target name="test_imu_b2"      board="lisa_l_1.1"/>
diff --git a/conf/airframes/examples/setup_lisam2.xml b/conf/airframes/examples/setup_lisam2.xml
index 0d228f0fd9..a300d55697 100644
--- a/conf/airframes/examples/setup_lisam2.xml
+++ b/conf/airframes/examples/setup_lisam2.xml
@@ -11,7 +11,7 @@
     <target name="test_led"	                board="lisa_m_2.0"/>
     <target name="test_sys_time"            board="lisa_m_2.0"/>
     <target name="test_uart"                board="lisa_m_2.0"/>
-    <target name="test_servos"              board="lisa_m_2.0"/>
+    <!--target name="test_servos"              board="lisa_m_2.0"/-->
     <target name="test_telemetry"           board="lisa_m_2.0"/>
     <target name="test_baro"                board="lisa_m_2.0"/>
     <target name="test_rc_spektrum"         board="lisa_m_2.0"/>
diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h
index 45566777af..02c159b6b0 100644
--- a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.h
@@ -29,7 +29,6 @@
 #define SERVOS_TICS_OF_USEC(_v) (_v)
 
 #define ActuatorPwmSet(_i, _v) {}
-#define ActuatorsPwmInit() {}
 #define ActuatorsPwmCommit() {}
 
 extern void actuators_pwm_arch_init(void);
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index 81fab9ac3f..3ec8b45a16 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -32,7 +32,7 @@
 #include "mcu_periph/sys_time.h"
 #include "state.h"
 
-#include "actuators/motor_mixing.h"
+#include "subsystems/actuators/motor_mixing.h"
 
 
 struct NpsAutopilot autopilot;

From 67768facc441bea452be9d1108b6683980217225 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 19 Oct 2012 22:54:38 +0200
Subject: [PATCH 23/26] [actuators] fix and clean

---
 conf/airframes/examples/lisa_asctec.xml       |  2 +-
 conf/airframes/examples/setup_lisam2.xml      |  2 +-
 conf/firmwares/lisa_test_progs.makefile       |  1 -
 .../subsystems/rotorcraft/fdm_jsbsim.makefile |  1 +
 .../subsystems/shared/actuators_pwm.makefile  |  1 -
 .../subsystems/actuators/actuators_pwm_arch.c |  5 ++--
 .../subsystems/actuators/actuators_pwm_arch.h |  2 --
 .../subsystems/actuators/actuators_pwm_arch.c |  1 -
 .../subsystems/actuators/actuators_pwm_arch.h |  1 -
 sw/airborne/lisa/test_servos.c                |  6 ++--
 .../subsystems/actuators/actuators_pwm.c      | 29 -------------------
 .../subsystems/actuators/actuators_pwm.h      | 13 ++++-----
 sw/simulator/nps/nps_autopilot_rotorcraft.c   |  2 +-
 13 files changed, 15 insertions(+), 51 deletions(-)
 delete mode 100644 sw/airborne/subsystems/actuators/actuators_pwm.c

diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml
index c65616a7a6..0d38e840c1 100644
--- a/conf/airframes/examples/lisa_asctec.xml
+++ b/conf/airframes/examples/lisa_asctec.xml
@@ -214,7 +214,7 @@
   <firmware name="lisa_test_progs">
     <target name="test_led"   board="lisa_l_1.1"/>
     <target name="test_uart"   board="lisa_l_1.1"/>
-    <!--target name="test_servos"   board="lisa_l_1.1"/-->
+    <target name="test_servos"   board="lisa_l_1.1"/>
     <target name="test_telemetry"   board="lisa_l_1.1"/>
     <target name="test_baro"        board="lisa_l_1.1"/>
     <target name="test_imu_b2"      board="lisa_l_1.1"/>
diff --git a/conf/airframes/examples/setup_lisam2.xml b/conf/airframes/examples/setup_lisam2.xml
index a300d55697..0d228f0fd9 100644
--- a/conf/airframes/examples/setup_lisam2.xml
+++ b/conf/airframes/examples/setup_lisam2.xml
@@ -11,7 +11,7 @@
     <target name="test_led"	                board="lisa_m_2.0"/>
     <target name="test_sys_time"            board="lisa_m_2.0"/>
     <target name="test_uart"                board="lisa_m_2.0"/>
-    <!--target name="test_servos"              board="lisa_m_2.0"/-->
+    <target name="test_servos"              board="lisa_m_2.0"/>
     <target name="test_telemetry"           board="lisa_m_2.0"/>
     <target name="test_baro"                board="lisa_m_2.0"/>
     <target name="test_rc_spektrum"         board="lisa_m_2.0"/>
diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile
index 3c4cafe1c5..c5d55b2e43 100644
--- a/conf/firmwares/lisa_test_progs.makefile
+++ b/conf/firmwares/lisa_test_progs.makefile
@@ -140,7 +140,6 @@ test_servos.srcs    = $(COMMON_TEST_SRCS)
 test_servos.CFLAGS  += -I$(SRC_LISA)
 test_servos.LDFLAGS += -lm
 test_servos.srcs    += $(SRC_LISA)/test_servos.c
-test_servos.srcs    += subsystems/actuators/actuators_pwm.c
 test_servos.srcs    += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
 ifeq ($(BOARD), lisa_m)
   test_servos.CFLAGS += -DUSE_SERVOS_7AND8
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index ccf7d83b07..99663ac7c6 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -82,6 +82,7 @@ nps.srcs += $(SRC_FIRMWARE)/telemetry.c \
             subsystems/datalink/downlink.c \
             $(SRC_ARCH)/ivy_transport.c
 
+nps.srcs   += subsystems/actuators.c
 nps.srcs   += $(SRC_FIRMWARE)/commands.c
 
 nps.srcs += $(SRC_FIRMWARE)/datalink.c
diff --git a/conf/firmwares/subsystems/shared/actuators_pwm.makefile b/conf/firmwares/subsystems/shared/actuators_pwm.makefile
index a47ef259c4..c476520918 100644
--- a/conf/firmwares/subsystems/shared/actuators_pwm.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_pwm.makefile
@@ -1,5 +1,4 @@
 
 
 $(TARGET).CFLAGS += -DACTUATORS
-$(TARGET).srcs   += subsystems/actuators/actuators_pwm.c
 $(TARGET).srcs   += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
index 27b9c2a361..82c801c03d 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
@@ -19,9 +19,10 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#include "std.h"
-#include "subsystems/actuators.h"
 #include "subsystems/actuators/actuators_pwm_arch.h"
+#include "std.h"
+#include BOARD_CONFIG
+#include "generated/airframe.h"
 
 /* 40 Hz */
 #ifndef SERVOS_PERIOD
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
index 071b4c288b..e54d87a41e 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.h
@@ -115,6 +115,4 @@ extern const uint8_t pwm_latch_value;
     PWMLER = pwm_latch_value;   \
   }
 
-extern void actuators_pwm_arch_init(void);
-
 #endif /* ACTUATORS_PWM_ARCH_H */
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
index 193fb1cb9d..8827c23bf6 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -23,7 +23,6 @@
  *  STM32 PWM servos handling.
  */
 
-#include "subsystems/actuators.h"
 #include "subsystems/actuators/actuators_pwm_arch.h"
 
 #include <libopencm3/stm32/f1/gpio.h>
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
index e5d0d547b7..e81fd46c41 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.h
@@ -40,7 +40,6 @@
 
 extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
 
-extern void actuators_pwm_arch_init(void);
 extern void actuators_pwm_commit(void);
 
 #define SERVOS_TICS_OF_USEC(_v) (_v)
diff --git a/sw/airborne/lisa/test_servos.c b/sw/airborne/lisa/test_servos.c
index 30fe68c776..3270f8bb70 100644
--- a/sw/airborne/lisa/test_servos.c
+++ b/sw/airborne/lisa/test_servos.c
@@ -42,7 +42,7 @@ int main(void) {
 static inline void main_init( void ) {
   mcu_init();
   sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
-  actuators_init();
+  ActuatorsPwmInit();
 }
 
 static inline void main_periodic( void ) {
@@ -50,9 +50,9 @@ static inline void main_periodic( void ) {
   foo += 0.0025;
   int32_t bar = 1500 + 500. * sin(foo);
   for (int i = 0; i < ACTUATORS_PWM_NB; i++) {
-    actuators_pwm_values[i] = bar;
+    ActuatorPwmSet(i, bar);
   }
-  actuators_pwm_commit();
+  ActuatorsPwmCommit();
 
   LED_PERIODIC();
 }
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.c b/sw/airborne/subsystems/actuators/actuators_pwm.c
deleted file mode 100644
index 0606133b38..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_pwm.c
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * Copyright (C) 2010-2012 The Paparazzi Team
- *
- * 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.
- */
-
-#include "subsystems/actuators/actuators_pwm.h"
-
-void actuators_pwm_init(void)
-{
-  // implemented in arch files
-  actuators_pwm_arch_init();
-}
-
diff --git a/sw/airborne/subsystems/actuators/actuators_pwm.h b/sw/airborne/subsystems/actuators/actuators_pwm.h
index 80953089fa..184d695957 100644
--- a/sw/airborne/subsystems/actuators/actuators_pwm.h
+++ b/sw/airborne/subsystems/actuators/actuators_pwm.h
@@ -22,16 +22,13 @@
 #ifndef ACTUATORS_PWM_H
 #define ACTUATORS_PWM_H
 
-#include "std.h"
-#include "generated/airframe.h"
-
-#include BOARD_CONFIG
 #include "subsystems/actuators/actuators_pwm_arch.h"
 
-extern void actuators_pwm_init(void);
-
-#define ActuatorsPwmInit() actuators_pwm_init()
-/* ActuatorSet and ActuatorsPwmCommit are implemented in arch files
+/** Arch dependent init file.
+ * implemented in arch files
  */
+extern void actuators_pwm_arch_init(void);
+
+#define ActuatorsPwmInit() actuators_pwm_arch_init()
 
 #endif /* ACTUATORS_PWM_H */
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index 3ec8b45a16..51ae195d95 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -101,7 +101,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
   /* scale final motor commands to 0-1 for feeding the fdm */
   /* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */
   for (uint8_t i=0; i<MOTOR_MIXING_NB_MOTOR; i++)
-    autopilot.commands[i] = (double)(motor_mixing.commands[i]/MAX_PPRZ);
+    autopilot.commands[i] = (double)motor_mixing.commands[i]/MAX_PPRZ;
 
 }
 

From a375cd09ca6d549d21095f2bd015992d352c448c Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 22 Oct 2012 18:26:24 +0200
Subject: [PATCH 24/26] [commands] merging rotorcraft and fixedwing commands

- all commands are now in pprz_t type (int16_t)
- should not be a problem since all commands are already scaled at pprz
  format
---
 conf/firmwares/lisa_passthrough.makefile      |  2 +-
 conf/firmwares/lisa_test_progs.makefile       |  6 +--
 conf/firmwares/rotorcraft.makefile            |  2 +-
 .../subsystems/fixedwing/autopilot.makefile   |  2 +-
 .../subsystems/rotorcraft/fdm_jsbsim.makefile |  2 +-
 sw/airborne/arch/sim/jsbsim_hw.h              |  2 +-
 sw/airborne/arch/sim/sim_ap.c                 |  2 +-
 sw/airborne/firmwares/beth/main_stm32.c       |  2 +-
 sw/airborne/firmwares/fixedwing/autopilot.h   | 24 +++++++--
 .../firmwares/fixedwing/fbw_downlink.h        |  2 +-
 sw/airborne/firmwares/fixedwing/main_fbw.c    |  2 +-
 sw/airborne/firmwares/rotorcraft/autopilot.c  |  9 ++--
 sw/airborne/firmwares/rotorcraft/autopilot.h  | 21 +++++++-
 sw/airborne/firmwares/rotorcraft/commands.c   | 26 ----------
 sw/airborne/firmwares/rotorcraft/commands.h   | 49 -------------------
 sw/airborne/firmwares/rotorcraft/main.c       |  2 +-
 sw/airborne/link_mcu_usart.c                  |  2 +-
 sw/airborne/lisa/lisa_stm_passthrough_main.c  |  2 +-
 .../lisa/test/lisa_test_actuators_mkk.c       |  2 +-
 sw/airborne/{ => subsystems}/commands.c       | 16 +-----
 sw/airborne/{ => subsystems}/commands.h       | 13 ++---
 sw/airborne/subsystems/electrical.c           |  2 +-
 sw/airborne/test/test_actuators.c             |  2 +-
 23 files changed, 69 insertions(+), 125 deletions(-)
 delete mode 100644 sw/airborne/firmwares/rotorcraft/commands.c
 delete mode 100644 sw/airborne/firmwares/rotorcraft/commands.h
 rename sw/airborne/{ => subsystems}/commands.c (79%)
 rename sw/airborne/{ => subsystems}/commands.h (81%)

diff --git a/conf/firmwares/lisa_passthrough.makefile b/conf/firmwares/lisa_passthrough.makefile
index 84d7c3d876..9bda00c80b 100644
--- a/conf/firmwares/lisa_passthrough.makefile
+++ b/conf/firmwares/lisa_passthrough.makefile
@@ -60,7 +60,7 @@ stm_passthrough.srcs += $(SRC_LISA)/lisa_overo_link.c           \
 stm_passthrough.srcs += math/pprz_trig_int.c
 stm_passthrough.srcs += lisa/plug_sys.c
 
-stm_passthrough.srcs += $(SRC_FIRMWARE)/commands.c
+stm_passthrough.srcs += subsystems/commands.c
 
 # Radio control
 #
diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile
index c5d55b2e43..281bdc3187 100644
--- a/conf/firmwares/lisa_test_progs.makefile
+++ b/conf/firmwares/lisa_test_progs.makefile
@@ -508,7 +508,7 @@ test_actuators_mkk.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
 test_actuators_mkk.srcs   += $(COMMON_TELEMETRY_SRCS)
 
 test_actuators_mkk.srcs   += test/test_actuators.c
-test_actuators_mkk.srcs   += $(SRC_FIRMWARE)/commands.c
+test_actuators_mkk.srcs   += subsystems/commands.c
 test_actuators_mkk.srcs   += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
 test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
 test_actuators_mkk.srcs   += $(SRC_FIRMWARE)/actuators/supervision.c
@@ -526,7 +526,7 @@ test_actuators_asctecv1.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
 test_actuators_asctecv1.srcs   += $(COMMON_TELEMETRY_SRCS)
 
 test_actuators_asctecv1.srcs   += test/test_actuators.c
-test_actuators_asctecv1.srcs   += $(SRC_FIRMWARE)/commands.c
+test_actuators_asctecv1.srcs   += subsystems/commands.c
 test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
 test_actuators_asctecv1.srcs   += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
 test_actuators_asctecv1.CFLAGS += -DUSE_I2C1
@@ -560,7 +560,7 @@ test_actuators_asctecv1.srcs   += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_ar
 #test_manual.srcs   += $(COMMON_TELEMETRY_SRCS)
 #
 #test_manual.srcs   += test/test_manual.c
-#test_manual.srcs   += $(SRC_FIRMWARE)/commands.c
+#test_manual.srcs   += subsystems/commands.c
 ##test_manual.srcs   += subsystems/actuators/actuators_pwm.c
 #test_manual.srcs   += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
 #
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index c46f2c7715..a67e181dc8 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -104,7 +104,7 @@ ifeq ($(TARGET), ap)
   include $(CFG_SHARED)/i2c_select.makefile
 endif
 
-ap.srcs += $(SRC_FIRMWARE)/commands.c
+ap.srcs += subsystems/commands.c
 ap.srcs += subsystems/actuators.c
 
 #
diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index 6d8fc2507d..cb424266aa 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -152,7 +152,7 @@ endif
 fbw_CFLAGS		+= -DFBW
 fbw_srcs 		+= $(SRC_FIRMWARE)/main_fbw.c
 fbw_srcs 		+= subsystems/electrical.c
-fbw_srcs 		+= $(SRC_FIXEDWING)/commands.c
+fbw_srcs 		+= subsystems/commands.c
 fbw_srcs 		+= subsystems/actuators.c
 fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index 99663ac7c6..22b8ee11e6 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -83,7 +83,7 @@ nps.srcs += $(SRC_FIRMWARE)/telemetry.c \
             $(SRC_ARCH)/ivy_transport.c
 
 nps.srcs   += subsystems/actuators.c
-nps.srcs   += $(SRC_FIRMWARE)/commands.c
+nps.srcs   += subsystems/commands.c
 
 nps.srcs += $(SRC_FIRMWARE)/datalink.c
 
diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h
index fa32784663..27406b89a6 100644
--- a/sw/airborne/arch/sim/jsbsim_hw.h
+++ b/sw/airborne/arch/sim/jsbsim_hw.h
@@ -40,7 +40,7 @@
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 #include "firmwares/fixedwing/guidance/guidance_v.h"
 #include "subsystems/sensors/infrared.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/fixedwing/main_ap.h"
 #include "ap_downlink.h"
 #include "sim_uart.h"
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index 7eaffac836..4a9a2ec3c4 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -18,7 +18,7 @@
 #include "subsystems/nav.h"
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 #include "firmwares/fixedwing/guidance/guidance_v.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/fixedwing/main_ap.h"
 #include "ap_downlink.h"
 #include "sim_uart.h"
diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c
index af702db5ee..a1e1b7c8c8 100644
--- a/sw/airborne/firmwares/beth/main_stm32.c
+++ b/sw/airborne/firmwares/beth/main_stm32.c
@@ -28,7 +28,7 @@
 #include "mcu_periph/can.h"
 #include "mcu_periph/sys_time.h"
 #include "subsystems/datalink/downlink.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/rotorcraft/actuators.h"
 //#include "booz/booz_radio_control.h"
 #include "subsystems/imu.h"
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h
index cbc0253d97..8c9cc9ab50 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot.h
@@ -33,6 +33,7 @@
 #include <inttypes.h>
 #include "std.h"
 #include "mcu_periph/sys_time.h"
+#include "generated/airframe.h"
 
 #define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
 
@@ -57,6 +58,10 @@ extern bool_t kill_throttle;
 /** flight time in seconds. */
 extern uint16_t autopilot_flight_time;
 
+#define autopilot_ResetFlightTimeAndLaunch(_) { \
+  autopilot_flight_time = 0; launch = FALSE; \
+}
+
 
 // FIXME, move to control
 #define LATERAL_MODE_MANUAL    0
@@ -85,6 +90,22 @@ extern bool_t gps_lost;
   (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
 })
 
+/** Commands trim for roll and pitch/
+ */
+#ifndef COMMAND_ROLL_TRIM
+#define COMMAND_ROLL_TRIM 0
+#endif
+
+#ifndef COMMAND_PITCH_TRIM
+#define COMMAND_PITCH_TRIM 0
+#endif
+
+extern pprz_t command_roll_trim = COMMAND_ROLL_TRIM;
+extern pprz_t command_pitch_trim = COMMAND_PITCH_TRIM;
+
+
+/** Power switch control.
+ */
 extern bool_t power_switch;
 
 #ifdef POWER_SWITCH_LED
@@ -96,9 +117,6 @@ extern bool_t power_switch;
 #define autopilot_SetPowerSwitch(_x) { power_switch = _x; }
 #endif // POWER_SWITCH_LED
 
-#define autopilot_ResetFlightTimeAndLaunch(_) { \
-  autopilot_flight_time = 0; launch = FALSE; \
-}
 
 /* CONTROL_RATE will be removed in the next release
  * please use CONTROL_FREQUENCY instead
diff --git a/sw/airborne/firmwares/fixedwing/fbw_downlink.h b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
index 0bc8cd48ab..19947da37e 100644
--- a/sw/airborne/firmwares/fixedwing/fbw_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
@@ -39,7 +39,7 @@
 #include "messages.h"
 #include "generated/periodic_telemetry.h"
 #include "generated/airframe.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 
 #include "mcu_periph/uart.h"
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 14c298e9c4..544ac2e36a 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -36,7 +36,7 @@
 #include "firmwares/fixedwing/main_fbw.h"
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #include "subsystems/electrical.h"
 #include "subsystems/radio_control.h"
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index 478683ba41..fd8b4c7524 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -23,7 +23,7 @@
 
 #include "subsystems/radio_control.h"
 #include "subsystems/gps.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/rotorcraft/navigation.h"
 #include "firmwares/rotorcraft/guidance.h"
 #include "firmwares/rotorcraft/stabilization.h"
@@ -102,15 +102,14 @@ void autopilot_periodic(void) {
 #else
   if (autopilot_mode == AP_MODE_KILL) {
 #endif
-    SetCommands(commands_failsafe,
-		autopilot_in_flight, autopilot_motors_on);
+    SetCommands(commands_failsafe);
   }
   else {
     guidance_v_run( autopilot_in_flight );
     guidance_h_run( autopilot_in_flight );
-    SetCommands(stabilization_cmd,
-        autopilot_in_flight, autopilot_motors_on);
+    SetCommands(stabilization_cmd);
   }
+  RotorcraftCommandsTest(commands, autopilot_in_flight, autopilot_motors_on);
 
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index d3fbfb1df3..9ad782dea9 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -65,8 +65,8 @@ extern bool_t autopilot_detect_ground_once;
 
 extern uint16_t autopilot_flight_time;
 
-
-
+/** Default RC mode.
+ */
 #ifndef MODE_MANUAL
 #define MODE_MANUAL AP_MODE_RATE_DIRECT
 #endif
@@ -109,6 +109,23 @@ extern uint16_t autopilot_flight_time;
   }
 #endif
 
+/** Thrust and Yaw commands limitation.
+ *  Limit thrust and/or yaw depending of the in_flight
+ *  and motors_on flag status
+ */
+#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
+#define RotorcraftCommandsTest(_cmd, _in_flight,  _motor_on) {  \
+  if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; }                 \
+  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }               \
+}
+#else
+#define RotorcraftCommandsTest(_cmd, _in_flight,  _motor_on) {  \
+  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }               \
+}
+#endif
+
+/** Ground detection based on accelerometers.
+ */
 #ifndef THRESHOLD_GROUND_DETECT
 #define THRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/commands.c b/sw/airborne/firmwares/rotorcraft/commands.c
deleted file mode 100644
index a07e5fe486..0000000000
--- a/sw/airborne/firmwares/rotorcraft/commands.c
+++ /dev/null
@@ -1,26 +0,0 @@
-/*  $Id$
- *
- * (c) 2009 Antoine Drouin <poinix@gmail.com>
- *
- * 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.
- */
-
-#include "firmwares/rotorcraft/commands.h"
-
-int32_t commands[COMMANDS_NB];
-const int32_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
diff --git a/sw/airborne/firmwares/rotorcraft/commands.h b/sw/airborne/firmwares/rotorcraft/commands.h
deleted file mode 100644
index fa1cb57842..0000000000
--- a/sw/airborne/firmwares/rotorcraft/commands.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*  $Id$
- *
- * (c) 2009 Antoine Drouin <poinix@gmail.com>
- *
- * 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.
- *
- */
-
-#ifndef COMMANDS_H
-#define COMMANDS_H
-
-#include "paparazzi.h"
-#include "generated/airframe.h"
-
-extern int32_t commands[COMMANDS_NB];
-extern const int32_t commands_failsafe[COMMANDS_NB];
-
-#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
-#define SetCommands(_in_cmd, _in_flight, _motors_on) {			\
-    commands[COMMAND_PITCH]  = _in_cmd[COMMAND_PITCH];		\
-    commands[COMMAND_ROLL]   = _in_cmd[COMMAND_ROLL];		\
-    commands[COMMAND_YAW]    = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \
-    commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
-  }
-#else
-#define SetCommands(_in_cmd, _in_flight, _motors_on) {			\
-    commands[COMMAND_PITCH]  = _in_cmd[COMMAND_PITCH];		\
-    commands[COMMAND_ROLL]   = _in_cmd[COMMAND_ROLL];		\
-    commands[COMMAND_YAW]    = _in_cmd[COMMAND_YAW]; \
-    commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
-  }
-#endif
-
-#endif /* COMMANDS_H */
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index e927c539a8..3218dec115 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -35,7 +35,7 @@
 #include "subsystems/settings.h"
 #include "subsystems/datalink/xbee.h"
 
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #if USE_MOTOR_MIXING
 #include "subsystems/actuators/motor_mixing.h"
diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c
index ec93d06f40..ebbf2201ae 100644
--- a/sw/airborne/link_mcu_usart.c
+++ b/sw/airborne/link_mcu_usart.c
@@ -24,7 +24,7 @@
 #include "mcu_periph/uart.h"
 #include "led.h"
 
-#include "commands.h"
+#include "subsystems/commands.h"
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 // LINK
diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c
index aeda89aa54..c2b61689b9 100644
--- a/sw/airborne/lisa/lisa_stm_passthrough_main.c
+++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c
@@ -26,7 +26,7 @@
 #include "mcu_periph/uart.h"
 #include "mcu_periph/sys_time.h"
 #include "subsystems/datalink/downlink.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #include "subsystems/actuators/actuators_pwm.h"
 #include "subsystems/imu.h"
diff --git a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
index cd86efe09b..25ab7e7e6f 100644
--- a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
+++ b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
@@ -24,7 +24,7 @@
 
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #include "subsystems/datalink/downlink.h"
 #include "led.h"
diff --git a/sw/airborne/commands.c b/sw/airborne/subsystems/commands.c
similarity index 79%
rename from sw/airborne/commands.c
rename to sw/airborne/subsystems/commands.c
index 3dd61cfdd3..b03927465c 100644
--- a/sw/airborne/commands.c
+++ b/sw/airborne/subsystems/commands.c
@@ -1,5 +1,4 @@
-/*  $Id$
- *
+/*
  * (c) 2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
@@ -26,18 +25,7 @@
  *
  */
 
-#include "commands.h"
-
-#ifndef COMMAND_ROLL_TRIM
-#define COMMAND_ROLL_TRIM 0
-#endif
-
-#ifndef COMMAND_PITCH_TRIM
-#define COMMAND_PITCH_TRIM 0
-#endif
-
-pprz_t command_roll_trim = COMMAND_ROLL_TRIM;
-pprz_t command_pitch_trim = COMMAND_PITCH_TRIM;
+#include "subsystems/commands.h"
 
 pprz_t commands[COMMANDS_NB];
 const pprz_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
diff --git a/sw/airborne/commands.h b/sw/airborne/subsystems/commands.h
similarity index 81%
rename from sw/airborne/commands.h
rename to sw/airborne/subsystems/commands.h
index 4716959233..762afac8c3 100644
--- a/sw/airborne/commands.h
+++ b/sw/airborne/subsystems/commands.h
@@ -1,5 +1,4 @@
-/*  $Id$
- *
+/*
  * (c) 2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
@@ -32,14 +31,12 @@
 #include "paparazzi.h"
 #include "generated/airframe.h"
 
-extern pprz_t command_roll_trim;
-extern pprz_t command_pitch_trim;
-
+/** Storage of intermediate command values.
+ *  These values come from the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
+ *  They are asyncronisly used to set the servos
+ */
 extern pprz_t commands[COMMANDS_NB];
 extern const pprz_t commands_failsafe[COMMANDS_NB];
-/** Storage of intermediate command values: these values come from
-the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
-They are asyncronisly used to set the servos */
 
 #define SetCommands(t) { \
   int i; \
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c
index 2d9c1d192c..de1a0e8b8d 100644
--- a/sw/airborne/subsystems/electrical.c
+++ b/sw/airborne/subsystems/electrical.c
@@ -1,7 +1,7 @@
 #include "subsystems/electrical.h"
 
 #include "mcu_periph/adc.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 
 #include "generated/airframe.h"
 #include BOARD_CONFIG
diff --git a/sw/airborne/test/test_actuators.c b/sw/airborne/test/test_actuators.c
index dc2d4eda10..d6e6c66a71 100644
--- a/sw/airborne/test/test_actuators.c
+++ b/sw/airborne/test/test_actuators.c
@@ -27,7 +27,7 @@
 #include "led.h"
 
 #include "mcu_periph/i2c.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 
 static inline void main_init( void );

From cd19383a81899113f52e728d2a4cb426e1334651 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 23 Oct 2012 10:52:20 +0200
Subject: [PATCH 25/26] [actuators] bug fix and config updates

---
 conf/airframes/CDW/DualBoardApFbw.xml         |  2 +-
 conf/airframes/CDW/classix.xml                |  2 +-
 .../subsystems/fixedwing/autopilot.makefile   |  1 +
 .../arch/lpc21/mcu_periph/sys_time_arch.c     |  2 +-
 sw/airborne/arch/sim/sim_gps.c                |  1 -
 sw/airborne/firmwares/fixedwing/autopilot.c   | 66 +++++++++++++++++++
 sw/airborne/firmwares/fixedwing/autopilot.h   | 25 +++++--
 sw/airborne/firmwares/fixedwing/main_ap.c     | 28 +-------
 .../subsystems/actuators/motor_mixing.c       |  2 +-
 .../subsystems/actuators/motor_mixing.h       |  3 +-
 10 files changed, 93 insertions(+), 39 deletions(-)
 create mode 100644 sw/airborne/firmwares/fixedwing/autopilot.c

diff --git a/conf/airframes/CDW/DualBoardApFbw.xml b/conf/airframes/CDW/DualBoardApFbw.xml
index 02e592bf27..619a34f2f0 100644
--- a/conf/airframes/CDW/DualBoardApFbw.xml
+++ b/conf/airframes/CDW/DualBoardApFbw.xml
@@ -265,7 +265,7 @@
       <subsystem name="radio_control" type="ppm"/>
 
       <!-- split of ap/fbw specific subsystems for fbw -->
-      <subsystem name="actuators" type="direct"/>
+      <subsystem name="actuators" type="pwm"/>
       <subsystem name="intermcu" type="uart">
         <configure name="INTERMCU_PORT_NR" value="2"/>
       </subsystem>
diff --git a/conf/airframes/CDW/classix.xml b/conf/airframes/CDW/classix.xml
index 2cb29964cd..8f73aacd32 100644
--- a/conf/airframes/CDW/classix.xml
+++ b/conf/airframes/CDW/classix.xml
@@ -2,7 +2,7 @@
 
 <airframe name="TwinStarXSens">
 
-  <servos>
+  <servos driver="Ppm">
     <servo name="THROTTLE_LEFT" no="1" min="1100" neutral="1100" max="1900"/>
     <servo name="THROTTLE_RIGHT" no="0" min="1100" neutral="1100" max="1900"/>
     <servo name="AILERON_RIGHT" no="2" min="2300" neutral="1550" max="700"/>
diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index cb424266aa..6c762740da 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -163,6 +163,7 @@ fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 
 ap_CFLAGS 		+= -DAP
 ap_srcs 		+= $(SRC_FIRMWARE)/main_ap.c
+ap_srcs 		+= $(SRC_FIRMWARE)/autopilot.c
 ap_srcs			+= $(SRC_FIRMWARE)/ap_downlink.c
 ap_srcs 		+= state.c
 
diff --git a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c
index ed2335ff66..1c41a460a3 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c
@@ -43,7 +43,7 @@
 #ifdef SERVOS_4017
 #include "subsystems/actuators/servos_4017_hw.h"
 #endif
-#ifdef SERVOS_PPM
+#ifdef SERVOS_PPM_MAT
 #include "subsystems/actuators/servos_ppm_hw.h"
 #endif
 #else
diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c
index 4c8d0cfa93..75ded9ea8b 100644
--- a/sw/airborne/arch/sim/sim_gps.c
+++ b/sw/airborne/arch/sim/sim_gps.c
@@ -7,7 +7,6 @@
 /** From airborne/autopilot/ */
 #include "generated/airframe.h"
 #include "generated/flight_plan.h"
-#include "autopilot.h"
 #include "subsystems/gps.h"
 #include "math/pprz_geodetic_float.h"
 #include "math/pprz_geodetic_int.h"
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c
new file mode 100644
index 0000000000..21e70adbc4
--- /dev/null
+++ b/sw/airborne/firmwares/fixedwing/autopilot.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+/** \file autopilot.c
+ *  \brief Autopilot inititalization
+ *
+ */
+
+#include "firmwares/fixedwing/autopilot.h"
+
+uint8_t pprz_mode;
+bool_t kill_throttle;
+
+bool_t launch;
+
+/** flight time in seconds. */
+uint16_t autopilot_flight_time;
+
+uint8_t lateral_mode;
+
+uint16_t vsupply;
+float energy;
+
+bool_t gps_lost;
+
+pprz_t command_roll_trim;
+pprz_t command_pitch_trim;
+
+bool_t power_switch;
+
+void autopilot_init(void) {
+  pprz_mode = PPRZ_MODE_AUTO2;
+  kill_throttle = FALSE;
+  launch = FALSE;
+  autopilot_flight_time = 0;
+
+  lateral_mode = LATERAL_MODE_MANUAL;
+
+  gps_lost = FALSE;
+
+  command_roll_trim = COMMAND_ROLL_TRIM;
+  command_pitch_trim = COMMAND_PITCH_TRIM;
+
+  power_switch = FALSE;
+
+}
+
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h
index 8c9cc9ab50..4c2db8c264 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot.h
@@ -1,6 +1,4 @@
 /*
- * $Id$
- *
  * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
@@ -32,15 +30,23 @@
 
 #include <inttypes.h>
 #include "std.h"
+#include "paparazzi.h"
 #include "mcu_periph/sys_time.h"
 #include "generated/airframe.h"
 
-#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
 
+/** Autopilot inititalization.
+ */
+extern void autopilot_init(void);
+
+/** Threshold for RC mode detection.
+ */
+#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
 #define THRESHOLD1 THRESHOLD_MANUAL_PPRZ
 #define THRESHOLD2 (MAX_PPRZ/2)
 
-
+/** AP modes.
+ */
 #define  PPRZ_MODE_MANUAL 0
 #define  PPRZ_MODE_AUTO1 1
 #define  PPRZ_MODE_AUTO2 2
@@ -76,7 +82,14 @@ extern uint8_t lateral_mode;
 
 #define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
 
+/** Supply voltage in deciVolt.
+ * This the ap copy of the measurement from fbw
+ */
 extern uint16_t vsupply;
+
+/** Fuel consumption (mAh)
+ * TODO: move to electrical subsystem
+ */
 extern float energy;
 
 extern bool_t launch;
@@ -100,8 +113,8 @@ extern bool_t gps_lost;
 #define COMMAND_PITCH_TRIM 0
 #endif
 
-extern pprz_t command_roll_trim = COMMAND_ROLL_TRIM;
-extern pprz_t command_pitch_trim = COMMAND_PITCH_TRIM;
+extern pprz_t command_roll_trim;
+extern pprz_t command_pitch_trim;
 
 
 /** Power switch control.
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 273a7903e5..6dfedc62ad 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -105,45 +105,20 @@ static inline void on_baro_abs_event( void );
 static inline void on_baro_dif_event( void );
 #endif
 
-bool_t power_switch;
-
 // what version is this ????
 static const uint16_t version = 1;
 
-uint8_t pprz_mode = PPRZ_MODE_AUTO2;
-uint8_t lateral_mode = LATERAL_MODE_MANUAL;
-
 static uint8_t  mcu1_status;
 
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
 static uint8_t  mcu1_ppm_cpt;
 #endif
 
-bool_t kill_throttle = FALSE;
-
-bool_t launch = FALSE;
-
-/* flight time in seconds */
-uint16_t autopilot_flight_time = 0;
-
-
-/** Supply voltage in deciVolt.
- * This the ap copy of the measurement from fbw
- */
-uint16_t vsupply;
-
 /** Supply current in milliAmpere.
  * This the ap copy of the measurement from fbw
  */
 static int32_t current;	// milliAmpere
 
-/** Fuel consumption (mAh)
- * TODO: move to electrical subsystem
- */
-float energy;
-
-bool_t gps_lost = FALSE;
-
 
 tid_t modules_tid;     ///< id for modules_periodic_task() timer
 tid_t telemetry_tid;   ///< id for telemetry_periodic() timer
@@ -196,6 +171,7 @@ void init_ap( void ) {
 #endif
 
   /************ Internal status ***************/
+  autopilot_init();
   h_ctl_init();
   v_ctl_init();
   nav_init();
@@ -226,8 +202,6 @@ void init_ap( void ) {
   IO0SET = _BV(AEROCOMM_DATA_PIN);
 #endif
 
-  power_switch = FALSE;
-
   /************ Multi-uavs status ***************/
 
 #ifdef TRAFFIC_INFO
diff --git a/sw/airborne/subsystems/actuators/motor_mixing.c b/sw/airborne/subsystems/actuators/motor_mixing.c
index 5975c4ca8f..49794c1538 100644
--- a/sw/airborne/subsystems/actuators/motor_mixing.c
+++ b/sw/airborne/subsystems/actuators/motor_mixing.c
@@ -142,7 +142,7 @@ void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
   }
 }
 
-void motor_mixing_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
+void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
   uint8_t i;
   if (motors_on) {
     int32_t min_cmd = INT32_MAX;
diff --git a/sw/airborne/subsystems/actuators/motor_mixing.h b/sw/airborne/subsystems/actuators/motor_mixing.h
index ad69d15441..3283983c3e 100644
--- a/sw/airborne/subsystems/actuators/motor_mixing.h
+++ b/sw/airborne/subsystems/actuators/motor_mixing.h
@@ -29,6 +29,7 @@
 #define MOTOR_MIXING_H
 
 #include "std.h"
+#include "paparazzi.h"
 #include "generated/airframe.h"
 
 struct MotorMixing {
@@ -42,7 +43,7 @@ struct MotorMixing {
 extern struct MotorMixing motor_mixing;
 
 extern void motor_mixing_init(void);
-extern void motor_mixing_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[]);
+extern void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[]);
 extern void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter);
 
 #endif /* MOTOR_MIXING_H */

From b3f9aa19bf3da801c1cab4df0e011f614576f5e6 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 23 Oct 2012 14:19:31 +0200
Subject: [PATCH 26/26] [fix] small bug fix

- include missing in pwm_arch
- command trims for FW in FBW only
- duplicate function name
- remove depreciate CONTROL_RATE
---
 conf/airframes/CDW/DualBoardApFbw.xml          |  1 -
 conf/airframes/CDW/tiny2_chimu_spi_pt.xml      |  1 -
 .../subsystems/actuators/actuators_pwm_arch.c  |  1 +
 .../subsystems/actuators/actuators_pwm_arch.c  |  1 +
 sw/airborne/firmwares/fixedwing/autopilot.c    |  6 ------
 sw/airborne/firmwares/fixedwing/autopilot.h    | 13 -------------
 sw/airborne/firmwares/fixedwing/main_fbw.c     | 18 ++++++++++++++++++
 sw/simulator/sim_ac_booz.c                     |  2 +-
 sw/simulator/sim_ac_fw.c                       |  2 +-
 sw/simulator/sim_ac_jsbsim.c                   |  2 +-
 sw/simulator/sim_ac_jsbsim.h                   |  2 +-
 11 files changed, 24 insertions(+), 25 deletions(-)

diff --git a/conf/airframes/CDW/DualBoardApFbw.xml b/conf/airframes/CDW/DualBoardApFbw.xml
index 619a34f2f0..bd93c11697 100644
--- a/conf/airframes/CDW/DualBoardApFbw.xml
+++ b/conf/airframes/CDW/DualBoardApFbw.xml
@@ -106,7 +106,6 @@
   <section name="MISC">
     <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
-    <define name="CONTROL_RATE" value="60" unit="Hz"/>
 
     <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
 
diff --git a/conf/airframes/CDW/tiny2_chimu_spi_pt.xml b/conf/airframes/CDW/tiny2_chimu_spi_pt.xml
index b28d2fe43f..22778dc457 100644
--- a/conf/airframes/CDW/tiny2_chimu_spi_pt.xml
+++ b/conf/airframes/CDW/tiny2_chimu_spi_pt.xml
@@ -79,7 +79,6 @@
   <section name="MISC">
     <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
-    <define name="CONTROL_RATE" value="60" unit="Hz"/>
  <!--   <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
     <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
     <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
index 82c801c03d..23f49dc6f8 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/actuators_pwm_arch.c
@@ -20,6 +20,7 @@
  */
 
 #include "subsystems/actuators/actuators_pwm_arch.h"
+#include "subsystems/actuators/actuators_pwm.h"
 #include "std.h"
 #include BOARD_CONFIG
 #include "generated/airframe.h"
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
index 8827c23bf6..c8c8b0b1e2 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -24,6 +24,7 @@
  */
 
 #include "subsystems/actuators/actuators_pwm_arch.h"
+#include "subsystems/actuators/actuators_pwm.h"
 
 #include <libopencm3/stm32/f1/gpio.h>
 #include <libopencm3/stm32/f1/rcc.h>
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c
index 21e70adbc4..347cc80dbf 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot.c
@@ -42,9 +42,6 @@ float energy;
 
 bool_t gps_lost;
 
-pprz_t command_roll_trim;
-pprz_t command_pitch_trim;
-
 bool_t power_switch;
 
 void autopilot_init(void) {
@@ -57,9 +54,6 @@ void autopilot_init(void) {
 
   gps_lost = FALSE;
 
-  command_roll_trim = COMMAND_ROLL_TRIM;
-  command_pitch_trim = COMMAND_PITCH_TRIM;
-
   power_switch = FALSE;
 
 }
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h
index 4c2db8c264..65c9b3495b 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot.h
@@ -103,19 +103,6 @@ extern bool_t gps_lost;
   (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
 })
 
-/** Commands trim for roll and pitch/
- */
-#ifndef COMMAND_ROLL_TRIM
-#define COMMAND_ROLL_TRIM 0
-#endif
-
-#ifndef COMMAND_PITCH_TRIM
-#define COMMAND_PITCH_TRIM 0
-#endif
-
-extern pprz_t command_roll_trim;
-extern pprz_t command_pitch_trim;
-
 
 /** Power switch control.
  */
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 544ac2e36a..31b5b9812e 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -58,6 +58,20 @@ uint8_t fbw_mode;
 #include "inter_mcu.h"
 
 
+/** Trim commands for roll and pitch/
+ */
+#ifndef COMMAND_ROLL_TRIM
+#define COMMAND_ROLL_TRIM 0
+#endif
+
+#ifndef COMMAND_PITCH_TRIM
+#define COMMAND_PITCH_TRIM 0
+#endif
+
+pprz_t command_roll_trim;
+pprz_t command_pitch_trim;
+
+
 volatile uint8_t fbw_new_actuators = 0;
 
 tid_t fbw_periodic_tid; ///< id for periodic_task_fbw() timer
@@ -90,6 +104,10 @@ void init_fbw( void ) {
 
   fbw_mode = FBW_MODE_FAILSAFE;
 
+  command_roll_trim = COMMAND_ROLL_TRIM;
+  command_pitch_trim = COMMAND_PITCH_TRIM;
+
+
   /**** start timers for periodic functions *****/
   fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
   electrical_tid = sys_time_register_timer(0.1, NULL);
diff --git a/sw/simulator/sim_ac_booz.c b/sw/simulator/sim_ac_booz.c
index cca05ed8f4..936c9366a4 100644
--- a/sw/simulator/sim_ac_booz.c
+++ b/sw/simulator/sim_ac_booz.c
@@ -37,7 +37,7 @@ void airborne_run_one_step(void) {
   // SEE  sim_run_one_step
 }
 
-void autopilot_init(void) {
+void sim_autopilot_init(void) {
   init_fbw();
   init_ap();
 }
diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c
index 6b3a5bda51..d817f0e598 100644
--- a/sw/simulator/sim_ac_fw.c
+++ b/sw/simulator/sim_ac_fw.c
@@ -65,7 +65,7 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
   parse_dl_move_wp(argv);
 }
 
-void autopilot_init(void) {
+void sim_autopilot_init(void) {
   IvyBindMsg(on_DL_PING, NULL, "^(\\S*) DL_PING");
   IvyBindMsg(on_DL_ACINFO, NULL, "^(\\S*) DL_ACINFO (\\S*) (\\S*) (\\S* (\\S*) (\\S*) (\\S*)) (\\S*) (\\S*)");
   IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c
index 57db201604..fdddd1635e 100644
--- a/sw/simulator/sim_ac_jsbsim.c
+++ b/sw/simulator/sim_ac_jsbsim.c
@@ -78,7 +78,7 @@ static void sim_init(void) {
   ivy_transport_init();
 
   // main AP init (feed the sensors once before ?)
-  autopilot_init();
+  sim_autopilot_init();
 
   printf("sys_time resolution: %f\n", SYS_TIME_RESOLUTION);
   printf("sys_time period in msec: %d\n", SYSTIME_PERIOD);
diff --git a/sw/simulator/sim_ac_jsbsim.h b/sw/simulator/sim_ac_jsbsim.h
index 4217698244..282300ccc4 100644
--- a/sw/simulator/sim_ac_jsbsim.h
+++ b/sw/simulator/sim_ac_jsbsim.h
@@ -51,7 +51,7 @@
 
 extern bool run_model;
 
-void autopilot_init(void);
+void sim_autopilot_init(void);
 void autopilot_periodic_task(void);
 void autopilot_event_task(void);
 void jsbsim_init(void);