Update sysid values

src/drivers/src/motors.c

@@ -159,11 +159,11 @@ float motorsCompensateBatteryVoltage(uint32_t id, float iThrust, float supplyVol
   */
   if (supplyVoltage < 2.0f)
   {
-      return 0.0f; // iThrust;
-    }
+    return 0.0f;
+  }
 
   float thrust = (iThrust / 65535.0f) * THRUST_MAX; // rescaling integer thrust to N
-  if (thrust < THRUST_MIN)                          // Make sure sqrt function gets positive values
+  if (thrust < THRUST_MIN)                          // Make sure inversion is unique
   {
     return 0.0f;
   }

src/platform/interface/platform_defaults_cf2.h

@@ -54,32 +54,36 @@
         #define CF_MASS 0.029f  // kg
     #endif
 #endif
-// thrust coefficients
+// Thrust curve coefficients (per motor) and minimum and maximal thrust per motor
+// Note: The maximum thrust is a trade-off between consistency of thrust over all battery levels
+// and maximum performance with a full battery. Increase this value at your own risk. More info
+// in #1526 or this blog post: 
+// https://www.bitcraze.io/2025/10/keeping-thrust-consistent-as-the-battery-drains/
 #ifdef CONFIG_ENABLE_THRUST_BAT_COMPENSATED
     #if defined(CONFIG_CRAZYFLIE_21_PLUS)    // default case, 2.1+ propellers
         #define VMOTOR2THRUST0  -0.02476537915958403f
         #define VMOTOR2THRUST1  0.06523793527519485f
         #define VMOTOR2THRUST2  -0.026792504967750107f
         #define VMOTOR2THRUST3  0.006776789303971145f
-        #define THRUST_MIN      0.02f
-        #define THRUST_MAX      0.1125f
-        #define THRUST2TORQUE   0.005964552f // TODO, value is for the legacy propellers and old battery compensation
-    #elif defined(CONFIG_CRAZYFLIE_THRUST_UPGRADE_KIT)  // Thrust upgrade kit
-        #define VMOTOR2THRUST0  -0.03978221591250353f
-        #define VMOTOR2THRUST1  0.10979738851226176f
-        #define VMOTOR2THRUST2  -0.05545304285403245f
-        #define VMOTOR2THRUST3  0.016215002062640885f
-        #define THRUST_MIN      0.03f
-        #define THRUST_MAX      0.1625f
-        #define THRUST2TORQUE   0.005964552f // TODO, value is for the legacy propellers and old battery compensation
+        #define THRUST_MIN      0.012817578393224994f  // N (per motor)
+        #define THRUST_MAX      0.12f                  // N (per motor)
+        #define THRUST2TORQUE   0.0069928948992470565f // m
+    #elif defined(CONFIG_CRAZYFLIE_THRUST_UPGRADE_KIT) // Thrust upgrade kit
+        #define VMOTOR2THRUST0  0.006728127583707208f
+        #define VMOTOR2THRUST1  0.01011557616217668f
+        #define VMOTOR2THRUST2  0.010263198062061085f
+        #define VMOTOR2THRUST3  0.0028358638322392503f
+        #define THRUST_MIN      0.01922636758983749f   // N (per motor)
+        #define THRUST_MAX      0.18f                  // N (per motor)
+        #define THRUST2TORQUE   0.0051648627905205285f // m
     #elif defined(CONFIG_CRAZYFLIE_LEGACY_PROPELLERS)  // legacy propellers
         #define VMOTOR2THRUST0  -0.014830744918356092f
         #define VMOTOR2THRUST1  0.04724465241828281f
         #define VMOTOR2THRUST2  -0.01847364358025878f
         #define VMOTOR2THRUST3  0.005960923942142f
-        #define THRUST_MIN      0.02f
-        #define THRUST_MAX      0.1125f
-        #define THRUST2TORQUE   0.005964552f
+        #define THRUST_MIN      0.012817578393224994f  // N (per motor)
+        #define THRUST_MAX      0.12f                  // N (per motor)
+        #define THRUST2TORQUE   0.007350862856566459f  // m
     #endif
 #endif
 

tools/system_id/README.md

