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 @@ + @@ -41,10 +42,10 @@ -
- - - +
+ + + @@ -72,11 +73,11 @@ - - - - - + + + + +
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 # -#
-# -# -# -# -# -# -# -# -# -#
+# 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 @@ # #
# -#
-# -# -# -# -# -# -# -# -# -#
-# +# 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: # -#
-# -# -# -# -# -# -# -# -# -#
-# # 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 +# +#
+# +# +# +# +# +# +# +# +# +#
+# +# + +$(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 +#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 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 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 -#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 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 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