diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml
new file mode 100644
index 0000000000..acf52e499f
--- /dev/null
+++ b/conf/airframes/tudelft/rot_wing_v3b.xml
@@ -0,0 +1,509 @@
+
+
+
+ RotatingWingV3B
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/wing_rotation_controller_servo.xml b/conf/modules/wing_rotation_controller_servo.xml
new file mode 100644
index 0000000000..947be2a0bd
--- /dev/null
+++ b/conf/modules/wing_rotation_controller_servo.xml
@@ -0,0 +1,27 @@
+
+
+
+ Module to control wing rotation servo command based on prefered angle setpoint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index b9973c6df4..0f9c3fd150 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -571,6 +571,17 @@
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="red"
/>
+
+ *
+ * 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, see
+ * .
+ */
+
+/** @file "modules/rot_wing_drone/wing_rotation_controller_v3b.c"
+ * @author Dennis van Wijngaarden
+ * Module to control wing rotation servo command based on prefered angle setpoint
+ */
+
+#include "modules/rot_wing_drone/wing_rotation_controller_servo.h"
+#include "modules/radio_control/radio_control.h"
+#include "firmwares/rotorcraft/guidance/guidance_h.h"
+#include "generated/airframe.h"
+#include "modules/core/abi.h"
+#include "mcu_periph/adc.h"
+
+#if USE_NPS
+#include "modules/actuators/actuators.h"
+#endif
+
+
+#if !USE_NPS
+
+#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION
+#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION ADC_5
+#endif
+
+#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES
+#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES 16
+#endif
+
+#endif // !USE_NPS
+
+#ifndef WING_ROTATION_CONTROLLER_FIRST_DYN
+#define WING_ROTATION_CONTROLLER_FIRST_DYN 0.001
+#endif
+
+#ifndef WING_ROTATION_CONTROLLER_SECOND_DYN
+#define WING_ROTATION_CONTROLLER_SECOND_DYN 0.003
+#endif
+
+// Parameters
+struct wing_rotation_controller_t wing_rotation_controller = {0};
+
+
+#if !USE_NPS
+static struct adc_buf buf_wing_rot_pos;
+#endif
+
+
+// Inline functions
+inline void wing_rotation_adc_to_deg(void);
+inline void wing_rotation_compute_pprz_cmd(void);
+
+// Initialization
+void wing_rotation_init(void)
+{
+ // ADC init
+#if !USE_NPS
+ adc_buf_channel(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION, &buf_wing_rot_pos, ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES);
+#endif
+
+ // Init Data
+ wing_rotation_controller.wing_angle_virtual_deg_sp = 45;
+ wing_rotation_controller.wing_rotation_first_order_dynamics = WING_ROTATION_CONTROLLER_FIRST_DYN;
+ wing_rotation_controller.wing_rotation_second_order_dynamics = WING_ROTATION_CONTROLLER_SECOND_DYN;
+}
+
+void wing_rotation_periodic(void)
+{
+ // After 5 loops, set current setpoint and enable wing_rotation
+ // freq = 1.0 Hz
+ if (!wing_rotation_controller.initialized) {
+ wing_rotation_controller.init_loop_count += 1;
+ if (wing_rotation_controller.init_loop_count > 4) {
+ wing_rotation_controller.initialized = true;
+ wing_rotation_controller.wing_angle_deg_sp = 45.;
+ }
+ }
+}
+
+void wing_rotation_event(void)
+{
+ // Update Wing position sensor
+ wing_rotation_adc_to_deg();
+
+ // Run control if initialized
+ if (wing_rotation_controller.initialized) {
+
+ // Setpoint checks
+ Bound(wing_rotation_controller.wing_angle_deg_sp, 0., 90.);
+
+ // Control the wing rotation position.
+ wing_rotation_compute_pprz_cmd();
+ }
+}
+
+void wing_rotation_adc_to_deg(void)
+{
+#if !USE_NPS
+ // Read ADC
+ wing_rotation_controller.adc_wing_rotation = buf_wing_rot_pos.sum / buf_wing_rot_pos.av_nb_sample;
+ wing_rotation_controller.wing_angle_deg = 0.00247111 * (float)wing_rotation_controller.adc_wing_rotation - 25.635294;
+
+#else // !USE_NPS
+ // Copy setpoint as actual angle in simulation
+ wing_rotation_controller.wing_angle_deg = wing_rotation_controller.wing_angle_virtual_deg_sp;
+#endif
+
+ // SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback
+ struct act_feedback_t feedback;
+ feedback.idx = SERVO_ROTATION_MECH;
+ feedback.position = 0.5 * M_PI - RadOfDeg(wing_rotation_controller.wing_angle_deg);
+ feedback.set.position = true;
+
+ // Send ABI message
+ AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
+
+}
+
+void wing_rotation_compute_pprz_cmd(void)
+{
+ // Smooth accerelation and rate limited setpoint
+ float angle_error = wing_rotation_controller.wing_angle_deg_sp - wing_rotation_controller.wing_angle_virtual_deg_sp;
+ float speed_sp = wing_rotation_controller.wing_rotation_first_order_dynamics * angle_error;
+ float speed_error = speed_sp - wing_rotation_controller.wing_rotation_speed;
+ wing_rotation_controller.wing_rotation_speed += wing_rotation_controller.wing_rotation_second_order_dynamics * speed_error;
+ wing_rotation_controller.wing_angle_virtual_deg_sp += wing_rotation_controller.wing_rotation_speed;
+
+ // Send to actuators
+ int32_t servo_pprz_cmd;
+ servo_pprz_cmd = (int32_t)(wing_rotation_controller.wing_angle_virtual_deg_sp / 90. * (float)MAX_PPRZ);
+ Bound(servo_pprz_cmd, 0, MAX_PPRZ);
+ wing_rotation_controller.servo_pprz_cmd = servo_pprz_cmd;
+
+ actuators_pprz[SERVO_ROTATION_MECH] = servo_pprz_cmd;
+}
+
+
diff --git a/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h b/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h
new file mode 100644
index 0000000000..e25eabc264
--- /dev/null
+++ b/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2022 Dennis van Wijngaarden
+ *
+ * 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, see
+ * .
+ */
+
+/** @file "modules/rot_wing_drone/wing_rotation_controller_servo.h"
+ * @author Dennis van Wijngaarden
+ * Module to control wing rotation servo command based on prefered angle setpoint
+ */
+
+#ifndef WING_ROTATION_CONTROLLER_SERVO_H
+#define WING_ROTATION_CONTROLLER_SERVO_H
+
+#include "std.h"
+
+extern void wing_rotation_init(void);
+extern void wing_rotation_periodic(void);
+extern void wing_rotation_event(void);
+
+// Paramaters
+struct wing_rotation_controller_t {
+ float wing_angle_deg; ///< Wing angle measurement in degrees
+ float wing_angle_deg_sp; ///< Wing angle setpoint in degrees
+
+ int32_t servo_pprz_cmd; ///< Servo command in pprz
+ uint16_t adc_wing_rotation; ///< ADC value of wing
+
+ float wing_rotation_speed; ///< Rate limiter state variable 1
+ float wing_angle_virtual_deg_sp; ///< Rate limiter state variable 2
+ float wing_rotation_first_order_dynamics; ///< Rate limiter for wing rotation
+ float wing_rotation_second_order_dynamics; ///< Acceleration limiter for wing rotation
+
+ bool initialized; ///< Wing rotation controller initialized
+ uint8_t init_loop_count; ///< Wing rotation controller initialization loop count
+};
+
+// Setters
+#define SetWingAngleDegSp(_wing_angle_deg_sp) (wing_rotation_controller.wing_angle_deg_sp = _wing_angle_deg_sp)
+
+extern struct wing_rotation_controller_t wing_rotation_controller;
+
+#endif // WING_ROTATION_CONTROLLER_SERVO_H