Merge branch 'master' into PFC

paparazzi · Dec 18, 2023 · 12f1d9a · 12f1d9a
2 parents 65b66ee + a07d674
commit 12f1d9a
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 13 deletions.
diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml
@@ -277,9 +277,9 @@
     <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>

diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
@@ -126,6 +126,7 @@
       <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/>
       <define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
       <define name="GUIDANCE_INDI_MIN_THROTTLE" value="2500"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="18"/>
     </module>
 
     <module name="motor_mixing"/>
@@ -300,9 +301,9 @@
     <define name="FILT_CUTOFF_R" value="4.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.0354"/>
-    <define name="ACT_DYN_Q" value="0.0354"/>
-    <define name="ACT_DYN_R" value="0.0354"/>
+    <define name="ACT_FREQ_P" value="18"/>
+    <define name="ACT_FREQ_Q" value="18"/>
+    <define name="ACT_FREQ_R" value="18"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>

diff --git a/conf/modules/imu_mpu9250_i2c.xml b/conf/modules/imu_mpu9250_i2c.xml
@@ -42,6 +42,7 @@
     <test>
       <define name="IMU_MPU9250_I2C_DEV" value="i2c1"/>
       <define name="USE_I2C1"/>
+      <define name="PERIODIC_FREQUENCY" value="512"/>
       <define name="IMU_MPU9250_PERIODIC_FREQ" value="512"/>
       <define name="PERIODIC_FREQUENCY" value="512"/>
     </test>

diff --git a/conf/modules/imu_mpu9250_spi.xml b/conf/modules/imu_mpu9250_spi.xml
@@ -51,6 +51,7 @@
       <define name="USE_SPI1"/>
       <define name="IMU_MPU9250_SPI_SLAVE_IDX" value="0"/>
       <define name="USE_SPI_SLAVE0"/>
+      <define name="PERIODIC_FREQUENCY" value="512"/>
       <define name="IMU_MPU9250_PERIODIC_FREQ" value="512"/>
       <define name="PERIODIC_FREQUENCY" value="512"/>
     </test>

diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -128,13 +128,14 @@ static void guidance_indi_filter_thrust(void);
 #warning "The thrust dynamics are now specified in continuous time with the corner frequency of the first order model!"
 #warning "define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ in rad/s"
 #warning "Use -log(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old value."
+#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ (-ln(1-GUIDANCE_INDI_THRUST_DYNAMICS)*PERIODIC_FREQUENCY)
 #endif
 
 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
-#ifndef STABILIZATION_INDI_ACT_DYN_P
-#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
+#ifndef STABILIZATION_INDI_ACT_FREQ_P
+#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
 #else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
-#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_DYN_P
+#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
 #endif
 #endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
 

diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.c b/sw/airborne/modules/rot_wing_drone/rotwing_state.c
@@ -613,13 +613,13 @@ static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id,
     // Sanity check that index is valid
     int idx = feedback_msg[i].idx;
     if (feedback_msg[i].set.rpm) {
-      if (idx == SERVO_MOTOR_FRONT_IDX) {
+      if ((idx == SERVO_MOTOR_FRONT_IDX) || (idx == SERVO_BMOTOR_FRONT_IDX)) {
         rotwing_state_hover_rpm[0] = feedback_msg->rpm;
-      } else if (idx == SERVO_MOTOR_RIGHT_IDX) {
+      } else if ((idx == SERVO_MOTOR_RIGHT_IDX) || (idx == SERVO_BMOTOR_RIGHT_IDX)) {
         rotwing_state_hover_rpm[1] = feedback_msg->rpm;
-      } else if (idx == SERVO_MOTOR_BACK_IDX) {
+      } else if ((idx == SERVO_MOTOR_BACK_IDX) || (idx == SERVO_BMOTOR_BACK_IDX)) {
         rotwing_state_hover_rpm[2] = feedback_msg->rpm;
-      } else if (idx == SERVO_MOTOR_LEFT_IDX) {
+      } else if ((idx == SERVO_MOTOR_LEFT_IDX) || (idx == SERVO_BMOTOR_LEFT_IDX)) {
         rotwing_state_hover_rpm[3] = feedback_msg->rpm;
       }
     }