@@ -3,7 +3,7 @@
 This folder contains scripts to measure propellers and motors using the systemId deck. Some more information can be found in those two blog articles:
 
 - [Building a Crazyflie thrust stand (2021)](https://www.bitcraze.io/2021/08/building-a-crazyflie-thrust-stand/)
-- [Keeping Thrust Consistent as the Battery Drains (2025) TODO](https://www.bitcraze.io/?p=14335&preview=true)
+- [Keeping Thrust Consistent as the Battery Drains (2025)](https://www.bitcraze.io/2025/10/keeping-thrust-consistent-as-the-battery-drains/)
 
 ## Setup
 
@@ -74,6 +74,18 @@ To verify the parameters, after adding the new values and flashing, we run the d
 
 #### Dynamic parameters
 
-Lastly, we want to know how fast the motors can change speed/thrust. In `mode = dynamic`, data is first collected with motors changing speed from lowest PWM to highest PWM command, once with and once without battery compensation.
+Lastly, we want to know how fast the motors can change speed/thrust. In `mode = dynamic`, data is first collected with motors following a reference for system identification with battery compensation enabled. Make sure to use a full battery for the data collection, since the trajectory is quite long.
 
-In the system_id part, we can observe the thrust dynamics. They can be modelled in two ways. From first principles we know the torque of a DC motor beeing $T=k\frac{V}{R}-k^2 \omega$ (in steady state, neglecting electrical dynamics) and the differential equation for the motor and propeller speed beeing $T=J\dot{\omega} + k_T \omega^2$. Rearranging the terms and ignoring the drag part ($k_T \omega^2$), we get a first order system for the propeller dynamics as: $\dot{RPM} = \frac{1}{\tau_{RPM}} (RPM_{CMD}-RPM)$. Alternatively, we can set up the thrust directly as first order system, which is also done in some works: $\dot{f} = \frac{1}{\tau_f} (f_{CMD}-f)$. We identify both parameters and save them in the same file as before.
+In the system_id part, we can observe the thrust dynamics. They can be modelled in two ways. From first principles we know the torque of a DC motor beeing $T=k\frac{V}{R}-k^2 \Omega$ (in steady state, neglecting electrical dynamics) and the differential equation for the motor and propeller speed beeing $T=J\dot{\Omega} + k_T \Omega^2$. Rearranging the terms and abusing $\Omega$ as RPM, we get the a system of the form: $\dot{\Omega} = a\Omega_\mathrm{set} + b\Omega + c\Omega^2$, where $\Omega_\mathrm{set}$ is the setpoint known from the thrust curves. This equation has two problems: First, the thrust curves should be collected on multiple runs using different motors to get a good average thrust curve. This is not possible here and it will overfit the particular motor from the experiment. Second, the parameters ($a,b,c$) are most likely different for acceleration and deceleration, since some motor types cannot brake (due to the used electronics). Thus, we adjust the model to fix both issues: 
+
+$$
+\dot{\Omega} = \begin{cases}
+    a\cdot(\Omega_\mathrm{set}-\Omega) + b\cdot(\Omega_\mathrm{set}^2-\Omega^2),  & \forall u_\Omega>\Omega. \\
+    c\cdot(\Omega_\mathrm{set}-\Omega) + d\cdot(\Omega_\mathrm{set}^2-\Omega^2),  & \forall u_\Omega\leq\Omega.
+\end{cases}
+$$
+Note: A good result here is dependent on a good fit earlier. 
+
+For brushless motors, the even simpler form of $\dot{\Omega} = a(\Omega_\mathrm{set}-\Omega)$ also provides a relatively good fit, so this parameter is also computed. 
+
+Alternatively, we can set up the thrust directly as first order system, which is also done in some works: $\dot{f} = \frac{1}{\tau_f} (f_{CMD}-f)$. We identify all parameters and save them in the same file as before. Also, the results of the fits are shown in plots and the console.

tools/system_id/collect_data.py

@@ -447,6 +447,8 @@ def _ramp_motors(self):
                 )
                 raise NotImplementedError(error)
             self._cf.param.set_value("motorPowerSet.enable", 3)
+            global PWM_MIN
+            PWM_MIN = 0  # in testing mode, we also want to check 0 thrust commands
             print("Collecting data with battery compensation")
         else:
             if self.torques:
@@ -488,8 +490,8 @@ def _ramp_motors(self):
 
                 info = f"time={time.time() - t_start:.1f}s, "
                 info += f"pwm cmd={pwm}, "
-                info += f"pwm actual={(data['motor.m1']) if not self.torques else (data['motor.m1'] + data['motor.m2']) / 2}, "
-                info += f"rotor speed={(data['rpm.m1']) if not self.torques else (data['rpm.m1'] + data['rpm.m2']) / 2}, "
+                info += f"pwm actual={(data['motor.m1']) if not self.torques else (data['motor.m1'] + data['motor.m2']) / 2:.1f}, "
+                info += f"rotor speed={(data['rpm.m1']) if not self.torques else (data['rpm.m1'] + data['rpm.m2']) / 2:.1f}, "
                 info += f"vbat={data['pm.vbatMV'] / 1000:.3f}V, "
                 info += f"vmotors={data['pm.vbatMV'] / 1000 * pwm / PWM_MAX:.3f}V, "
                 info += f"thrust={data['loadcell.weight'] * self.g:.3f}mN"
@@ -509,7 +511,7 @@ def _ramp_motors(self):
             if self.loadcell is not None and i % 20 == 0:
                 self._measure(0, min_samples=10)
                 self._measure(0, min_samples=10, a_motors=False)
-                time.sleep(1.0)
+                time.sleep(2.0)
                 self.loadcell.calibrate()
 
             if time.time() - t_start > t_max:
@@ -526,7 +528,14 @@ class CollectDataDynamic(CollectData):
     """
 
     def __init__(
-        self, link_uri, calib_a, calib_b, comb, extra, loadcell=None, verbose=False
+        self,
+        link_uri,
+        calib_a,
+        calib_b,
+        comb,
+        extra,
+        loadcell=None,
+        verbose=False,
     ):
         """Initialize and run the example with the specified link_uri"""
         self.samplerate = 7  # has to be in [0,7], where 7 is the highest.
@@ -539,6 +548,7 @@ def __init__(
             "dynamic",
             comb,
             extra,
+            batComp=True,
             loadcell=loadcell,
             verbose=verbose,
         )
@@ -571,74 +581,106 @@ def _applyThrust(self, thrust, duration):
         self._cf.param.set_value("motorPowerSet.m1", int(thrust))
         print(f"Applied pwm={thrust}, waiting for {duration}s")
         while time.time() - start < duration:
-            self._localization.send_emergency_stop_watchdog()
-            time.sleep(0.1)
+            # self._localization.send_emergency_stop_watchdog()
+            time.sleep(0.001)
 
     def _ramp_motors(self):
         self._check_parameters()
 
         self._cf.param.set_value("motorPowerSet.m1", 0)
 
-        duration = 1.0
-
-        # collecting data twice:
-        # Once with and once without battery compensation
-        while self.is_connected:
-            if self.batComp:
-                self._cf.param.set_value("motorPowerSet.enable", 3)
-                print("Collecting data with battery compensation")
-            else:
-                self._cf.param.set_value("motorPowerSet.enable", 2)
-                print("Collecting data without battery compensation")
-
-            # base speed
-            self._applyThrust(PWM_MIN, duration)
-
-            # 0 -> 0.25
-            self._applyThrust((PWM_MAX + PWM_MIN) * 1 / 4, duration)
-            # 0.25 -> 0
-            self._applyThrust(PWM_MIN, duration)
-
-            # 0 -> 0.5
-            self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
-            # 0.5 -> 0
-            self._applyThrust(PWM_MIN, duration)
-
-            # 0 -> 0.75
-            self._applyThrust((PWM_MAX + PWM_MIN) * 3 / 4, duration)
-            # 0.75 -> 0
-            self._applyThrust(PWM_MIN, duration)
-
-            # 0 -> 1
-            self._applyThrust(PWM_MAX, duration)
-            # 1 -> 0
-            self._applyThrust(PWM_MIN, duration)
-
-            # 0.5 -> 0.25
-            self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
-            self._applyThrust((PWM_MAX + PWM_MIN) * 1 / 4, duration)
-            # 0.25 -> 0.5
-            self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
-
-            # 0.5 -> 0.75
-            self._applyThrust((PWM_MAX + PWM_MIN) * 3 / 4, duration)
-            # 0.75 -> 0.5
-            self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
-
-            # 0.5 -> 1.0
-            self._applyThrust(PWM_MAX, duration)
-            # 1.0 -> 0.5
-            self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
-
-            self._applyThrust(0, duration)
+        if self.batComp:
+            self._cf.param.set_value("motorPowerSet.enable", 3)
+            print("Collecting data with battery compensation")
+        else:
+            self._cf.param.set_value("motorPowerSet.enable", 2)
+            print("Collecting data without battery compensation")
 
-            if self.batComp:
-                break
-            else:
-                self.batComp = True
+        self._ramp_motors_chirp()
+        self._ramp_motors_step()
 
         self._close()
 
+    def _ramp_motors_step(self, duration: float = 1.0):
+        # base speed
+        self._applyThrust(PWM_MIN, 2 * duration)
+
+        # 0 -> 0.25
+        self._applyThrust((PWM_MAX + PWM_MIN) * 1 / 4, duration)
+        # 0.25 -> 0
+        self._applyThrust(PWM_MIN, duration)
+
+        # 0 -> 0.5
+        self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
+        # 0.5 -> 0
+        self._applyThrust(PWM_MIN, duration)
+
+        # 0 -> 0.75
+        self._applyThrust((PWM_MAX + PWM_MIN) * 3 / 4, duration)
+        # 0.75 -> 0
+        self._applyThrust(PWM_MIN, duration)
+
+        # 0 -> 1
+        self._applyThrust(PWM_MAX, duration)
+        # 1 -> 0
+        self._applyThrust(PWM_MIN, duration)
+
+        # 0.5 -> 0.25
+        self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
+        self._applyThrust((PWM_MAX + PWM_MIN) * 1 / 4, duration)
+        # 0.25 -> 0.5
+        self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
+
+        # 0.5 -> 0.75
+        self._applyThrust((PWM_MAX + PWM_MIN) * 3 / 4, duration)
+        # 0.75 -> 0.5
+        self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
+
+        # 0.5 -> 1.0
+        self._applyThrust(PWM_MAX, duration)
+        # 1.0 -> 0.5
+        self._applyThrust((PWM_MAX + PWM_MIN) / 2, duration)
+
+        # base speed
+        self._applyThrust(PWM_MIN, 2 * duration)
+
+    def _ramp_motors_chirp(self):
+        # base speed
+        self._applyThrust(PWM_MAX / 4, 2.0)
+
+        chirp_duration = 30  # s
+        chirp_send_frequency = 100  # Hz
+        chirp_f0 = 0.1  # Hz
+        chirp_f1 = 5  # Hz
+
+        from scipy.signal import chirp
+
+        t = np.linspace(0, chirp_duration, chirp_duration * chirp_send_frequency)
+        c = chirp(t, chirp_f0, chirp_duration, chirp_f1, method="lin", phi=180)
+        # [PWM_MIN, PWM_MIN+PWM_MAX/4]
+        for sine in c:
+            sine = (sine + 1) / 2  # Scaling to [0,1]
+            pwm = sine * PWM_MAX / 4 + PWM_MIN  # Scaling to useful PWM values
+            self._applyThrust(pwm, 1 / chirp_send_frequency)
+
+        # [PWM_MIN, PWM_MIN+PWM_MAX/2]
+        for sine in c:
+            sine = (sine + 1) / 2  # Scaling to [0,1]
+            pwm = sine * PWM_MAX / 2 + PWM_MIN  # Scaling to useful PWM values
+            self._applyThrust(pwm, 1 / chirp_send_frequency)
+
+        # [PWM_MAX/4, PWM_MAX*3/4]
+        for sine in c:
+            sine = (sine + 1) / 2  # Scaling to [0,1]
+            pwm = sine * PWM_MAX / 2 + PWM_MAX / 4  # Scaling to useful PWM values
+            self._applyThrust(pwm, 1 / chirp_send_frequency)
+
+        # [PWM_MAX/2, PWM_MAX]
+        for sine in c:
+            sine = (sine + 1) / 2  # Scaling to [0,1]
+            pwm = sine * PWM_MAX / 2 + PWM_MAX / 2  # Scaling to useful PWM values
+            self._applyThrust(pwm, 1 / chirp_send_frequency)
+
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()