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 @@ - + @@ -60,7 +60,6 @@ - @@ -68,7 +67,6 @@ -
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 @@ Servo switch - +
@@ -17,14 +17,12 @@ 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);