[actuators] servo_switch directly controls actuators again

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_">

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"/>

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);
+  }
 }

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
 
 

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 */
 
 

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);