[modules] wing-rotation controller with servo (#3143)

* [modules] wing-rotation controller with servo

conf/modules/wing_rotation_controller_servo.xml

@@ -0,0 +1,27 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="wing_rotation_controller_servo" dir="rot_wing_drone">
+  <doc>
+    <description>Module to control wing rotation servo command based on prefered angle setpoint</description>
+  </doc>
+  <settings>
+    <dl_settings NAME="wing_rotation">
+      <dl_settings NAME="wing_rot">
+        <dl_setting MAX="90" MIN="0" STEP="1" VAR="wing_rotation_controller.wing_angle_deg_sp" shortname="wing_sp_deg"/>
+        <dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation_controller.wing_rotation_first_order_dynamics" shortname="first_dyn"/>
+        <dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation_controller.wing_rotation_second_order_dynamics" shortname="second_dyn"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <header>
+    <file name="wing_rotation_controller_servo.h"/>
+  </header>
+    <init fun="wing_rotation_init()"/>
+    <periodic fun="wing_rotation_periodic()" freq="1.0"/>
+    <periodic fun="wing_rotation_event()"/>
+  <makefile>
+    <configure name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" default="ADC_5" case="lower|upper"/>
+    <define name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" value="$(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_UPPER)"/>
+    <define name="USE_$(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_UPPER)"/>
+    <file name="wing_rotation_controller_servo.c"/>
+  </makefile>
+</module>

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"
   />
+  <aircraft
+   name="RotatingWingV3b"
+   ac_id="39"
+   airframe="airframes/tudelft/rot_wing_v3b.xml"
+   radio="radios/crossfire_sbus.xml"
+   telemetry="telemetry/highspeed_rotorcraft.xml"
+   flight_plan="flight_plans/tudelft/rotating_wing25kg_EHVB.xml"
+   settings="settings/rotorcraft_basic.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml modules/wing_rotation_controller_servo.xml"
+   gui_color="red"
+  />
   <aircraft
    name="rot_wing_25kg"
    ac_id="29"

sw/airborne/modules/meteo/ekf_aw_wrapper.c

@@ -388,7 +388,7 @@ void ekf_aw_wrapper_fetch(void)
   update_butterworth_2_low_pass(&filt_pusher_prop_rpm, ekf_aw.last_RPM_pusher * 1.0f);
 
 #if EKF_AW_WRAPPER_ROT_WING
-  update_butterworth_2_low_pass(&filt_skew, wing_rotation.wing_angle_rad);
+  update_butterworth_2_low_pass(&filt_skew, RadOfDeg(wing_rotation.wing_angle_deg));
 
   // Get elevator pprz signal
   int16_t *elev_pprz = &actuators_pprz[5];

sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c

@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/rot_wing_drone/wing_rotation_controller_v3b.c"
+ * @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ * 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;
+}
+
+

sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/rot_wing_drone/wing_rotation_controller_servo.h"
+ * @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ * 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